<?xml version="1.0"?>

<!--
 * Copyright (C) 2020 Ghost Robotics - All Rights Reserved
 * Unauthorized copying of this file, via any medium is strictly prohibited
 * Proprietary and confidential
 * Written by Avik De <avik@ghostrobotics.io> and Tom Jacobs <tom.jacobs@ghostrobotics.io>
-->

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="vision60_macro">
	<!-- Define parameters -->
	<xacro:property name="bodyLength" value="0.83"/>
	<xacro:property name="bodyWidth" value="0.25"/>
	<xacro:property name="bodyHeight" value="0.19"/>
	<xacro:property name="bodyMass" value="17.9"/>
	<xacro:property name="upperLegMass" value="2.525"/>
	<xacro:property name="lowerLegMass" value="0.375"/>
	<xacro:property name="toeMass" value="0.15"/>
	<xacro:property name="upperLegLength" value="0.34"/>
	<xacro:property name="upperLegWidth" value="0.055"/>
	<xacro:property name="upperLegHeight" value="0.075"/>
	<xacro:property name="lowerLegLength" value="0.34"/>
	<xacro:property name="lowerLegRadius" value="0.011"/>
	<xacro:property name="lowerLegAttachLinkLength" value="0"/>
	<xacro:property name="podMass" value="1.975"/>
	<xacro:property name="podLength" value="0.15"/>
	<xacro:property name="podXOffsetFromOuterEdge" value="0.085"/>
	<xacro:property name="podWidth" value="0.15"/>
	<xacro:property name="podHeight" value="0.09"/>
	<xacro:property name="dPodAxisToEdge" value="0.07"/>
	<xacro:property name="dx" value="0.34385"/>
	<xacro:property name="dy" value="0.092865"/>
	<xacro:property name="toeRadius" value="0.03"/>
	<xacro:property name="toeOffset" value="0.0"/>
	<xacro:property name="toeStiffness" value="30000.0"/>
	<xacro:property name="toeDamping" value="1000.0"/>
	<xacro:property name="toeSpinningFriction" value="0.3"/>
	<xacro:property name="toeLateralFriction" value="3.0"/>
	<xacro:property name="abKinLimit" value="0.43"/>
	<xacro:property name="abSpeedLimit" value="8.6"/>
	<xacro:property name="abEffortLimit" value="375"/>
	<xacro:property name="jointSpeedLimit" value="30"/>
	<xacro:property name="jointEffortLimit" value="87.5"/>
	<xacro:property name="hipBumper" value="0.3"/> <!-- This is the joint angle offset at the hip bumper -->

	<!-- Define materials -->
	<material name="gray">
		<color rgba="0.6 0.6 0.6 1.0"/>
	</material>
	<material name="darkgray">
		<color rgba="0.3 0.3 0.3 1.0"/>
	</material>
	<material name="blue">
		<color rgba="0 0 0.8 1"/>
	</material>
	<material name="black">
		<color rgba="0 0 0 1"/>
	</material>
	<material name="purple">
		<color rgba="0.5 0.0 0.5 1.0"/>
	</material>
	<material name="red">
		<color rgba="0.8 0.0 0.2 1.0"/>
	</material>


	<xacro:macro name="vision60">
		<!-- Body -->
		<link name="body">
			<visual>
				<origin rpy="0 0 0" xyz="0 0 -0.1"/>
				<geometry>
					<mesh filename="package://ghost_master/meshes/Frame.STL" scale="1 1 1"/>
				</geometry>
				<material name="gray"/>
			</visual>

			<inertial>
				<mass value="${bodyMass}"/>
				<!-- Uniform box -->
				<!-- <inertia ixx="1/12*${bodyMass}*(${bodyWidth}*${bodyWidth} + ${bodyHeight}*${bodyHeight})"  ixy="0"  ixz="0" iyy="1/12*${bodyMass}*(${bodyLength}*${bodyLength} + ${bodyHeight}*${bodyHeight})" iyz="0" izz="1/12*${bodyMass}*(${bodyLength}*${bodyLength} + ${bodyWidth}*${bodyWidth})"/> -->
				<inertia ixx="0.0986"  ixy="0"  ixz="0" iyy="0.8105" iyz="0" izz="0.8369" />
			</inertial>

			<!-- Just copy geometry for collision -->
			<collision>
				<origin rpy="0 0 0" xyz="0 0 0"/>
				<geometry>
					<box size="${bodyLength} ${bodyWidth} ${bodyHeight}"/>
				</geometry>
			</collision>
		</link>

		<!-- Define our leg macro -->
		<xacro:macro name="leg" params="suffix left front hipJointName kneeJointName abductionJointName hipJointLowerLimit hipJointUpperLimit color">

			<!-- Hip motor -->
			<link name="hip${suffix}">
				<visual>
					<!-- <origin rpy="${pi/2} 0 0" xyz="${-front * (podXOffsetFromOuterEdge - 0.5 * podLength)} ${left * (dPodAxisToEdge - podWidth/2)} 0"/> -->
					<xacro:if value="${front == 1 and left == 1}">
						<origin rpy="0 -${pi} 0" xyz="0.05 -0.073 0"/>
					</xacro:if>

					<xacro:if value="${front == -1 and left == 1}">
						<origin rpy="0 0 0" xyz="-0.05 -0.073 0"/>
					</xacro:if>

					<xacro:if value="${front == 1 and left == -1}">
						<origin rpy="${pi} ${pi} 0" xyz="0.05 0 -0.05"/>
					</xacro:if>

					<xacro:if value="${front == -1 and left == -1}">
						<origin rpy="${pi} 0 0" xyz="-0.05 0.0 0.05"/>
					</xacro:if>

					<xacro:if value="${left == 1}">
						<geometry>
							<!-- <cylinder length="${lowerLegLength}" radius="${lowerLegRadius}"/> -->
							<mesh filename="package://ghost_master/meshes/Hip_Pod_Assembly.stl" scale="1 1 1"/>
						</geometry>
					</xacro:if>
					<xacro:if value="${left == -1}">
						<geometry>
							<!-- <cylinder length="${lowerLegLength}" radius="${lowerLegRadius}"/> -->
							<mesh filename="package://ghost_master/meshes/Hip_Pod_Assembly_Mirrored.stl" scale="1 1 1"/>
						</geometry>
					</xacro:if>
					<material name="gray"/>
					<!-- <geometry>
						<box size="${podLength} ${podHeight} ${podWidth}"/>
					</geometry>
					<material name="${color}"/> -->
				</visual>

				<collision>
					<origin rpy="${pi/2} 0 0" xyz="${-front * (podXOffsetFromOuterEdge - 0.5 * podLength)} ${left * (dPodAxisToEdge - podWidth/2)} 0"/>
					<geometry>
						<box size="${podLength} ${podHeight} ${podWidth}"/>
					</geometry>
				</collision>

				<inertial>
					<mass value="${podMass}"/>
					<!-- <inertia ixx="1/12*${podMass}*(${podWidth}*${podWidth} + ${podHeight}*${podHeight})"  ixy="0"  ixz="0" iyy="1/12*${podMass}*(${podLength}*${podLength} + ${podWidth}*${podWidth})" iyz="0" izz="1/12*${podMass}*(${podLength}*${podLength} + ${podHeight}*${podHeight})"/> -->
					<inertia ixx="0.00891458"  ixy="0"  ixz="0" iyy="0.0100573" iyz="0" izz="0.01533698" />
				</inertial>
			</link>

			<!-- Abduction joint. Joint names are: 8 9 10 11 -->
			<joint name="${abductionJointName}" type="revolute">
				<parent link="body"/>
				<child link="hip${suffix}"/>
				<axis xyz="1 0 0"/>
				<origin xyz="${front * dx} ${left * dy} 0"/>
				<limit effort="${abEffortLimit}" velocity="${abSpeedLimit}" lower="${-abKinLimit}" upper="${abKinLimit}"/>
				<dynamics damping="0.0" friction="0.0"/>
			</joint>

			<!-- Upper leg -->
			<link name="upper${suffix}">
				<visual>
					<xacro:if value="${left == 1}">
						<origin rpy="3.14 0.1179843 0" xyz="0 0 0"/>
						<geometry>
							<mesh filename="package://ghost_master/meshes/Leg_Assembly.stl" scale="0.001 0.001 0.001"/>
						</geometry>
					</xacro:if>
					<xacro:if value="${left == -1}"> <!-- Right-->
						<origin rpy="0 0.1179843 0" xyz="0 0 0"/>
						<geometry>
							<!-- <cylinder length="${lowerLegLength}" radius="${lowerLegRadius}"/> -->
							<mesh filename="package://ghost_master/meshes/Leg_Assembly_Mirrored.stl" scale="0.001 0.001 0.001"/>
						</geometry>
					</xacro:if>
					<material name="gray"/>
				</visual>

				<collision>
					<origin rpy="0 0 0" xyz="-${upperLegLength/2} ${left*0.2*0.0} 0"/>
					<geometry>
						<box size="${upperLegLength} ${upperLegWidth} ${upperLegHeight}"/>
					</geometry>
				</collision>

				<inertial>
					<mass value="${upperLegMass}"/>
					<!-- <inertia ixx="1/12*${upperLegMass}*(${upperLegWidth}*${upperLegWidth} + ${upperLegHeight}*${upperLegHeight})"  ixy="0" ixz="0" iyy="1/12*${upperLegMass}*(${upperLegLength}*${upperLegLength} + ${upperLegHeight}*${upperLegHeight})" iyz="0" izz="1/12*${upperLegMass}*(${upperLegWidth}*${upperLegWidth} + ${upperLegLength}*${upperLegLength})"/> -->
					<inertia ixx="0.0003125"  ixy="0" ixz="0" iyy="0.0040625" iyz="0" izz="0.0040625" />
				</inertial>
			</link>

			<!-- Hip joint. (Motor to upper leg). Joint names are: 0 2 4 6 -->
			<joint name="${hipJointName}" type="revolute">
				<parent link="hip${suffix}"/>
				<child link="upper${suffix}"/>
				<axis xyz="0 -1 0"/>
				<origin xyz="0 ${left * (dPodAxisToEdge + upperLegWidth/2)} 0" /> <!--  rpy="0 -0.3 0" />-->
				<limit effort="${jointEffortLimit}" velocity="${jointSpeedLimit}" lower="${hipJointLowerLimit}" upper="${hipJointUpperLimit}"/>
				<dynamics damping="0.0" friction="0.0"/>
			</joint>

			<!-- Lower leg -->
			<link name="lower${suffix}">
				<visual>
					<origin rpy="0 -0.3874631  0" xyz="0 0 0"/>
					<geometry>
						<!-- <cylinder length="${lowerLegLength}" radius="${lowerLegRadius}"/> -->
						<mesh filename="package://ghost_master/meshes/Lower_Leg_Assembly.stl" scale="0.001 0.001 0.001"/>
					</geometry>

					<material name="gray"/>
				</visual>

				<collision>
					<origin rpy="0 ${pi/2} 0" xyz="${lowerLegLength/2} 0 -${lowerLegAttachLinkLength}"/>
					<geometry>
						<cylinder length="${lowerLegLength}" radius="${lowerLegRadius}"/>
					</geometry>
				</collision>

				<inertial>
					<mass value="${lowerLegMass}"/>
					<inertia ixx="0.00131075"  ixy="0"  ixz="0" iyy="0.00131075" iyz="0" izz="0.0001" />
					<!-- <inertia ixx="1/12*${lowerLegMass}*${lowerLegLength}*${lowerLegLength} + 1/4*${lowerLegMass}*${lowerLegRadius}*${lowerLegRadius}"  ixy="0"  ixz="0" iyy="1/12*${lowerLegMass}*${lowerLegLength}*${lowerLegLength} + 1/4*${lowerLegMass}*${lowerLegRadius}*${lowerLegRadius}" iyz="0" izz="1/2*${lowerLegMass}*${lowerLegRadius}*${lowerLegRadius}" /> -->
				</inertial>
			</link>

			<!-- Knee joint. Joint names are: 1 3 5 7 -->
			<joint name="${kneeJointName}" type="revolute">
				<parent link="upper${suffix}"/>
				<child link="lower${suffix}"/>
				<axis xyz="0 1 0"/>
				<origin xyz="-${upperLegLength} 0 0" rpy="0 -0.05 0" /> <!-- rpy="0 0.261799 0"/>-->
				<limit effort="${jointEffortLimit}" velocity="${jointSpeedLimit}" lower="0" upper="${pi}"/>
				<dynamics damping="0.0" friction="0.0"/>
			</joint>

			<!-- Toe -->
			<link name="toe${suffix}">
				<contact>
					<friction_anchor/>
					<stiffness value="${toeStiffness}"/>
					<damping value="${toeDamping}"/>
					<spinning_friction value="${toeSpinningFriction}"/>
					<lateral_friction value="${toeLateralFriction}"/>
				</contact>
				<!-- <visual>
					<origin rpy="0 0 0" xyz="0 0 0"/>
					<geometry>
						<sphere radius="${toeRadius}"/>
					</geometry>
					<material name="darkgray"/>
				</visual> -->

				<collision>
					<origin rpy="0 0 0" xyz="0 0 0"/>
					<geometry>
						<sphere radius="${toeRadius}"/>
					</geometry>
				</collision>

				<inertial>
					<mass value="${toeMass}"/>
					<inertia ixx="0.000025"  ixy="0"  ixz="0" iyy="0.000025" iyz="0" izz="0.000025" />
					<!-- <inertia ixx="2/5*${toeMass}*${toeRadius}*${toeRadius}"  ixy="0"  ixz="0" iyy="2/5*${toeMass}*${toeRadius}*${toeRadius}" iyz="0" izz="2/5*${toeMass}*${toeRadius}*${toeRadius}" /> -->
				</inertial>
			</link>
			<joint name="jtoe${suffix}" type="fixed">
				<parent link="lower${suffix}"/>
				<child link="toe${suffix}"/>
				<origin xyz="${lowerLegLength} 0 -${lowerLegAttachLinkLength + toeOffset}" rpy="0 0.0 0" />
				<dynamics damping="0.0" friction="0.0"/>
			</joint>

		</xacro:macro>

	<!-- Our four legs (with hip bumpers. But only works well in normal upside mode)
	<xacro:red_leg suffix="0" hipJointName="0" kneeJointName="1" abductionJointName="8" front="1" left="1" hipJointLowerLimit="${-hipBumper}" hipJointUpperLimit="${-hipBumper+2*pi}"/>
 	<xacro:leg suffix="1" hipJointName="2" kneeJointName="3" abductionJointName="9" front="-1" left="1" hipJointLowerLimit="${-pi-hipBumper}" hipJointUpperLimit="${pi-hipBumper}"/>
 	<xacro:leg suffix="2" hipJointName="4" kneeJointName="5" abductionJointName="10" front="1" left="-1" hipJointLowerLimit="${-hipBumper}" hipJointUpperLimit="${-hipBumper+2*pi}"/>
 	<xacro:leg suffix="3" hipJointName="6" kneeJointName="7" abductionJointName="11" front="-1" left="-1" hipJointLowerLimit="${-pi-hipBumper}" hipJointUpperLimit="${pi-hipBumper}"/>
 -->

 <!-- Our four legs -->
 <xacro:leg suffix="0" hipJointName="0" kneeJointName="1" abductionJointName="8" front="1" left="1" hipJointLowerLimit="${-2*pi}" hipJointUpperLimit="${2*pi}" color="red"/>
 <xacro:leg suffix="1" hipJointName="2" kneeJointName="3" abductionJointName="9" front="-1" left="1" hipJointLowerLimit="${-2*pi}" hipJointUpperLimit="${2*pi}" color="purple"/>
 <xacro:leg suffix="2" hipJointName="4" kneeJointName="5" abductionJointName="10" front="1" left="-1" hipJointLowerLimit="${-2*pi}" hipJointUpperLimit="${2*pi}" color="purple"/>
 <xacro:leg suffix="3" hipJointName="6" kneeJointName="7" abductionJointName="11" front="-1" left="-1" hipJointLowerLimit="${-2*pi}" hipJointUpperLimit="${2*pi}" color="purple"/>

</xacro:macro>
</robot>
