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

add example robots in pybullet_robots module

Browse Source
This commit is contained in:
Erwin Coumans
2020-01-12 07:11:57 -08:00
parent 3ca233193f
commit fb15aea414
4 changed files with 0 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
examples/pybullet/gym/pybullet_robots/laikago/laikago.py Normal file
View File

@@ -0,0 +1,116 @@
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_toes.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.)
index = 0
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[index]) / jointDirections[index]))
index = index+1
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)

43
examples/pybullet/gym/pybullet_robots/xarm/xarm.py Normal file
View File

@@ -0,0 +1,43 @@
import pybullet as p
import pybullet_data as pd
import time
p.connect(p.GUI)#, options="--background_color_red=1.0 --background_color_blue=1.0 --background_color_green=1.0")
p.setAdditionalSearchPath(pd.getDataPath())
useFixedBase = True
flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION
#plane_pos = [0,0,0]
#plane = p.loadURDF("plane.urdf", plane_pos, flags = flags, useFixedBase=useFixedBase)
table_pos = [0,0,-0.625]
table = p.loadURDF("table/table.urdf", table_pos, flags = flags, useFixedBase=useFixedBase)
xarm = p.loadURDF("xarm/xarm6_robot.urdf", flags = flags, useFixedBase=useFixedBase)
jointIds = []
paramIds = []
for j in range(p.getNumJoints(xarm)):
p.changeDynamics(xarm, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(xarm, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
skip_cam_frames = 10
while (1):
p.stepSimulation()
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
skip_cam_frames -= 1
if (skip_cam_frames<0):
p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL )
skip_cam_frames = 10
time.sleep(1./240.)

123
examples/pybullet/gym/pybullet_robots/xarm/xarm_sim.py Normal file
View File

@@ -0,0 +1,123 @@
import time
import numpy as np
import math
useNullSpace = 0
useDynamics = 1
useIKFast = 0 #ikfast doesn't get solutions and is actually slower than pybullet IK (300us versus 150us), probably a configuration issue
if useIKFast:
import ikfastpy
ikSolver = 0
xarmEndEffectorIndex = 6
xarmNumDofs = 6
ll = [-17]*xarmNumDofs
#upper limits for null space (todo: set them to proper range)
ul = [17]*xarmNumDofs
#joint ranges for null space (todo: set them to proper range)
jr = [17]*xarmNumDofs
#restposes for null space
jointPositions=[0,0,0,0,0,0]
rp = jointPositions
jointPoses = jointPositions
class XArm6Sim(object):
def __init__(self, bullet_client, offset):
if useIKFast:
# Initialize kinematics for UR5 robot arm
self.xarm_kin = ikfastpy.PyKinematics()
self.n_joints = self.xarm_kin.getDOF()
print("numJoints IKFast=", self.n_joints)
self.bullet_client = bullet_client
self.offset = np.array(offset)
self.jointPoses = [0]*xarmNumDofs
#print("offset=",offset)
flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES
legos=[]
self.bullet_client.loadURDF("tray/traybox.urdf", [0.4+offset[0], offset[1], offset[2]], [0,0,0,1], flags=flags)
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, 0.1, 0.3])+self.offset, flags=flags))
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.3, -0.1, 0.3])+self.offset, flags=flags))
legos.append(self.bullet_client.loadURDF("lego/lego.urdf",np.array([0.5, 0.1, 0.3])+self.offset, flags=flags))
sphereId = self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.4, 0, 0.3])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.3, 0, 0.3])+self.offset, flags=flags)
self.bullet_client.loadURDF("sphere_small.urdf",np.array( [0.5, 0, 0.3])+self.offset, flags=flags)
orn=[0,0,0,1]
self.xarm = self.bullet_client.loadURDF("xarm/xarm6_robot.urdf", np.array([0,0,0])+self.offset, orn, useFixedBase=True, flags=flags)
index = 0
for j in range(self.bullet_client.getNumJoints(self.xarm)):
self.bullet_client.changeDynamics(self.xarm, j, linearDamping=0, angularDamping=0)
info = self.bullet_client.getJointInfo(self.xarm, j)
jointName = info[1]
jointType = info[2]
if (jointType == self.bullet_client.JOINT_PRISMATIC):
self.bullet_client.resetJointState(self.xarm, j, jointPositions[index])
index=index+1
if (jointType == self.bullet_client.JOINT_REVOLUTE):
self.bullet_client.resetJointState(self.xarm, j, jointPositions[index])
index=index+1
self.t = 0.
def reset(self):
pass
def step(self):
t = self.t
self.t += 1./60.
pos = [0.407+self.offset[0]+0.2 * math.sin(1.5 * t), 0.0+self.offset[1]+0.2 * math.cos(1.5 * t), 0.12+self.offset[2]]
#orn = self.bullet_client.getQuaternionFromEuler([math.pi/2.,0.,0.])
orn = [1,0,0,0]
if useIKFast:
#mat = self.bullet_client.getMatrixFromQuaternion(orn)
#ee_pose = np.array([mat[0],mat[1],mat[2],pos[0],
# mat[3],mat[4],mat[5],pos[1],
# mat[6],mat[7],mat[8],pos[2]])
#ee_pose = np.asarray(ee_pose).reshape(3,4)
joint_angles = [0.39,0,0,0,0,0]#-3.1,-1.6,1.6,-1.6,-1.6,0.] # in radians
self.bullet_client.submitProfileTiming("ikfast.forwardDynamics")
ee_pose = self.xarm_kin.forward(joint_angles)
self.bullet_client.submitProfileTiming()
ee_pose = np.asarray(ee_pose).reshape(3,4)
#print("ee_pose=",ee_pose)
self.bullet_client.submitProfileTiming("ikfast.inverseDynamics")
joint_configs = self.xarm_kin.inverse(ee_pose.reshape(-1).tolist())
self.bullet_client.submitProfileTiming()
n_solutions = int(len(joint_configs)/self.n_joints)
print("%d solutions found:"%(n_solutions))
joint_configs = np.asarray(joint_configs).reshape(n_solutions,self.n_joints)
for joint_config in joint_configs:
print(joint_config)
jointPoses = [0]*self.n_joints
else:
if useNullSpace:
jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn,lowerLimits=ll,
upperLimits=ul,jointRanges=jr, restPoses=np.array(self.jointPoses).tolist(),residualThreshold=1e-5, maxNumIterations=50)
self.jointPoses = [0,0,jointPoses[2],jointPoses[3],jointPoses[4],jointPoses[5]]
else:
jointPoses = self.bullet_client.calculateInverseKinematics(self.xarm,xarmEndEffectorIndex, pos, orn, maxNumIterations=50)
#print("jointPoses=",jointPoses)
if useDynamics:
for i in range(xarmNumDofs):
self.bullet_client.setJointMotorControl2(self.xarm, i+1, self.bullet_client.POSITION_CONTROL, jointPoses[i],force=5 * 240.)
else:
for i in range(xarmNumDofs):
self.bullet_client.resetJointState(self.xarm, i+1, jointPoses[i])
ls = self.bullet_client.getLinkState(self.xarm, xarmEndEffectorIndex, computeForwardKinematics=True)
linkComPos=np.array(ls[0])
linkComOrn=ls[1]
linkUrdfPos=np.array(ls[4])
linkUrdfOrn=ls[5]
#print("linkComPos=",linkComPos)
#print("linkUrdfOrn=",linkUrdfOrn)
mat = self.bullet_client.getMatrixFromQuaternion(linkUrdfOrn)
#print("mat=",mat)
self.bullet_client.addUserDebugLine(pos, linkUrdfPos, [1,0,0,1],lifeTime=100)
diff = linkUrdfPos-np.array(pos)
#print("diff=",diff)