Bart/bullet3
1
0 Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
bla committed 2019-06-06 21:54:47 +00:00
2 parents 6ab09fe06e d7c0c9c017
commit 3e32cf94e7
59 files changed
+135459 -11686
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

examples/pybullet/gym/pybullet_data/laikago/laikago.py
+110 -110
View File
@@ -1,110 +1,110 @@
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open("data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
import pybullet as p
import time
p.connect(p.GUI)
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open("data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
+10259
View File
File diff suppressed because it is too large. Load diff
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
+37168
View File
File diff suppressed because it is too large. Load diff
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_lower_link.obj
+32070
View File
File diff suppressed because it is too large. Load diff
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_upper_link.obj
+45910
View File
File diff suppressed because it is too large. Load diff
examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf
+480
View File
@@ -0,0 +1,480 @@
<?xml version="1.0" ?>
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
<link name="body">
<inertial>
<mass value="3.3"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_fr_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
<parent link="body"/>
<child link="abduct_fr"/>
</joint>
<link name="abduct_fr">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="abduct_fr_to_thigh_fr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="abduct_fr"/>
<child link="thigh_fr"/>
</joint>
<link name="thigh_fr">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_fr_to_knee_fr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_fr"/>
<child link="shank_fr"/>
</joint>
<link name="shank_fr">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fr_joint" type="fixed">
<parent link="shank_fr"/>
<child link="toe_fr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_fl_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
<parent link="body"/>
<child link="abduct_fl"/>
</joint>
<link name="abduct_fl">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="abduct_fl_to_thigh_fl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="abduct_fl"/>
<child link="thigh_fl"/>
</joint>
<link name="thigh_fl">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_fl_to_knee_fl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_fl"/>
<child link="shank_fl"/>
</joint>
<link name="shank_fl">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_fl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_fl_joint" type="fixed">
<parent link="shank_fl"/>
<child link="toe_fl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hr_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
<parent link="body"/>
<child link="abduct_hr"/>
</joint>
<link name="abduct_hr">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="abduct_hr_to_thigh_hr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="abduct_hr"/>
<child link="thigh_hr"/>
</joint>
<link name="thigh_hr">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_hr_to_knee_hr_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_hr"/>
<child link="shank_hr"/>
</joint>
<link name="shank_hr">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hr">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hr_joint" type="fixed">
<parent link="shank_hr"/>
<child link="toe_hr"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="torso_to_abduct_hl_j" type="continuous">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
<parent link="body"/>
<child link="abduct_hl"/>
</joint>
<link name="abduct_hl">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="abduct_hl_to_thigh_hl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="abduct_hl"/>
<child link="thigh_hl"/>
</joint>
<link name="thigh_hl">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0" xyz="0 0 0"/>
</collision>
</link>
<joint name="thigh_hl_to_knee_hl_j" type="continuous">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="thigh_hl"/>
<child link="shank_hl"/>
</joint>
<link name="shank_hl">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0" xyz="0 0 0"/>
</collision>
</link>
<link name="toe_hl">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="3.0"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.015"/>
</geometry>
</collision>
<inertial>
<mass value="0.15"/>
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
</link>
<joint name="toe_hl_joint" type="fixed">
<parent link="shank_hl"/>
<child link="toe_hl"/>
<origin xyz="0 0 -0.18"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
</robot>
examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.stl
BIN
View File
Binary file not shown.
examples/pybullet/gym/pybullet_data/quadruped/microtaur/channel.urdf
-29
View File
@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.13"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.019 -0.019 -0.1502"/>
<geometry>
<mesh filename="channel.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.038 0.038 0.305"/>
</geometry>
</collision>
</link>
</robot>
examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.stl
BIN
View File
Binary file not shown.
examples/pybullet/gym/pybullet_data/quadruped/microtaur/d435i.urdf
-29
View File
@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.081"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0125"/>
<geometry>
<mesh filename="d435i.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.09 0.025 0.025"/>
</geometry>
</collision>
</link>
</robot>
examples/pybullet/gym/pybullet_data/quadruped/microtaur/microtaur.urdf
-1663
View File
File diff suppressed because it is too large. Load diff
examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.stl
BIN
View File
Binary file not shown.
examples/pybullet/gym/pybullet_data/quadruped/microtaur/plate.urdf
-29
View File
@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.13"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.037 0.0045 0"/>
<geometry>
<mesh filename="plate.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.074 0.009 0.138"/>
</geometry>
</collision>
</link>
</robot>
examples/pybullet/gym/pybullet_data/quadruped/microtaur/quadru.py
-22
View File
@@ -1,22 +0,0 @@
import pybullet as p
cin = p.connect(p.SHARED_MEMORY)
if (cin < 0):
cin = p.connect(p.GUI)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("quadruped/microtaur/microtaur.urdf", 0.858173,-0.698485,0.227967,-0.002864,0.000163,0.951778,0.306776)]
ob = objects[0]
jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.568555, 0.000000, -2.177277, 1.570089, 0.000000, -2.184705, 1.570229, 0.000000, -2.182261, 1.570008, 0.000000, -2.184197, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, -1.569978, 0.000000, 2.184092, -1.569669, 0.000000, 2.186906, -1.570584, 0.000000, 2.181503, -1.568404, 0.000000, 2.178427 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
cid0 = p.createConstraint(1,35,1,32,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid0,maxForce=1000.000000)
cid1 = p.createConstraint(1,7,1,10,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid1,maxForce=1000.000000)
cid2 = p.createConstraint(1,41,1,38,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid2,maxForce=1000.000000)
cid3 = p.createConstraint(1,13,1,16,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.100000],[0.000000,0.010000,0.100000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000])
p.changeConstraint(cid3,maxForce=1000.000000)
p.setGravity(0.000000,0.000000,-10.000000)
p.stepSimulation()
p.disconnect()
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.stl
BIN
View File
Binary file not shown.
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xavier.urdf
-29
View File
@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="xavier">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.64"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.045 -0.045 -0.018"/>
<geometry>
<mesh filename="xavier.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.092 0.105 0.045"/>
</geometry>
</collision>
</link>
</robot>
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.stl
BIN
View File
Binary file not shown.
examples/pybullet/gym/pybullet_data/quadruped/microtaur/xm430w210.urdf
-29
View File
@@ -1,29 +0,0 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.082"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0.011 0"/>
<geometry>
<mesh filename="xm430w210.stl" scale="1.0 1.0 1.0"/>
</geometry>
<material name="black">
<color rgba=".3 .3 .3 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.024 0.047 0.034"/>
</geometry>
</collision>
</link>
</robot>
examples/pybullet/gym/pybullet_envs/deep_mimic/learning/nets/__init__.py
+1 -1
View File
@@ -1 +1 @@
from . import *
from . import *
examples/pybullet/gym/pybullet_envs/examples/laikago.py
+114 -114
View File
@@ -1,114 +1,114 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
#print (currentline)
#print("-----")
frame = currentline[0]
t = currentline[1]
#print("frame[",frame,"]")
joints = currentline[2:14]
#print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos + jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
#print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
#print("num points=",len(pts))
#for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[j]) / jointDirections[j]))
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos + jointOffsets[i],
force=maxForce)
examples/pybullet/gym/pybullet_envs/examples/microtaur.py
-467
View File
@@ -1,467 +0,0 @@
import pybullet as p
import pybullet_data as pd
import time
import math
def drawInertiaBox(parentUid, parentLinkIndex, color):
return
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
mass = dyn[0]
frictionCoeff = dyn[1]
inertia = dyn[2]
if (mass > 0):
Ixx = inertia[0]
Iyy = inertia[1]
Izz = inertia[2]
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
[-halfExtents[0], halfExtents[1], halfExtents[2]],
[halfExtents[0], -halfExtents[1], halfExtents[2]],
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
[halfExtents[0], halfExtents[1], -halfExtents[2]],
[-halfExtents[0], halfExtents[1], -halfExtents[2]],
[halfExtents[0], -halfExtents[1], -halfExtents[2]],
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
p.addUserDebugLine(pts[0],
pts[1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[0],
pts[4],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[5],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[6],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[7],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 0],
pts[4 + 1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 1],
pts[4 + 3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 3],
pts[4 + 2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 2],
pts[4 + 0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
toeConstraint = True
useMaximalCoordinates = False
useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1. / 100
numSolverIterations = 50
if (useMaximalCoordinates):
fixedTimeStep = 1. / 500
numSolverIterations = 200
speed = 10
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
p.setAdditionalSearchPath(pd.getDataPath())
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/microtaur/microtaur.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_joint = jointNameToId['knee_front_rightL_joint']
hip_front_rightR_joint = jointNameToId['hip_front_rightR_joint']
knee_front_rightR_joint = jointNameToId['knee_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_joint = jointNameToId['hip_front_leftR_joint']
knee_front_leftR_joint = jointNameToId['knee_front_leftR_joint']
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
knee_front_leftL_joint = jointNameToId['knee_front_leftL_joint']
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']
knee_back_rightR_joint = jointNameToId['knee_back_rightR_joint']
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
motor_back_rightL_joint = jointNameToId['motor_back_rightL_joint']
knee_back_rightL_joint = jointNameToId['knee_back_rightL_joint']
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
knee_back_leftR_joint = jointNameToId['knee_back_leftR_joint']
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
knee_back_leftL_joint = jointNameToId['knee_back_leftL_joint']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
motordir[2] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
motordir[3] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
motordir[4] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
motordir[5] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
motordir[6] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
motordir[7] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftR_joint, p.POSITION_CONTROL,
motordir[1] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftL_joint, p.POSITION_CONTROL,
motordir[2] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftR_joint, p.POSITION_CONTROL,
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightL_joint, p.POSITION_CONTROL,
motordir[4] * (kneeangle) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightR_joint, p.POSITION_CONTROL,
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightL_joint, p.POSITION_CONTROL,
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightR_joint, p.POSITION_CONTROL,
motordir[7] * kneeangle * float(aa) / steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
p.resetJointState(quadruped, knee_front_leftL_joint, motordir[0] * kneeangle)
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
p.resetJointState(quadruped, knee_front_leftR_joint, motordir[1] * kneeangle)
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
p.resetJointState(quadruped, knee_back_leftL_joint, motordir[2] * kneeangle)
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
p.resetJointState(quadruped, knee_back_leftR_joint, motordir[3] * kneeangle)
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped, knee_front_rightL_joint, motordir[4] * kneeangle)
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped, knee_front_rightR_joint, motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped, knee_back_rightL_joint, motordir[6] * kneeangle)
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped, knee_back_rightR_joint, motordir[7] * kneeangle)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped, knee_front_leftR_joint, quadruped, knee_front_leftL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_front_rightR_joint, quadruped, knee_front_rightL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_leftR_joint, quadruped, knee_back_leftL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_rightR_joint, quadruped, knee_back_rightL_joint,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
if (1):
p.setJointMotorControl(quadruped, knee_front_leftL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_leftR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_joint, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightL_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightR_joint, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setGravity(0, 0, -10)
legnumbering = [
motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
motor_back_rightL_joint, motor_back_rightR_joint
]
for i in range(8):
print(legnumbering[i])
#use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
t = 0.0
t_end = t + 5
ref_time = time.time()
while (t < t_end):
p.setGravity(0, 0, -10)
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (useRealTime == 0):
p.stepSimulation()
time.sleep(fixedTimeStep)
print("quadruped Id = ")
print(quadruped)
p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
#jump
t = 0.0
t_end = t + 100
i = 0
ref_time = time.time()
while (1):
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (True):
target = math.sin(t * speed) * jump_amp + 1.57
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
if (useRealTime == 0):
p.stepSimulation()
time.sleep(fixedTimeStep)
examples/pybullet/gym/pybullet_envs/examples/mini_cheetah_test.py
+23
View File
@@ -0,0 +1,23 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)
p.setGravity(0,0,-9.8)
p.setAdditionalSearchPath(pd.getDataPath())
floor = p.loadURDF("plane.urdf")
startPos = [0,0,0.5]
robot = p.loadURDF("mini_cheetah/mini_cheetah.urdf",startPos)
numJoints = p.getNumJoints(robot)
p.changeVisualShape(robot,-1,rgbaColor=[1,1,1,1])
for j in range (numJoints):
p.changeVisualShape(robot,j,rgbaColor=[1,1,1,1])
force=200
pos=0
p.setJointMotorControl2(robot,j,p.POSITION_CONTROL,pos,force=force)
dt = 1./240.
p.setTimeStep(dt)
while (1):
p.stepSimulation()
time.sleep(dt)