TORQUE_CONTROL Not working
Posted: Thu Nov 25, 2021 4:38 pm
I am using pybullet to simulate a bipedal robot using pre calculated trajectory. I have joints angles for the whole trajectory. It works perfectly using POSITION_CONTROL with "setJointMotorControl2". However, using a torque input with TORQUE_CONTROL the robot gets loose and the joints don't move.
I am also using the pdControllerExplicit from the examples provided in bullet3 repository by pybullet to generate torque values from the desired angles and error. I have also made sure to turn off the velocity control giving zero force.
1. Why is the TORQUE_CONTROL failing to work? I have tried the following to troubleshoot it am I missing something else?
-Changing the masses and inertias (I tried both using inertia values from the URDF file and letting pybullet calculate them)
-enabled the JointForceTorqueSensor and recorded the values for the torque and they gave zeros or they give constant values throughout the simulation.
-checked the code for bugs as well, here is the code and the URDF file below.
-Changing the timestep and sleeptime values.
Is there a problem with the TORQUE_CONTROL command or something I mentioned or something else entirely.
2. Also, How does the "setJointMotorControl2" work? Is there an open access source code for it?
URDF File
I am also using the pdControllerExplicit from the examples provided in bullet3 repository by pybullet to generate torque values from the desired angles and error. I have also made sure to turn off the velocity control giving zero force.
1. Why is the TORQUE_CONTROL failing to work? I have tried the following to troubleshoot it am I missing something else?
-Changing the masses and inertias (I tried both using inertia values from the URDF file and letting pybullet calculate them)
-enabled the JointForceTorqueSensor and recorded the values for the torque and they gave zeros or they give constant values throughout the simulation.
-checked the code for bugs as well, here is the code and the URDF file below.
-Changing the timestep and sleeptime values.
Is there a problem with the TORQUE_CONTROL command or something I mentioned or something else entirely.
2. Also, How does the "setJointMotorControl2" work? Is there an open access source code for it?
Code: Select all
import pybullet as pb
import time
from numpy import *
import pybullet_data
import struct
def computePD(bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
# numJoints = pb.getNumJoints(bodyUniqueId)
numJoints = len(jointIndices)
jointStates = pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
for i in range(numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
q = array(q1)
qdot = array(qdot1)
qdes = array(desiredPositions)
qdotdes = array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = diagflat(kps)
Kd = diagflat(kds)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = array(maxForces)
forces = clip(forces, -maxF, maxF)
return forces
#______________________________________________________________________________
#joint axis matrix
jointIdListR = [0,1,2,3,4,5]
jointIdListL = [6,7,8,9,10,11]
LEG_DOF = 6
maxForce = 10e20
maxForceListForLeg = [maxForce]*LEG_DOF
with open("JointsLeftTotalMATLAB6.csv") as jointslefttotal:
jointslefttotal = loadtxt(jointslefttotal, delimiter=",")
with open("JointsRightTotalMATLAB6.csv") as jointsrighttotal:
jointsrighttotal = loadtxt(jointsrighttotal, delimiter=",")
with open("torright.csv") as torright:
torright = loadtxt(torright, delimiter=",")
with open("torleft.csv") as torleft:
torleft = loadtxt(torleft, delimiter=",")
with open("velleft.csv") as velleft:
velleft = loadtxt(velleft, delimiter=",")
with open("velright.csv") as velright:
velright = loadtxt(velright, delimiter=",")
#______________________________________________________________________________
pb.connect(pb.GUI)
#Add earth like gravity
pb.setGravity(0,0,-9.807)
pb.setRealTimeSimulation(1)
#Point the camera at the robot at the desired angle and distance
pb.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=135, cameraPitch=-30, cameraTargetPosition=[0.0, 0.0, 0.25])
if (0):
t0=time.time()
t=time.time()
while ((t-t0)<10):
t=time.time()
pb.disconnect()
break
#______________________________________________________________________________
# controlMode=pb.POSITION_CONTROL
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = pb.loadURDF("plane.urdf")
robotId = pb.loadURDF("urdf/bipedalMini.urdf",[0,0,0.8],useFixedBase=True)
for i in range (1):
pb.stepSimulation()
time.sleep(1./240.)
# qua = pb.getBasePositionAndOrientation(robotId)
position, orientation = pb.getBasePositionAndOrientation(robotId)
pb.resetBasePositionAndOrientation(robotId, position, orientation)
numJoint = pb.getNumJoints(robotId)
jointIdList = list(range(numJoint))
#______________________________________________________________________________
TorLHY = array([])
TorLHR = array([])
TorLHP = array([])
TorLKP = array([])
TorLAP = array([])
TorLAR = array([])
TorRHY = array([])
TorRHR = array([])
TorRHP = array([])
TorRKP = array([])
TorRAP = array([])
TorRAR = array([])
TorLHY1 = transpose(array([0,0,0,0,0,0]))
TorLHR1 = transpose(array([0,0,0,0,0,0]))
TorLHP1 = transpose(array([0,0,0,0,0,0]))
TorLKP1 = transpose(array([0,0,0,0,0,0]))
TorLAP1 = transpose(array([0,0,0,0,0,0]))
TorLAR1 = transpose(array([0,0,0,0,0,0]))
TorRHY1 = transpose(array([0,0,0,0,0,0]))
TorRHR1 = transpose(array([0,0,0,0,0,0]))
TorRHP1 = transpose(array([0,0,0,0,0,0]))
TorRKP1 = transpose(array([0,0,0,0,0,0]))
TorRAP1 = transpose(array([0,0,0,0,0,0]))
TorRAR1 = transpose(array([0,0,0,0,0,0]))
#______________________________________________________________________________
for i in arange(len(jointIdList)):
pb.enableJointForceTorqueSensor(robotId, jointIndex=i)
#______________________________________________________________________________
for i in arange(len(jointIdList)):
pb.setJointMotorControl2(robotId, jointIndex = i, controlMode=pb.VELOCITY_CONTROL, force=0)
#______________________________________________________________________________
pb.setTimeStep(1/240)
kp = [1000]*LEG_DOF
kd = [300]*LEG_DOF
timeStep = 1./240.
#______________________________________________________________________________
for i in arange(len(jointsrighttotal[0])):
tausright = computePD(robotId, jointIdListR, jointsrighttotal[:,i], velright[:,i], kp, kd, maxForceListForLeg, timeStep)
tausleft = computePD(robotId, jointIdListL, jointslefttotal[:,i], velleft[:,i], kp, kd, maxForceListForLeg, timeStep)
print(tausright[2])
pb.setJointMotorControl2(robotId, jointIndex=0, controlMode=pb.TORQUE_CONTROL,
force=tausright[0])
pb.setJointMotorControl2(robotId, jointIndex=1, controlMode=pb.TORQUE_CONTROL,
force=tausright[1])
pb.setJointMotorControl2(robotId, jointIndex=2, controlMode=pb.TORQUE_CONTROL,
force=tausright[2])
pb.setJointMotorControl2(robotId, jointIndex=3, controlMode=pb.TORQUE_CONTROL,
force=tausright[3])
pb.setJointMotorControl2(robotId, jointIndex=4, controlMode=pb.TORQUE_CONTROL,
force=tausright[4])
pb.setJointMotorControl2(robotId, jointIndex=5, controlMode=pb.TORQUE_CONTROL,
force=tausright[5])
pb.setJointMotorControl2(robotId, jointIndex=6, controlMode=pb.TORQUE_CONTROL,
force=tausleft[0])
pb.setJointMotorControl2(robotId, jointIndex=7, controlMode=pb.TORQUE_CONTROL,
force=tausleft[1])
pb.setJointMotorControl2(robotId, jointIndex=8, controlMode=pb.TORQUE_CONTROL,
force=tausleft[2])
pb.setJointMotorControl2(robotId, jointIndex=9, controlMode=pb.TORQUE_CONTROL,
force=tausleft[3])
pb.setJointMotorControl2(robotId, jointIndex=10, controlMode=pb.TORQUE_CONTROL,
force=tausleft[4])
pb.setJointMotorControl2(robotId, jointIndex=11, controlMode=pb.TORQUE_CONTROL,
force=tausleft[5])
TorLHY = append(TorLHY,pb.getJointState(robotId, jointIndex=0)[-1])
TorLHR = append(TorLHR,pb.getJointState(robotId, jointIndex=1)[-1])
TorLHP = append(TorLHP,pb.getJointState(robotId, jointIndex=2)[-1])
TorLKP = append(TorLKP,pb.getJointState(robotId, jointIndex=3)[-1])
TorLAP = append(TorLAP,pb.getJointState(robotId, jointIndex=4)[-1])
TorLAR = append(TorLAR,pb.getJointState(robotId, jointIndex=5)[-1])
TorRHY = append(TorRHY,pb.getJointState(robotId, jointIndex=6)[-1])
TorRHR = append(TorRHR,pb.getJointState(robotId, jointIndex=7)[-1])
TorRHP = append(TorRHP,pb.getJointState(robotId, jointIndex=8)[-1])
TorRKP = append(TorRKP,pb.getJointState(robotId, jointIndex=9)[-1])
TorRAP = append(TorRAP,pb.getJointState(robotId, jointIndex=10)[-1])
TorRAR = append(TorRAR,pb.getJointState(robotId, jointIndex=11)[-1])
TorLHY1 = vstack((TorLHY1,pb.getJointState(robotId, jointIndex=0)[-2]))
TorLHR1 = vstack((TorLHR1,pb.getJointState(robotId, jointIndex=1)[-2]))
TorLHP1 = vstack((TorLHP1,pb.getJointState(robotId, jointIndex=2)[-2]))
TorLKP1 = vstack((TorLKP1,pb.getJointState(robotId, jointIndex=3)[-2]))
TorLAP1 = vstack((TorLAP1,pb.getJointState(robotId, jointIndex=4)[-2]))
TorLAR1 = vstack((TorLAR1,pb.getJointState(robotId, jointIndex=5)[-2]))
TorRHY1 = vstack((TorRHY1,pb.getJointState(robotId, jointIndex=6)[-2]))
TorRHR1 = vstack((TorRHR1,pb.getJointState(robotId, jointIndex=7)[-2]))
TorRHP1 = vstack((TorRHP1,pb.getJointState(robotId, jointIndex=8)[-2]))
TorRKP1 = vstack((TorRKP1,pb.getJointState(robotId, jointIndex=9)[-2]))
TorRAP1 = vstack((TorRAP1,pb.getJointState(robotId, jointIndex=10)[-2]))
TorRAR1 = vstack((TorRAR1,pb.getJointState(robotId, jointIndex=11)[-2]))
pb.stepSimulation()
time.sleep(1./960.)
#______________________________________________________________________________
pb.disconnect()
URDF File
Code: Select all
<?xml version="1.0" ?>
<robot name="quadrupedal_robot" xmlns:xacro="https://ros.org/wiki/xacro">
<!--link name="world"/>
<joint name="world_joint" type="floating">
<parent link="world"/>
<child link="body"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint -->
<link name="body">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.06765345" ixy="0.0" ixz="0.0" iyy="0.053440875" iyz="0.0" izz="0.017235495"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/TorsoMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/TorsoMini.stl"/>
</geometry>
</collision>
</link>
<joint name="r_yaw_hipJoint" type="revolute">
<parent link="body"/>
<child link="r_yaw_hipLink"/>
<axis xyz="0 0 1"/>
<origin rpy="0 0 0" xyz="0.0 -0.12 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="0" upper="0" velocity="10.0"/>
</joint>
<transmission name="r_yaw_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_yaw_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_yaw_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_yaw_hipLink">
<inertial>
<mass value="0.108149"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="3.9598421e-05" ixy="0.0" ixz="0.0" iyy="3.9598427e-05" iyz="0.0" izz="5.0357027e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/hip_Zaxis_link.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/hip_Zaxis_link.stl"/>
</geometry>
</collision>
</link>
<joint name="r_roll_hipJoint" type="revolute">
<parent link="r_yaw_hipLink"/>
<child link="r_roll_hipLink"/>
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-0.5" upper="0.5" velocity="10.0"/>
</joint>
<transmission name="r_roll_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_roll_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_roll_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_roll_hipLink">
<inertial>
<mass value="0.108149"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="5.0357027e-05" ixy="0.0" ixz="0.0" iyy="3.9598427e-05" iyz="0.0" izz="3.9598421e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/hip_Xaxis_link.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/hip_Xaxis_link.stl"/>
</geometry>
</collision>
</link>
<joint name="r_pitch_hipJoint" type="revolute">
<parent link="r_roll_hipLink"/>
<child link="r_thighLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="r_pitch_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_pitch_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_pitch_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_thighLink">
<inertial>
<mass value="0.12805"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.000525492139" ixy="0.0" ixz="0.0" iyy="0.000525518548" iyz="0.0" izz="2.4541119e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/upper_leg_lengthMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/upper_leg_lengthMini.stl"/>
</geometry>
</collision>
</link>
<joint name="r_kneeJoint" type="revolute">
<parent link="r_thighLink"/>
<child link="r_kneeLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.40"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="r_kneeTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_kneeJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_kneeMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_kneeLink">
<inertial>
<mass value="0.123672"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.18"/>
<inertia ixx="0.000345322965" ixy="0.0" ixz="0.0" iyy="0.000345322962" iyz="0.0" izz="2.2817471e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/lower_leg_lengthMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/lower_leg_lengthMini.stl"/>
</geometry>
</collision>
</link>
<joint name="r_pitch_ankleJoint" type="revolute">
<parent link="r_kneeLink"/>
<child link="r_ankleLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.38"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="r_pitch_ankleTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_pitch_ankleJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_pitch_ankleMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_ankleLink">
<inertial>
<mass value="0.0972"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="2.025e-05" ixy="0.0" ixz="0.0" iyy="2.025e-05" iyz="0.0" izz="1.458e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/Ankle_Sphere_Hidden.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/Ankle_Sphere_Hidden.stl"/>
</geometry>
</collision>
</link>
<joint name="r_roll_ankleJoint" type="revolute">
<parent link="r_ankleLink"/>
<child link="r_tipLink"/>
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="r_roll_ankleTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="r_roll_ankleJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="r_roll_ankleMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="r_tipLink">
<inertial>
<mass value="0.139622"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="8.1336818e-05" ixy="0.0" ixz="7.800852e-06" iyy="0.000185289302" iyz="0.0" izz="0.000251133962"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/FootMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/FootMini.stl"/>
</geometry>
</collision>
</link>
<!-- left leg -->
<joint name="l_yaw_hipJoint" type="revolute">
<parent link="body"/>
<child link="l_yaw_hipLink"/>
<axis xyz="0 0 1"/>
<origin rpy="0 0 0" xyz="0.0 0.12 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="0" upper="0" velocity="10.0"/>
</joint>
<transmission name="l_yaw_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_yaw_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_yaw_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_yaw_hipLink">
<inertial>
<mass value="0.108149"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="3.9598421e-05" ixy="0.0" ixz="0.0" iyy="3.9598427e-05" iyz="0.0" izz="5.0357027e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/hip_Zaxis_link.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/hip_Zaxis_link.stl"/>
</geometry>
</collision>
</link>
<joint name="l_roll_hipJoint" type="revolute">
<parent link="l_yaw_hipLink"/>
<child link="l_roll_hipLink"/>
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-0.5" upper="0.5" velocity="10.0"/>
</joint>
<transmission name="l_roll_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_roll_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_roll_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_roll_hipLink">
<inertial>
<mass value="0.108149"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="5.0357027e-05" ixy="0.0" ixz="0.0" iyy="3.9598427e-05" iyz="0.0" izz="3.9598421e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/hip_Xaxis_link.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/hip_Xaxis_link.stl"/>
</geometry>
</collision>
</link>
<joint name="l_pitch_hipJoint" type="revolute">
<parent link="l_roll_hipLink"/>
<child link="l_thighLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="l_pitch_hipTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_pitch_hipJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_pitch_hipMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_thighLink">
<inertial>
<mass value="0.12805"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.000525492139" ixy="0.0" ixz="0.0" iyy="0.000525518548" iyz="0.0" izz="2.4541119e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/upper_leg_lengthMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/upper_leg_lengthMini.stl"/>
</geometry>
</collision>
</link>
<joint name="l_kneeJoint" type="revolute">
<parent link="l_thighLink"/>
<child link="l_kneeLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.40"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="l_kneeTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_kneeJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_kneeMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_kneeLink">
<inertial>
<mass value="0.123672"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.18"/>
<inertia ixx="0.000345322965" ixy="0.0" ixz="0.0" iyy="0.000345322962" iyz="0.0" izz="2.2817471e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/lower_leg_lengthMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/lower_leg_lengthMini.stl"/>
</geometry>
</collision>
</link>
<joint name="l_pitch_ankleJoint" type="revolute">
<parent link="l_kneeLink"/>
<child link="l_ankleLink"/>
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.38"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="l_pitch_ankleTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_pitch_ankleJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_pitch_ankleMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_ankleLink">
<inertial>
<mass value="0.0972"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="2.025e-05" ixy="0.0" ixz="0.0" iyy="2.025e-05" iyz="0.0" izz="1.458e-05"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/Ankle_Sphere_Hidden.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/Ankle_Sphere_Hidden.stl"/>
</geometry>
</collision>
</link>
<joint name="l_roll_ankleJoint" type="revolute">
<parent link="l_ankleLink"/>
<child link="l_tipLink"/>
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<!--dynamics damping="0.05"/-->
<dynamics friction="0.05"/>
<limit effort="18.0" lower="-1.5" upper="1.5" velocity="10.0"/>
</joint>
<transmission name="l_roll_ankleTrans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="l_roll_ankleJoint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="l_roll_ankleMotor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanismReduction>1</mechanismReduction>
</actuator>
</transmission>
<link name="l_tipLink">
<inertial>
<mass value="0.139622"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="8.1336818e-05" ixy="0.0" ixz="7.800852e-06" iyy="0.000185289302" iyz="0.0" izz="0.000251133962"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/stl/FootMini.stl"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="meshes/stl/FootMini.stl"/>
</geometry>
</collision>
</link>
</robot>