Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
bla
@@ -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)
|
||||
|
||||
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
10259
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_abad.obj
Normal file
File diff suppressed because it is too large
Load Diff
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
37168
examples/pybullet/gym/pybullet_data/mini_cheetah/meshes/mini_body.obj
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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>
|
||||
Binary file not shown.
@@ -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>
|
||||
|
||||
Binary file not shown.
@@ -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>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -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>
|
||||
|
||||
@@ -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()
|
||||
Binary file not shown.
@@ -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>
|
||||
|
||||
Binary file not shown.
@@ -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>
|
||||
|
||||
@@ -1 +1 @@
|
||||
from . import *
|
||||
from . import *
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user