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_envs/examples/laikago.py
View File

@@ -1,116 +0,0 @@
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)

139
examples/pybullet/gym/pybullet_envs/examples/testBike.py
View File

@@ -1,139 +0,0 @@
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)
import pybullet as p
import math
import time
import pybullet_data
p.connect(p.GUI)
#p.loadURDF("wheel.urdf",[0,0,3])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
plane = p.loadURDF("plane100.urdf", [0, 0, 0])
timestep = 1. / 240.
bike = -1
for i in range(1):
bike = p.loadURDF("bicycle/bike.urdf", [0, 0 + 3 * i, 1.5], [0, 0, 0, 1], useFixedBase=False)
p.setJointMotorControl2(bike, 0, p.VELOCITY_CONTROL, targetVelocity=0, force=0.05)
#p.setJointMotorControl2(bike,1,p.VELOCITY_CONTROL,targetVelocity=5, force=1000)
p.setJointMotorControl2(bike, 1, p.VELOCITY_CONTROL, targetVelocity=5, force=0)
p.setJointMotorControl2(bike, 2, p.VELOCITY_CONTROL, targetVelocity=15, force=20)
p.changeDynamics(plane, -1, mass=0, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 1, lateralFriction=1, linearDamping=0, angularDamping=0)
p.changeDynamics(bike, 2, lateralFriction=1, linearDamping=0, angularDamping=0)
#p.resetJointState(bike,1,0,100)
#p.resetJointState(bike,2,0,100)
#p.resetBaseVelocity(bike,[0,0,0],[0,0,0])
#p.setPhysicsEngineParameter(numSubSteps=0)
#bike=p.loadURDF("frame.urdf",useFixedBase=True)
#bike = p.loadURDF("handlebar.urdf", useFixedBase=True)
#p.loadURDF("handlebar.urdf",[0,2,1])
#coord = p.loadURDF("handlebar.urdf", [0.7700000000000005, 0, 0.22000000000000006],useFixedBase=True)# p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos = [0.07, 0, -0.6900000000000004]
coordOrnEuler = [0, 0, 0]
coordPos2 = [0, 0, 0]
coordOrnEuler2 = [0, 0, 0]
invPos, invOrn = p.invertTransform(coordPos, p.getQuaternionFromEuler(coordOrnEuler))
mPos, mOrn = p.multiplyTransforms(invPos, invOrn, coordPos2,
p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"", eul[0], eul[1], eul[2], "\" xyz=\"", mPos[0], mPos[1], mPos[2])
shift = 0
gui = 1
frame = 0
states = []
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
reverting = False
p.getCameraImage(320, 200) #,renderer=p.ER_BULLET_HARDWARE_OPENGL )
while (1):
updateCam = 0
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (reverting or (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
reverting = True
stateIndex = len(states) - 1
#print("prestateIndex=",stateIndex)
time.sleep(timestep)
updateCam = 1
if (stateIndex > 0):
stateIndex -= 1
states = states[:stateIndex + 1]
#observations=observations[:stateIndex+1]
#print("states=",states)
#print("stateIndex =",stateIndex )
p.restoreState(states[stateIndex])
#obs=observations[stateIndex]
#obs, r, done, _ = env.step(a)
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
reverting = False
if (k == ord('1') and (v & p.KEY_WAS_TRIGGERED)):
gui = not gui
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
running = False
if (running or (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED))):
running = True
if (running):
p.stepSimulation()
updateCam = 1
time.sleep(timestep)
pts = p.getContactPoints()
#print("numPoints=",len(pts))
#for point in pts:
# print("Point:bodyA=", point[1],"bodyB=",point[2],"linkA=",point[3],"linkB=",point[4],"dist=",point[8],"force=",point[9])
states.append(p.saveState())
#observations.append(obs)
stateIndex = len(states)
#print("stateIndex =",stateIndex )
frame += 1
if (updateCam):
distance = 5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(bike)
humanBaseVel = p.getBaseVelocity(bike)
#print("frame",frame, "humanPos=",humanPos, "humanVel=",humanBaseVel)
if (gui):
camInfo = p.getDebugVisualizerCamera()
curTargetPos = camInfo[11]
distance = camInfo[10]
yaw = camInfo[8]
pitch = camInfo[9]
targetPos = [
0.95 * curTargetPos[0] + 0.05 * humanPos[0], 0.95 * curTargetPos[1] + 0.05 * humanPos[1],
curTargetPos[2]
]
p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)

43
examples/pybullet/gym/pybullet_envs/examples/xarm.py
View File

@@ -1,43 +0,0 @@
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_envs/examples/xarm_sim.py
View File

@@ -1,123 +0,0 @@
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)