I'm new to PyBullet, so I wouldn't be surprised if the answer to my question is something fairly obvious.
I used the SolidWorks to URDF exporter to export my bipedal robot design to PyBullet. When the model loads in and I run the simulation in real time, the robot can fall over on its side but the joints can't move.
POSITION_CONTROL has no effect. I also tried velocity control and torque control with force=0, expecting the robot to collapse like a ragdoll, but it continues to act like a figurine. When I load other models in (e.g. biped2d_pybullet.urdf), they collapse like ragdolls.
The robot has 8 revolute joints, and there is no damping specified in the URDF (so it should default to zero).
Strangely enough, the following command just causes the robot to vibrate without moving its joints:
Code: Select all
for joint in range(p.getNumJoints(robot)):
p.setJointMotorControl2(robot, joint, p.VELOCITY_CONTROL, targetVelocity=50, force=500)
Code: Select all
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.5.1-0-g916b5db Build Version: 1.5.7152.31018
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="spryped rev02">
<link
name="body">
<inertial>
<origin
xyz="0.00146683164005432 -0.0471884402468711 0.076169520744687"
rpy="0 0 0" />
<mass
value="3.70313889840838" />
<inertia
ixx="0.00429570716761128"
ixy="3.03278167284736E-09"
ixz="-2.3332902790982E-06"
iyy="0.00335471620497785"
iyz="-8.32059718979089E-06"
izz="0.00591264443127761" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/body.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/body.STL" />
</geometry>
</collision>
</link>
<link
name="left femur">
<inertial>
<origin
xyz="0.0014637 0.050952 -0.056842"
rpy="0 0 0" />
<mass
value="0.30999" />
<inertia
ixx="0.00069104"
ixy="-9.483E-08"
ixz="3.5642E-05"
iyy="0.00070116"
iyz="3.5726E-05"
izz="0.00059024" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left femur.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left femur.STL" />
</geometry>
</collision>
</link>
<joint
name="1"
type="revolute">
<origin
xyz="0.14547 -0.10347 0.055904"
rpy="0 0 0" />
<parent
link="body" />
<child
link="left femur" />
<axis
xyz="0 -1 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="left tibiotarsus">
<inertial>
<origin
xyz="0.0492126930232007 0.0535384623347315 -0.0842779052215516"
rpy="0 0 0" />
<mass
value="2.87772219223662" />
<inertia
ixx="0.00533528507057087"
ixy="7.37273330855451E-08"
ixz="-1.09674863027954E-07"
iyy="0.0034959774077804"
iyz="0.00148266889917068"
izz="0.00225851284239546" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left tibiotarsus.STL" />
</geometry>
<material
name="">
<color
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left tibiotarsus.STL" />
</geometry>
</collision>
</link>
<joint
name="2"
type="revolute">
<origin
xyz="-0.04965 0.05165 -0.114"
rpy="0 0 0" />
<parent
link="left femur" />
<child
link="left tibiotarsus" />
<axis
xyz="-1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="left tarsometatarsus">
<inertial>
<origin
xyz="0.0653386529323088 0.00410211529259828 -0.10795430435685"
rpy="0 0 0" />
<mass
value="0.386426569747224" />
<inertia
ixx="0.00159650766426425"
ixy="3.44828011287871E-05"
ixz="-3.16861898281007E-05"
iyy="0.00150999755658726"
iyz="-0.000310806569340194"
izz="0.000553884465795286" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left tarsometatarsus.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left tarsometatarsus.STL" />
</geometry>
</collision>
</link>
<joint
name="3"
type="revolute">
<origin
xyz="-0.0005 0.10545 -0.16876"
rpy="0 0 0" />
<parent
link="left tibiotarsus" />
<child
link="left tarsometatarsus" />
<axis
xyz="-1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="left toe">
<inertial>
<origin
xyz="0.00549999991446565 0.0246536250465857 -0.0376986621147014"
rpy="0 0 0" />
<mass
value="0.111889664829208" />
<inertia
ixx="0.000217562035776632"
ixy="-7.62138636248417E-13"
ixz="8.82511593906485E-14"
iyy="3.26048621480854E-05"
iyz="-1.99012084230601E-06"
izz="0.00019521205650501" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left toe.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/left toe.STL" />
</geometry>
</collision>
</link>
<joint
name="4"
type="revolute">
<origin
xyz="0.0444 -0.12296 -0.48273"
rpy="0 0 0" />
<parent
link="left tarsometatarsus" />
<child
link="left toe" />
<axis
xyz="-1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="right femur">
<inertial>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<mass
value="0" />
<inertia
ixx="0"
ixy="0"
ixz="0"
iyy="0"
iyz="0"
izz="0" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right femur.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right femur.STL" />
</geometry>
</collision>
</link>
<joint
name="5"
type="revolute">
<origin
xyz="-0.14253 -0.10347 0.055904"
rpy="0 0 0" />
<parent
link="body" />
<child
link="right femur" />
<axis
xyz="0 -1 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="right tibiotarsus">
<inertial>
<origin
xyz="-0.0492495047620073 0.053501663392721 -0.0842309932012287"
rpy="0 0 0" />
<mass
value="2.87581042362099" />
<inertia
ixx="0.00533529391220343"
ixy="-7.07923527083726E-08"
ixz="1.22239241571791E-07"
iyy="0.00349593182338153"
iyz="0.00148267255132337"
izz="0.00225848427762363" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right tibiotarsus.STL" />
</geometry>
<material
name="">
<color
rgba="0.776470588235294 0.756862745098039 0.737254901960784 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right tibiotarsus.STL" />
</geometry>
</collision>
</link>
<joint
name="6"
type="revolute">
<origin
xyz="0.04965 0.05165 -0.114"
rpy="0 0 0" />
<parent
link="right femur" />
<child
link="right tibiotarsus" />
<axis
xyz="1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="right tarsometatarsus">
<inertial>
<origin
xyz="-0.0654870852390804 0.00442186689390905 -0.107335066340509"
rpy="0 0 0" />
<mass
value="0.388385413325526" />
<inertia
ixx="0.0015965161251087"
ixy="-3.45791013100941E-05"
ixz="3.15809364112141E-05"
iyy="0.00151198241193131"
iyz="-0.000307831493495565"
izz="0.000551992188290932" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right tarsometatarsus.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right tarsometatarsus.STL" />
</geometry>
</collision>
</link>
<joint
name="7"
type="revolute">
<origin
xyz="0.0005 0.10545 -0.16876"
rpy="0 0 0" />
<parent
link="right tibiotarsus" />
<child
link="right tarsometatarsus" />
<axis
xyz="1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
<link
name="right toe">
<inertial>
<origin
xyz="-0.0160000000855337 0.00863889472199542 -0.0319952935808615"
rpy="0 0 0" />
<mass
value="0.111889664829208" />
<inertia
ixx="0.000217562035776632"
ixy="-7.62138591136326E-13"
ixz="8.82511158185202E-14"
iyy="3.26048621484914E-05"
iyz="-1.99012084072618E-06"
izz="0.000195212056504604" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right toe.STL" />
</geometry>
<material
name="">
<color
rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://spryped rev02/meshes/right toe.STL" />
</geometry>
</collision>
</link>
<joint
name="8"
type="revolute">
<origin
xyz="-0.0339 -0.10545 -0.48875"
rpy="0 0 0" />
<parent
link="right tarsometatarsus" />
<child
link="right toe" />
<axis
xyz="-1 0 0" />
<limit
lower="0"
upper="0"
effort="0"
velocity="0" />
</joint>
</robot>
Code: Select all
import pybullet as p
import time
import pybullet_data
GRAVITY = -9.81
dt = 1e-3
iters = 2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
planeID = p.loadURDF("plane.urdf")
robot = p.loadURDF("Desktop/spryped rev02/urdf/spryped rev02.urdf", [0,0,0.8], useFixedBase=0)
p.setGravity(0,0, GRAVITY)
p.setTimeStep(dt)
jointFrictionForce = 0
for joint in range(p.getNumJoints(robot)):
p.setJointMotorControl2(robot, joint, p.VELOCITY_CONTROL, targetVelocity=50, force=20)
p.setRealTimeSimulation(1)
time.sleep(10000)