TORQUE_CONTROL Not working

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
exo
Posts: 2
Joined: Fri Nov 19, 2021 7:08 pm

TORQUE_CONTROL Not working

Post by exo »

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?

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>

Post Reply