I have created a complete URDF for my robot in PyBullet, but I'm having trouble getting the joints to rotate correctly. Specifically, when I try to move a joint, it doesn't rotate about the correct coordinate, and the link separates from the body of the robot.
Initially, I kept the joint origin and axis at their default values in the URDF and did not move any joints, which resulted in the robot being in a correct and complete static orientation. However, when I try to make a joint move in pybullet, the joint does not rotate about the correct point, and the link separates from the parent body of the robot.
I have already created reference axes for the servos in my SolidWorks files and attempted to enter this information into the URDF as the joint origin parameter. However, this approach has not resolved the issue.
I suspect that the problem lies in my translation between the parent origin coordinate, joint origin coordinate, and child origin coordinate. I need help in determining the correct joint origin/axis parameter that will allow my joints to rotate properly around the correct vector.
Can anyone offer any advice or suggestions on how to properly determine the joint origin parameter for the joints in my URDF and resolve this issue?
Python code:
import pybullet as p
import time
import pybullet_data
import numpy as np
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,0)
groundId = p.loadURDF("plane.urdf")
robotStartPos = [0,0,.25]
robotStartOrientation = p.getQuaternionFromEuler([1.57,0,0])
urdf = "C:\\Users\\danie\\OneDrive\\Desktop\\Robot_Simulation\\myrobot.urdf" #change to urdf file path
robotId = p.loadURDF(urdf,robotStartPos, robotStartOrientation)
jointIndex = 2 # first joint is number 0
for i in range (10000):
p.stepSimulation()
p.setJointMotorControl2(robotId, jointIndex, controlMode = p.POSITION_CONTROL, targetPosition=0.8+0.6*np.sin(i*0.01))
time.sleep(1./240.)
robotPos, robotOrn = p.getBasePositionAndOrientation(robotId)
print(robotPos, robotOrn)
p.disconnect()
URDF File:
<?xml version="1.0"?>
<robot name="myrobot">
<link name="base">
<visual>
<geometry>
<mesh filename="base.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Cyan">
<color rgba="0 1.0 1.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.845"/>
<origin xyz="-.036 .058 -.123"/>
<inertia ixx=".0018" ixy="-.00177" ixz="0.00374" iyy=".0156" iyz="-.006" izz=".00526" />
</inertial>
</link>
<link name="hip_left">
<visual>
<geometry>
<mesh filename="hip_left.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Orange">
<color rgba="1.0 0.41 0.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.0588"/>
<origin xyz="-.036 .028 -.222"/>
<inertia ixx=".03" ixy="-0.00006" ixz="0.0004" iyy=".002" iyz="-0.000371" izz=".000175" />
</inertial>
</link>
<link name="hip_right">
<visual>
<geometry>
<mesh filename="hip_right.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Orange">
<color rgba="1.0 0.41 0.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.0588"/>
<origin xyz="-.036 .029 -.254"/>
<inertia ixx=".03" ixy="-0.00006" ixz="0.0004" iyy=".002" iyz="-0.000371" izz=".000175" />
</inertial>
</link>
<link name="upper_leg_left">
<visual>
<geometry>
<mesh filename="upper_leg_left.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Green">
<color rgba="0.0 1.0 0.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.05846"/>
<origin xyz="-.035 -.092 -.223"/>
<inertia ixx=".0035" ixy="0.0018" ixz="0.000456" iyy=".0029" iyz="0.00119" izz=".00065" />
</inertial>
</link>
<link name="upper_leg_right">
<visual>
<geometry>
<mesh filename="upper_leg_right.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Green">
<color rgba="0.0 1.0 0.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.05846"/>
<origin xyz="-.0378 -.091 -.2444"/>
<inertia ixx=".0035" ixy="0.0018" ixz="0.000456" iyy=".0029" iyz="0.00119" izz=".00065" />
</inertial>
</link>
<link name="lower_leg_left">
<visual>
<geometry>
<mesh filename="lower_leg_left.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Pink">
<color rgba="1.0 0.0 1.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.05846"/>
<origin xyz="-.0427 -.2019 -.2223"/>
<inertia ixx=".00369" ixy="0.00036" ixz="0.00179" iyy=".00209" iyz="0.00179" izz=".001823" />
</inertial>
</link>
<link name="lower_leg_right">
<visual>
<geometry>
<mesh filename="lower_leg_right.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name = "Pink">
<color rgba="1.0 0.0 1.0 1.0"/>
</material>
</visual>
<inertial>
<mass value="0.05846"/>
<origin xyz="-.05398 -.202 -.0253"/>
<inertia ixx=".00369" ixy="0.00036" ixz="0.00179" iyy=".00209" iyz="0.00179" izz=".001823" />
</inertial>
</link>
<joint name="joint1" type="revolute">
<parent link="base"/>
<child link="hip_left"/>
<origin xyz="0 0 0"/>
<axis xyz="0 -1 0"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
<joint name="joint2" type="revolute">
<parent link="base"/>
<child link="hip_right"/>
<origin xyz="0 0 0"/>
<axis xyz="0 -1 0"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
<joint name="joint3" type="revolute">
<parent link="hip_left"/>
<child link="upper_leg_left"/>
<origin xyz="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
<joint name="joint4" type="revolute">
<parent link="hip_right"/>
<child link="upper_leg_right"/>
<origin xyz="0 0 0"/>
<axis xyz="0 0 -1"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
<joint name="joint5" type="revolute">
<parent link="upper_leg_left"/>
<child link="lower_leg_left"/>
<origin xyz="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
<joint name="joint6" type="revolute">
<parent link="upper_leg_right"/>
<child link="lower_leg_right"/>
<origin xyz="0 0 0"/>
<axis xyz="0 0 -1"/>
<limit lower="-3.14159" upper="3.14159" effort="10" velocity="10"/>
</joint>
</robot>
I tried putting in various values for the joint origins in the URDF using various translations but none helped fix the issue. I have yet to attempt to change the joint axis parameter.
