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

add yapf style and apply yapf to format all Python files

Browse Source 
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
2019-04-27 07:31:15 -07:00
parent c591735042
commit ef9570c315
347 changed files with 70304 additions and 22752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.style.yapf Normal file
View File

@@ -0,0 +1,5 @@
[style]
based_on_style = google
column_limit = 99
indent_width = 2

10
Extras/Serialize/HeaderGenerator/blenderGenerate.py
View File

@@ -1,6 +1,5 @@
import dump import dump
header = """/* Copyright (C) 2006 Charlie C header = """/* Copyright (C) 2006 Charlie C
* *
* This software is provided 'as-is', without any express or implied * This software is provided 'as-is', without any express or implied
@@ -27,14 +26,17 @@ dtList = dump.DataTypeList
out = "../BlenderSerialize/autogenerated/" out = "../BlenderSerialize/autogenerated/"
spaces = 4 spaces = 4
def addSpaces(file, space): def addSpaces(file, space):
for i in range(0, space): for i in range(0, space):
file.write(" ") file.write(" ")
def write(file, spaces, string): def write(file, spaces, string):
addSpaces(file, spaces) addSpaces(file, spaces)
file.write(string) file.write(string)
################################################################################### ###################################################################################
blender = open(out + "blender.h", 'w') blender = open(out + "blender.h", 'w')
blender.write(header) blender.write(header)
@@ -69,7 +71,6 @@ blenderC.write("}\n")
blenderC.write("#endif//__BLENDERCOMMON_H__") blenderC.write("#endif//__BLENDERCOMMON_H__")
blenderC.close() blenderC.close()
for dt in dtList: for dt in dtList:
fp = open(out + dt.filename + ".h", 'w') fp = open(out + dt.filename + ".h", 'w')
@@ -80,7 +81,6 @@ for dt in dtList:
fp.write("#define __%s__H__\n" % strUpper) fp.write("#define __%s__H__\n" % strUpper)
fp.write("\n\n") fp.write("\n\n")
fp.write("// -------------------------------------------------- //\n") fp.write("// -------------------------------------------------- //\n")
fp.write("#include \"blender_Common.h\"\n") fp.write("#include \"blender_Common.h\"\n")
@@ -93,7 +93,6 @@ for dt in dtList:
addSpaces(fp, 4) addSpaces(fp, 4)
fp.write("// ---------------------------------------------- //\n") fp.write("// ---------------------------------------------- //\n")
write(fp, 4, "class %s\n" % dt.name) write(fp, 4, "class %s\n" % dt.name)
write(fp, 4, "{\n") write(fp, 4, "{\n")
@@ -101,11 +100,8 @@ for dt in dtList:
for i in dt.dataTypes: for i in dt.dataTypes:
write(fp, 8, i + ";\n") write(fp, 8, i + ";\n")
write(fp, 4, "};\n") write(fp, 4, "};\n")
fp.write("}\n") fp.write("}\n")
fp.write("\n\n") fp.write("\n\n")
fp.write("#endif//__%s__H__\n" % strUpper) fp.write("#endif//__%s__H__\n" % strUpper)
fp.close() fp.close()

7
Extras/Serialize/HeaderGenerator/bulletGenerate.py
View File

@@ -2,8 +2,6 @@ import sys
sys.path.append(".") sys.path.append(".")
import dump import dump
header = """/* Copyright (C) 2011 Erwin Coumans & Charlie C header = """/* Copyright (C) 2011 Erwin Coumans & Charlie C
* *
* This software is provided 'as-is', without any express or implied * This software is provided 'as-is', without any express or implied
@@ -30,14 +28,17 @@ dtList = dump.DataTypeList
out = "autogenerated/" out = "autogenerated/"
spaces = 4 spaces = 4
def addSpaces(file, space): def addSpaces(file, space):
for i in range(0, space): for i in range(0, space):
file.write(" ") file.write(" ")
def write(file, spaces, string): def write(file, spaces, string):
addSpaces(file, spaces) addSpaces(file, spaces)
file.write(string) file.write(string)
################################################################################### ###################################################################################
blender = open(out + "bullet.h", 'w') blender = open(out + "bullet.h", 'w')
blender.write(header) blender.write(header)
@@ -46,7 +47,6 @@ blender.write("#define __BULLET_H__\n")
#for dt in dtList: #for dt in dtList:
# blender.write("struct %s;\n"%dt.filename) # blender.write("struct %s;\n"%dt.filename)
################################################################################### ###################################################################################
blender.write("namespace Bullet {\n") blender.write("namespace Bullet {\n")
@@ -63,7 +63,6 @@ blender.write(strUnRes)
for dt in dtList: for dt in dtList:
write(blender, 4, "class %s;\n" % dt.name) write(blender, 4, "class %s;\n" % dt.name)
for dt in dtList: for dt in dtList:
strUpper = dt.filename.upper() strUpper = dt.filename.upper()

97
data/dinnerware/generate.py
View File

@@ -1,7 +1,9 @@
from __future__ import print_function from __future__ import print_function
import numpy as np import numpy as np
class Obj: class Obj:
def __init__(self, fn): def __init__(self, fn):
self.ind_v = 0 self.ind_v = 0
self.ind_vt = 0 self.ind_vt = 0
@@ -9,18 +11,22 @@ class Obj:
self.fn = fn self.fn = fn
self.out = open(fn + ".tmp", "w") self.out = open(fn + ".tmp", "w")
self.out.write("mtllib dinnerware.mtl\n") self.out.write("mtllib dinnerware.mtl\n")
def __del__(self): def __del__(self):
self.out.close() self.out.close()
import shutil import shutil
shutil.move(self.fn + ".tmp", self.fn) shutil.move(self.fn + ".tmp", self.fn)
def push_v(self, v): def push_v(self, v):
self.out.write("v %f %f %f\n" % (v[0], v[1], v[2])) self.out.write("v %f %f %f\n" % (v[0], v[1], v[2]))
self.ind_v += 1 self.ind_v += 1
return self.ind_v return self.ind_v
def push_vt(self, vt): def push_vt(self, vt):
self.out.write("vt %f %f\n" % (vt[0], vt[1])) self.out.write("vt %f %f\n" % (vt[0], vt[1]))
self.ind_vt += 1 self.ind_vt += 1
return self.ind_vt return self.ind_vt
def push_vn(self, vn): def push_vn(self, vn):
vn /= np.linalg.norm(vn) vn /= np.linalg.norm(vn)
self.out.write("vn %f %f %f\n" % (vn[0], vn[1], vn[2])) self.out.write("vn %f %f %f\n" % (vn[0], vn[1], vn[2]))
@@ -43,15 +49,16 @@ def convex_hull(points, vind, nind, tind, obj):
pos = (inner <= C + 0.0001).all() pos = (inner <= C + 0.0001).all()
neg = (inner >= C - 0.0001).all() neg = (inner >= C - 0.0001).all()
if not pos and not neg: continue if not pos and not neg: continue
obj.out.write("f %i//%i %i//%i %i//%i\n" % ( obj.out.write("f %i//%i %i//%i %i//%i\n" %
(vind[a], nind[a], vind[b], nind[b], vind[c], nind[c]) ((vind[a], nind[a], vind[b], nind[b], vind[c], nind[c]) if
if (inner - C).sum() < 0 else (inner - C).sum() < 0 else
(vind[a], nind[a], vind[c], nind[c], vind[b], nind[b]))) (vind[a], nind[a], vind[c], nind[c], vind[b], nind[b])))
#obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( #obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
# (vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c]) # (vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c])
# if (inner - C).sum() < 0 else # if (inner - C).sum() < 0 else
# (vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) ) # (vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) )
def test_convex_hull(): def test_convex_hull():
obj = Obj("convex_test.obj") obj = Obj("convex_test.obj")
vlist = np.random.uniform(low=-0.1, high=+0.1, size=(100, 3)) vlist = np.random.uniform(low=-0.1, high=+0.1, size=(100, 3))
@@ -62,7 +69,9 @@ def test_convex_hull():
tind = [obj.push_vt(uv) for uv in tlist] tind = [obj.push_vt(uv) for uv in tlist]
convex_hull(vlist, vind, nind, tind, obj) convex_hull(vlist, vind, nind, tind, obj)
class Contour: class Contour:
def __init__(self): def __init__(self):
self.vprev_vind = None self.vprev_vind = None
@@ -71,14 +80,26 @@ class Contour:
for i1 in range(cnt): for i1 in range(cnt):
i2 = i1 - 1 i2 = i1 - 1
obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
vlist_vind[i2], vlist_tind[i2], vlist_nind[i2], vlist_vind[i2],
vlist_vind[i1], vlist_tind[i1], vlist_nind[i1], vlist_tind[i2],
self.vprev_vind[i1], self.vprev_tind[i1], self.vprev_nind[i1], vlist_nind[i2],
vlist_vind[i1],
vlist_tind[i1],
vlist_nind[i1],
self.vprev_vind[i1],
self.vprev_tind[i1],
self.vprev_nind[i1],
)) ))
obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
vlist_vind[i2], vlist_tind[i2], vlist_nind[i2], vlist_vind[i2],
self.vprev_vind[i1], self.vprev_tind[i1], self.vprev_nind[i1], vlist_tind[i2],
self.vprev_vind[i2], self.vprev_tind[i2], self.vprev_nind[i2], vlist_nind[i2],
self.vprev_vind[i1],
self.vprev_tind[i1],
self.vprev_nind[i1],
self.vprev_vind[i2],
self.vprev_tind[i2],
self.vprev_nind[i2],
)) ))
def belt(self, obj, vlist, nlist, tlist): def belt(self, obj, vlist, nlist, tlist):
@@ -98,6 +119,7 @@ class Contour:
def finish(self, obj): def finish(self, obj):
self.f(obj, self.first_vind, self.first_tind, self.first_nind) self.f(obj, self.first_vind, self.first_tind, self.first_nind)
def test_contour(): def test_contour():
RAD1 = 2.0 RAD1 = 2.0
RAD2 = 1.5 RAD2 = 1.5
@@ -113,24 +135,24 @@ def test_contour():
beta = b / 50.0 * 2 * np.pi beta = b / 50.0 * 2 * np.pi
r = RAD2 * np.cos(beta) + RAD1 r = RAD2 * np.cos(beta) + RAD1
z = RAD2 * np.sin(beta) z = RAD2 * np.sin(beta)
belt_v.append( np.array( [ belt_v.append(np.array([np.cos(angle) * r, np.sin(angle) * r, z]))
np.cos(angle)*r, belt_n.append(
np.sin(angle)*r, np.array([np.cos(angle) * np.cos(beta),
z] ) )
belt_n.append( np.array( [
np.cos(angle)*np.cos(beta),
np.sin(angle) * np.cos(beta), np.sin(angle) * np.cos(beta),
np.sin(beta)])) np.sin(beta)]))
belt_t.append((0, 0)) belt_t.append((0, 0))
contour.belt(obj, belt_v, belt_n, belt_t) contour.belt(obj, belt_v, belt_n, belt_t)
contour.finish(obj) contour.finish(obj)
#test_convex_hull() #test_convex_hull()
#test_contour() #test_contour()
class RotationFigureParams: class RotationFigureParams:
pass pass
def generate_plate(p, obj, collision_prefix): def generate_plate(p, obj, collision_prefix):
contour = Contour() contour = Contour()
belt_vlist_3d_prev = None belt_vlist_3d_prev = None
@@ -141,11 +163,7 @@ def generate_plate(p, obj, collision_prefix):
if step % p.COLLISION_EVERY == 0: if step % p.COLLISION_EVERY == 0:
vlist_3d = [] vlist_3d = []
for x, y in p.belt_simple: for x, y in p.belt_simple:
vlist_3d.append( [ vlist_3d.append([np.cos(angle) * x * 1.06, np.sin(angle) * x * 1.06, y])
np.cos(angle)*x*1.06,
np.sin(angle)*x*1.06,
y
] )
if belt_vlist_3d_prev: if belt_vlist_3d_prev:
obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY)) obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY))
obj2.out.write("usemtl pan_tefal\n") obj2.out.write("usemtl pan_tefal\n")
@@ -165,28 +183,19 @@ def generate_plate(p, obj, collision_prefix):
belt_n = [] belt_n = []
belt_t = [] belt_t = []
for x, y, nx, ny in p.belt: for x, y, nx, ny in p.belt:
belt_v.append( np.array( [ belt_v.append(np.array([np.cos(angle) * x, np.sin(angle) * x, y]))
np.cos(angle)*x, belt_n.append(np.array([np.cos(angle) * nx, np.sin(angle) * nx, ny]))
np.sin(angle)*x,
y
] ) )
belt_n.append( np.array( [
np.cos(angle)*nx,
np.sin(angle)*nx,
ny
] ) )
if ny - nx >= 0: if ny - nx >= 0:
belt_t.append( ( belt_t.append((127.0 / 512 + np.cos(angle) * x / p.RAD_HIGH * 105 / 512,
127.0/512 + np.cos(angle)*x/p.RAD_HIGH*105/512,
(512 - 135.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 105 / 512)) (512 - 135.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 105 / 512))
else: else:
belt_t.append( ( belt_t.append((382.0 / 512 + np.cos(angle) * x / p.RAD_HIGH * 125 / 512,
382.0/512 + np.cos(angle)*x/p.RAD_HIGH*125/512,
(512 - 380.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 125 / 512)) (512 - 380.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 125 / 512))
contour.belt(obj, belt_v, belt_n, belt_t) contour.belt(obj, belt_v, belt_n, belt_t)
contour.finish(obj) contour.finish(obj)
def tefal(): def tefal():
p = RotationFigureParams() p = RotationFigureParams()
p.RAD_LOW = 0.240 / 2 p.RAD_LOW = 0.240 / 2
@@ -207,16 +216,13 @@ def tefal():
(p.RAD_LOW - p.THICK, 3 * p.THICK, -1, 0), (p.RAD_LOW - p.THICK, 3 * p.THICK, -1, 0),
] ]
p.belt.reverse() p.belt.reverse()
p.belt_simple = [ p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
(p.RAD_HIGH-p.THICK, p.H), (p.RAD_LOW - p.THICK, 0)]
(p.RAD_HIGH+p.THICK, p.H),
(p.RAD_LOW , 0),
(p.RAD_LOW-p.THICK , 0)
]
obj = Obj("pan_tefal.obj") obj = Obj("pan_tefal.obj")
obj.out.write("usemtl pan_tefal\n") obj.out.write("usemtl pan_tefal\n")
generate_plate(p, obj, "pan_tefal-collision%02i.obj") generate_plate(p, obj, "pan_tefal-collision%02i.obj")
def plate(): def plate():
p = RotationFigureParams() p = RotationFigureParams()
p.RAD_LOW = 0.110 / 2 p.RAD_LOW = 0.110 / 2
@@ -237,16 +243,11 @@ def plate():
(p.RAD_LOW - p.THICK, 3 * p.THICK, -0.5, 1.0), (p.RAD_LOW - p.THICK, 3 * p.THICK, -0.5, 1.0),
] ]
p.belt.reverse() p.belt.reverse()
p.belt_simple = [ p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
(p.RAD_HIGH-p.THICK, p.H), (p.RAD_LOW - p.THICK, 0)]
(p.RAD_HIGH+p.THICK, p.H),
(p.RAD_LOW , 0),
(p.RAD_LOW-p.THICK , 0)
]
obj = Obj("plate.obj") obj = Obj("plate.obj")
obj.out.write("usemtl solid_color\n") obj.out.write("usemtl solid_color\n")
generate_plate(p, obj, "plate-collision%02i.obj") generate_plate(p, obj, "plate-collision%02i.obj")
plate() plate()

231
data/quadruped/vision/vision.py
View File

@@ -14,14 +14,11 @@ maxMotorForce = 5000
maxGearForce = 10000 maxGearForce = 10000
jointFriction = 0.1 jointFriction = 0.1
p.connect(p.GUI) p.connect(p.GUI)
amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5) amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5)
timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1) timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1)
jointTypeNames = {} jointTypeNames = {}
jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE" jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED] = "JOINT_FIXED" jointTypeNames[p.JOINT_FIXED] = "JOINT_FIXED"
@@ -31,15 +28,12 @@ p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
jointNamesToIndex = {} jointNamesToIndex = {}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False) vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
for j in range(p.getNumJoints(vision)): for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision, j) jointInfo = p.getJointInfo(vision, j)
jointInfoName = jointInfo[1].decode("utf-8") jointInfoName = jointInfo[1].decode("utf-8")
@@ -48,7 +42,6 @@ for j in range(p.getNumJoints(vision)):
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName]) #print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction) p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center'] chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint'] motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint'] motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
@@ -78,7 +71,6 @@ motC_rf_Id= p.addUserDebugParameter("motC_rf",-limitVal,limitVal,legposSright)
erp_rf_Id = p.addUserDebugParameter("erp_rf", 0, 1, defaultERP) erp_rf_Id = p.addUserDebugParameter("erp_rf", 0, 1, defaultERP)
relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0) relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0)
motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos) motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos)
motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos) motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos)
motC_lf_Id = p.addUserDebugParameter("motC_lf", -limitVal, limitVal, legposSleft) motC_lf_Id = p.addUserDebugParameter("motC_lf", -limitVal, limitVal, legposSleft)
@@ -93,7 +85,6 @@ motC_rb_Id= p.addUserDebugParameter("motC_rb",-limitVal,limitVal,legposSright)
erp_rb_Id = p.addUserDebugParameter("erp_rb", 0, 1, defaultERP) erp_rb_Id = p.addUserDebugParameter("erp_rb", 0, 1, defaultERP)
relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0) relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0)
motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos) motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos)
motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos) motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos)
motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft) motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft)
@@ -107,25 +98,46 @@ camYaw = -2
camPitch = -16 camPitch = -16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos) p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,
knee_front_rightR_joint,
vision,
motor_front_rightL_joint,
c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_rf, gearRatio=-1, gearAuxLink=motor_front_rightR_joint, maxForce=maxGearForce) p.changeConstraint(c_rf, gearRatio=-1, gearAuxLink=motor_front_rightR_joint, maxForce=maxGearForce)
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c_lf = p.createConstraint(vision,
knee_front_leftL_joint,
vision,
motor_front_leftR_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_lf, gearRatio=-1, gearAuxLink=motor_front_leftL_joint, maxForce=maxGearForce) p.changeConstraint(c_lf, gearRatio=-1, gearAuxLink=motor_front_leftL_joint, maxForce=maxGearForce)
c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c_rb = p.createConstraint(vision,
knee_back_rightR_joint,
vision,
motor_back_rightL_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_rb, gearRatio=-1, gearAuxLink=motor_back_rightR_joint, maxForce=maxGearForce) p.changeConstraint(c_rb, gearRatio=-1, gearAuxLink=motor_back_rightR_joint, maxForce=maxGearForce)
c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c_lb = p.createConstraint(vision,
knee_back_leftL_joint,
vision,
motor_back_leftR_joint,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c_lb, gearRatio=-1, gearAuxLink=motor_back_leftL_joint, maxForce=maxGearForce) p.changeConstraint(c_lb, gearRatio=-1, gearAuxLink=motor_back_leftL_joint, maxForce=maxGearForce)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
for i in range(1): for i in range(1):
#while (1): #while (1):
@@ -135,43 +147,108 @@ for i in range (1):
erp_rf = p.readUserDebugParameter(erp_rf_Id) erp_rf = p.readUserDebugParameter(erp_rf_Id)
relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id) relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
#motC_rf #motC_rf
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce) motor_front_rightR_joint,
p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce) targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.changeConstraint(c_rf,
gearRatio=-1,
gearAuxLink=motor_front_rightR_joint,
erp=erp_rf,
relativePositionTarget=relPosTarget_rf,
maxForce=maxGearForce)
motA_lf = p.readUserDebugParameter(motA_lf_Id) motA_lf = p.readUserDebugParameter(motA_lf_Id)
motB_lf = p.readUserDebugParameter(motB_lf_Id) motB_lf = p.readUserDebugParameter(motB_lf_Id)
motC_lf = p.readUserDebugParameter(motC_lf_Id) motC_lf = p.readUserDebugParameter(motC_lf_Id)
erp_lf = p.readUserDebugParameter(erp_lf_Id) erp_lf = p.readUserDebugParameter(erp_lf_Id)
relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id) relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce) motor_front_leftL_joint,
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,erp=erp_lf, relativePositionTarget=relPosTarget_lf,maxForce=maxGearForce) targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.changeConstraint(c_lf,
gearRatio=-1,
gearAuxLink=motor_front_leftL_joint,
erp=erp_lf,
relativePositionTarget=relPosTarget_lf,
maxForce=maxGearForce)
motA_rb = p.readUserDebugParameter(motA_rb_Id) motA_rb = p.readUserDebugParameter(motA_rb_Id)
motB_rb = p.readUserDebugParameter(motB_rb_Id) motB_rb = p.readUserDebugParameter(motB_rb_Id)
motC_rb = p.readUserDebugParameter(motC_rb_Id) motC_rb = p.readUserDebugParameter(motC_rb_Id)
erp_rb = p.readUserDebugParameter(erp_rb_Id) erp_rb = p.readUserDebugParameter(erp_rb_Id)
relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id) relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce) motor_back_rightR_joint,
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,erp=erp_rb, relativePositionTarget=relPosTarget_rb,maxForce=maxGearForce) targetPosition=motA_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rb,
force=maxMotorForce)
p.changeConstraint(c_rb,
gearRatio=-1,
gearAuxLink=motor_back_rightR_joint,
erp=erp_rb,
relativePositionTarget=relPosTarget_rb,
maxForce=maxGearForce)
motA_lb = p.readUserDebugParameter(motA_lb_Id) motA_lb = p.readUserDebugParameter(motA_lb_Id)
motB_lb = p.readUserDebugParameter(motB_lb_Id) motB_lb = p.readUserDebugParameter(motB_lb_Id)
motC_lb = p.readUserDebugParameter(motC_lb_Id) motC_lb = p.readUserDebugParameter(motC_lb_Id)
erp_lb = p.readUserDebugParameter(erp_lb_Id) erp_lb = p.readUserDebugParameter(erp_lb_Id)
relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id) relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce) motor_back_leftL_joint,
p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,erp=erp_lb, relativePositionTarget=relPosTarget_lb,maxForce=maxGearForce) targetPosition=motA_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lb,
force=maxMotorForce)
p.changeConstraint(c_lb,
gearRatio=-1,
gearAuxLink=motor_back_leftL_joint,
erp=erp_lb,
relativePositionTarget=relPosTarget_lb,
maxForce=maxGearForce)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
time.sleep(1. / 240.) time.sleep(1. / 240.)
@@ -196,25 +273,69 @@ while (1):
motB_lf = -(math.sin(t) * amp + legpos) motB_lf = -(math.sin(t) * amp + legpos)
motB_lb = -(math.sin(t) * amp + legpos) motB_lb = -(math.sin(t) * amp + legpos)
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce) motor_front_rightR_joint,
p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce) p.POSITION_CONTROL,
targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce) motor_back_rightR_joint,
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce) p.POSITION_CONTROL,
targetPosition=motA_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce) motor_back_leftL_joint,
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce) p.POSITION_CONTROL,
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce) targetPosition=motA_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce) motor_back_leftR_joint,
p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce) p.POSITION_CONTROL,
p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce) targetPosition=motB_lb,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_back_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lb,
force=maxMotorForce)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
time.sleep(1. / 240.) time.sleep(1. / 240.)

5
data/terrain.py
View File

@@ -16,7 +16,8 @@ for i in range (NUM_VERTS_X):
for j in range(NUM_VERTS_Y): for j in range(NUM_VERTS_Y):
gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 0] = (i - NUM_VERTS_X * 0.5) * TRIANGLE_SIZE gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 0] = (i - NUM_VERTS_X * 0.5) * TRIANGLE_SIZE
gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 1] = (j - NUM_VERTS_Y * 0.5) * TRIANGLE_SIZE gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 1] = (j - NUM_VERTS_Y * 0.5) * TRIANGLE_SIZE
gGroundVertices[(i+j*NUM_VERTS_X)*3+2] = waveheight*math.sin(float(i))*math.cos(float(j)+offset) gGroundVertices[(i + j * NUM_VERTS_X) * 3 +
2] = waveheight * math.sin(float(i)) * math.cos(float(j) + offset)
index = 0 index = 0
for i in range(NUM_VERTS_X - 1): for i in range(NUM_VERTS_X - 1):
@@ -55,5 +56,3 @@ for i in range (totalTriangles):
print(" "), print(" "),
print(gGroundIndices[i * 3 + 2]), print(gGroundIndices[i * 3 + 2]),
print(" ") print(" ")

1
data/threecubes/threecubes.py
View File

@@ -5,4 +5,3 @@ p.loadSDF("newsdf.sdf")
while (1): while (1):
p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK) p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation() p.stepSimulation()

18
data/threecubes/threecubes_org.py
View File

@@ -14,9 +14,19 @@ else:
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL) p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1): while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0] viewmat = [
projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] 0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
]
projmat = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
0.0, 0.0, -0.02000020071864128, 0.0
]
p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK ) p.getCameraImage(64,
64,
viewMatrix=viewmat,
projectionMatrix=projmat,
flags=p.ER_NO_SEGMENTATION_MASK)
p.stepSimulation() p.stepSimulation()

33
data/xacro_standalone.py
View File

@@ -93,11 +93,14 @@ def fixed_writexml(self, writer, indent="", addindent="", newl=""):
writer.write("%s</%s>%s" % (indent, self.tagName, newl)) writer.write("%s</%s>%s" % (indent, self.tagName, newl))
else: else:
writer.write("/>%s" % (newl)) writer.write("/>%s" % (newl))
# replace minidom's function with ours # replace minidom's function with ours
xml.dom.minidom.Element.writexml = fixed_writexml xml.dom.minidom.Element.writexml = fixed_writexml
class Table: class Table:
def __init__(self, parent=None): def __init__(self, parent=None):
self.parent = parent self.parent = parent
self.table = {} self.table = {}
@@ -120,6 +123,7 @@ class Table:
class QuickLexer(object): class QuickLexer(object):
def __init__(self, **res): def __init__(self, **res):
self.str = "" self.str = ""
self.top = None self.top = None
@@ -196,6 +200,7 @@ def child_nodes(elt):
yield c yield c
c = c.nextSibling c = c.nextSibling
all_includes = [] all_includes = []
# Deprecated message for <include> tags that don't have <xacro:include> prepended: # Deprecated message for <include> tags that don't have <xacro:include> prepended:
@@ -259,8 +264,8 @@ def process_includes(doc, base_dir):
included = parse(f) included = parse(f)
except Exception as e: except Exception as e:
raise XacroException( raise XacroException(
"included file \"%s\" generated an error during XML parsing: %s" "included file \"%s\" generated an error during XML parsing: %s" %
% (filename, str(e))) (filename, str(e)))
except IOError as e: except IOError as e:
raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e))) raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e)))
@@ -332,8 +337,8 @@ def grab_properties(doc):
break break
if has_bad: if has_bad:
sys.stderr.write('Property names may not have whitespace, ' + sys.stderr.write('Property names may not have whitespace, ' + '"{", "}", or "$" : "' +
'"{", "}", or "$" : "' + name + '"') name + '"')
else: else:
table[name] = value table[name] = value
@@ -453,6 +458,7 @@ def eval_expr(lex, symbols):
def eval_text(text, symbols): def eval_text(text, symbols):
def handle_expr(s): def handle_expr(s):
lex = QuickLexer(IGNORE=r"\s+", lex = QuickLexer(IGNORE=r"\s+",
NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?", NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?",
@@ -530,7 +536,8 @@ def eval_all(root, macros, symbols):
while block and block.nodeType != xml.dom.Node.ELEMENT_NODE: while block and block.nodeType != xml.dom.Node.ELEMENT_NODE:
block = block.nextSibling block = block.nextSibling
if not block: if not block:
raise XacroException("Not enough blocks while evaluating macro %s" % str(node.tagName)) raise XacroException("Not enough blocks while evaluating macro %s" %
str(node.tagName))
params.remove(param) params.remove(param)
scoped[param] = block scoped[param] = block
block = block.nextSibling block = block.nextSibling
@@ -590,7 +597,9 @@ def eval_all(root, macros, symbols):
elif value == 'false': keep = False elif value == 'false': keep = False
else: keep = float(value) else: keep = float(value)
except ValueError: except ValueError:
raise XacroException("Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]" % value) raise XacroException(
"Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]"
% value)
if node.tagName in ['unless', 'xacro:unless']: keep = not keep if node.tagName in ['unless', 'xacro:unless']: keep = not keep
if keep: if keep:
for e in list(child_nodes(node)): for e in list(child_nodes(node)):
@@ -630,12 +639,14 @@ def print_usage(exit_code=0):
def set_substitution_args_context(context={}): def set_substitution_args_context(context={}):
substitution_args_context['arg'] = context substitution_args_context['arg'] = context
def open_output(output_filename): def open_output(output_filename):
if output_filename is None: if output_filename is None:
return sys.stdout return sys.stdout
else: else:
return open(output_filename, 'w') return open(output_filename, 'w')
def main(): def main():
try: try:
opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes']) opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes'])
@@ -689,11 +700,14 @@ def main():
print() print()
else: else:
eval_self_contained(doc) eval_self_contained(doc)
banner = [xml.dom.minidom.Comment(c) for c in banner = [
[" %s " % ('=' * 83), xml.dom.minidom.Comment(c) for c in [
" %s " % ('=' * 83),
" | This document was autogenerated by xacro from %-30s | " % args[0], " | This document was autogenerated by xacro from %-30s | " % args[0],
" | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "", " | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "",
" %s " % ('=' * 83)]] " %s " % ('=' * 83)
]
]
first = doc.firstChild first = doc.firstChild
for comment in banner: for comment in banner:
doc.insertBefore(comment, first) doc.insertBefore(comment, first)
@@ -701,5 +715,6 @@ def main():
open_output(output_filename).write(doc.toprettyxml(indent=' ')) open_output(output_filename).write(doc.toprettyxml(indent=' '))
print() print()
if __name__ == '__main__': if __name__ == '__main__':
main() main()

61
docs/pybullet_quickstart_guide/PlainPreview/BuildMarkdeepUtility.py
View File

@@ -2,42 +2,43 @@ import re
if (__name__ == "__main__"): if (__name__ == "__main__"):
# Assemble the script which embeds the Markdeep page into the preview blog # Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8"); PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL); HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
HeadAttributes=HeadMatch.group(1); HeadAttributes = HeadMatch.group(1)
FullDocumentHead=HeadMatch.group(2); FullDocumentHead = HeadMatch.group(2)
BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL); BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
BodyAttributes=BodyMatch.group(1); BodyAttributes = BodyMatch.group(1)
FullPreviewBody=BodyMatch.group(2); FullPreviewBody = BodyMatch.group(2)
ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)"; ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro); iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro]; DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):]; DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8"); FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
ReplacementList=[ ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix), ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix) ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
];
for Macro, Replacement in ReplacementList: for Macro, Replacement in ReplacementList:
FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'")); FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
Macro,
Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
# Generate code which sets body and head attributes appropriately # Generate code which sets body and head attributes appropriately
for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]: for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n"; FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode): for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n"; FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8")); 1) + "\",\"" + Match.group(2) + "\");\r\n"
open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
# Concatenate all the scripts together # Concatenate all the scripts together
SourceFileList = [ SourceFileList = [
"PrepareHTML.full.js", "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
"SetMarkdeepMode.js",
"markdeep.min.js",
"DisplayMarkdeepOutput.js",
"InvokeMathJax.js" "InvokeMathJax.js"
]; ]
OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]); OutputCode = "\r\n\r\n".join([
OutputFile=open("MarkdeepUtility.js","wb"); "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
OutputFile.write(OutputCode.encode("utf-8")); for SourceFile in SourceFileList
OutputFile.close(); ])
print("Done."); OutputFile = open("MarkdeepUtility.js", "wb")
OutputFile.write(OutputCode.encode("utf-8"))
OutputFile.close()
print("Done.")

61
docs/pybullet_quickstart_guide/WordpressPreview/BuildMarkdeepUtility.py
View File

@@ -2,42 +2,43 @@ import re
if (__name__ == "__main__"): if (__name__ == "__main__"):
# Assemble the script which embeds the Markdeep page into the preview blog # Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8"); PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL); HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
HeadAttributes=HeadMatch.group(1); HeadAttributes = HeadMatch.group(1)
FullDocumentHead=HeadMatch.group(2); FullDocumentHead = HeadMatch.group(2)
BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL); BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
BodyAttributes=BodyMatch.group(1); BodyAttributes = BodyMatch.group(1)
FullPreviewBody=BodyMatch.group(2); FullPreviewBody = BodyMatch.group(2)
ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)"; ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro); iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro]; DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):]; DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8"); FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
ReplacementList=[ ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix), ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix) ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
];
for Macro, Replacement in ReplacementList: for Macro, Replacement in ReplacementList:
FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'")); FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
Macro,
Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
# Generate code which sets body and head attributes appropriately # Generate code which sets body and head attributes appropriately
for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]: for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n"; FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode): for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n"; FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8")); 1) + "\",\"" + Match.group(2) + "\");\r\n"
open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
# Concatenate all the scripts together # Concatenate all the scripts together
SourceFileList = [ SourceFileList = [
"PrepareHTML.full.js", "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
"SetMarkdeepMode.js",
"markdeep.min.js",
"DisplayMarkdeepOutput.js",
"InvokeMathJax.js" "InvokeMathJax.js"
]; ]
OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]); OutputCode = "\r\n\r\n".join([
OutputFile=open("MarkdeepUtility.js","wb"); "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
OutputFile.write(OutputCode.encode("utf-8")); for SourceFile in SourceFileList
OutputFile.close(); ])
print("Done."); OutputFile = open("MarkdeepUtility.js", "wb")
OutputFile.write(OutputCode.encode("utf-8"))
OutputFile.close()
print("Done.")

55
examples/SharedMemory/grpc/pybullet_client.py
View File

@@ -10,6 +10,7 @@ import pybullet_pb2_grpc
#todo: how to add this? #todo: how to add this?
MJCF_COLORS_FROM_FILE = 512 MJCF_COLORS_FROM_FILE = 512
def run(): def run():
print("grpc.insecure_channel") print("grpc.insecure_channel")
channel = grpc.insecure_channel('localhost:6667') channel = grpc.insecure_channel('localhost:6667')
@@ -18,61 +19,77 @@ def run():
response = 0 response = 0
print("submit CheckVersionCommand") print("submit CheckVersionCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(clientVersion=123))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(
clientVersion=123)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit_ResetSimulationCommand") print("submit_ResetSimulationCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand())) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand()))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit LoadUrdfCommand ") print("submit LoadUrdfCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(
fileName="door.urdf",
initialPosition=pybullet_pb2.vec3(x=0, y=0, z=0),
useMultiBody=True,
useFixedBase=True,
globalScaling=2,
flags=1)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
bodyUniqueId = response.urdfStatus.bodyUniqueId bodyUniqueId = response.urdfStatus.bodyUniqueId
print("submit LoadSdfCommand") print("submit LoadSdfCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(
fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit LoadMjcfCommand") print("submit LoadMjcfCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(fileName="mjcf/humanoid.xml",flags=MJCF_COLORS_FROM_FILE))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(loadMjcfCommand=pybullet_pb2.LoadMjcfCommand(
fileName="mjcf/humanoid.xml", flags=MJCF_COLORS_FROM_FILE)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit ChangeDynamicsCommand ") print("submit ChangeDynamicsCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(
bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit GetDynamicsCommand ") print("submit GetDynamicsCommand ")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(getDynamicsCommand=pybullet_pb2.GetDynamicsCommand(
bodyUniqueId=bodyUniqueId, linkIndex=-1)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit InitPoseCommand") print("submit InitPoseCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(bodyUniqueId=bodyUniqueId, initialStateQ=[1,2,3],hasInitialStateQ=[1,1,1]))) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(initPoseCommand=pybullet_pb2.InitPoseCommand(
bodyUniqueId=bodyUniqueId, initialStateQ=[1, 2, 3], hasInitialStateQ=[1, 1, 1])))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
print("submit RequestActualStateCommand") print("submit RequestActualStateCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True ))) response = stub.SubmitCommand(
pybullet_pb2.
PyBulletCommand(requestActualStateCommand=pybullet_pb2.RequestActualStateCommand(
bodyUniqueId=bodyUniqueId, computeForwardKinematics=True, computeLinkVelocities=True)))
print("PyBullet client received: ", response) print("PyBullet client received: ", response)
i = 0 i = 0
while (True): while (True):
i = i + 1 i = i + 1
print("submit StepSimulationCommand: ", i) print("submit StepSimulationCommand: ", i)
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand())) response = stub.SubmitCommand(
pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
print("PyBullet client received: ", response.statusType) print("PyBullet client received: ", response.statusType)
#print("TerminateServerCommand") #print("TerminateServerCommand")
#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand())) #response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
#print("PyBullet client received: " , response.statusType) #print("PyBullet client received: " , response.statusType)
if __name__ == '__main__': if __name__ == '__main__':
run() run()

46
examples/pybullet/examples/addPlanarReflection.py
View File

@@ -2,24 +2,36 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX, mouseY): def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos rayFrom = camPos
oneOverWidth = float(1) / float(width) oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height) oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight] dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayToCenter = [
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], ]
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] rayTo = [
rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
@@ -39,14 +51,28 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0] shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) fileName="duck.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="duck_vhacd.obj",
collisionFramePosition=shift,
meshScale=meshScale)
rangex = 3 rangex = 3
rangey = 3 rangey = 3
for i in range(rangex): for i in range(rangex):
for j in range(rangey): for j in range(rangey):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)

11
examples/pybullet/examples/batchRayCast.py
View File

@@ -15,7 +15,6 @@ p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
#p.loadURDF("samurai.urdf") #p.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf", [3, 3, 1]) p.loadURDF("r2d2.urdf", [3, 3, 1])
rayFrom = [] rayFrom = []
rayTo = [] rayTo = []
rayIds = [] rayIds = []
@@ -24,7 +23,6 @@ numRays = 1024
rayLen = 13 rayLen = 13
rayHitColor = [1, 0, 0] rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0] rayMissColor = [0, 1, 0]
@@ -32,7 +30,10 @@ replaceLines = True
for i in range(numRays): for i in range(numRays):
rayFrom.append([0, 0, 1]) rayFrom.append([0, 0, 1])
rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) rayTo.append([
rayLen * math.sin(2. * math.pi * float(i) / numRays),
rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
])
if (replaceLines): if (replaceLines):
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
else: else:
@@ -53,9 +54,6 @@ for i in range (numSteps):
#for i in range (10): #for i in range (10):
# p.removeAllUserDebugItems() # p.removeAllUserDebugItems()
if (useGui): if (useGui):
if (not replaceLines): if (not replaceLines):
p.removeAllUserDebugItems() p.removeAllUserDebugItems()
@@ -63,7 +61,6 @@ for i in range (numSteps):
for i in range(numRays): for i in range(numRays):
hitObjectUid = results[i][0] hitObjectUid = results[i][0]
if (hitObjectUid < 0): if (hitObjectUid < 0):
hitPosition = [0, 0, 0] hitPosition = [0, 0, 0]
p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor, replaceItemUniqueId=rayIds[i]) p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor, replaceItemUniqueId=rayIds[i])

4
examples/pybullet/examples/biped2d_pybullet.py
View File

@@ -15,9 +15,7 @@ p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf") planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1.13] cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0]) cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
cubeStartPos,
cubeStartOrientation)
#disable the default velocity motors #disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose #and set some position control with small force to emulate joint friction/return to a rest pose

2
examples/pybullet/examples/changeTexture.py
View File

@@ -17,7 +17,6 @@ colorR = 0
colorG = 0 colorG = 0
colorB = 0 colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) #p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
@@ -40,4 +39,3 @@ for i in range (100000):
p.stopStateLogging(logId) p.stopStateLogging(logId)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1) #p.configureDebugVisualizer(p.COV_ENABLE_GUI,1)

3
examples/pybullet/examples/commandLogAndPlayback.py
View File

@@ -7,9 +7,8 @@ p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf", [0, 0, 1]) p.loadURDF("r2d2.urdf", [0, 0, 1])
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.resetSimulation(); p.resetSimulation()
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin") logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin")
while (p.isConnected()): while (p.isConnected()):
time.sleep(1. / 240.) time.sleep(1. / 240.)

5
examples/pybullet/examples/contactFriction.py
View File

@@ -25,10 +25,5 @@ while(1):
totalLateralFrictionForce[1] += pt[11][1] * pt[10] + pt[13][1] * pt[12] totalLateralFrictionForce[1] += pt[11][1] * pt[10] + pt[13][1] * pt[12]
totalLateralFrictionForce[2] += pt[11][2] * pt[10] + pt[13][2] * pt[12] totalLateralFrictionForce[2] += pt[11][2] * pt[10] + pt[13][2] * pt[12]
print("totalNormalForce=", totalNormalForce) print("totalNormalForce=", totalNormalForce)
print("totalLateralFrictionForce=", totalLateralFrictionForce) print("totalLateralFrictionForce=", totalLateralFrictionForce)

91
examples/pybullet/examples/createMesh.py
View File

@@ -8,48 +8,33 @@ p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE) #p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0) #p.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); #p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh: #a few different ways to create a mesh:
vertices=[ [-0.246350,-0.246483,-0.000624], vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
[-0.151407, -0.176325, 0.172867], [-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
[ -0.246350, 0.249205, -0.000624], [0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
[ -0.151407, 0.129477, 0.172867], [0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]]
[ 0.249338, -0.246483, -0.000624], indices = [
[ 0.154395, -0.176325, 0.172867], 0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4,
[ 0.249338, 0.249205, -0.000624], 0, 3, 1, 5
[ 0.154395, 0.129477, 0.172867]
] ]
indices=[0,3,2,
3,6,2,
7,4,6,
5,0,4,
6,0,2,
3,5,7,
0,1,3,
3,7,6,
7,5,4,
5,1,0,
6,4,0,
3,1,5]
#convex mesh from obj #convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices) stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
boxHalfLength = 0.5 boxHalfLength = 0.5
boxHalfWidth = 2.5 boxHalfWidth = 2.5
boxHalfHeight = 0.1 boxHalfHeight = 0.1
segmentLength = 5 segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
@@ -57,32 +42,44 @@ visualShapeId = -1
segmentStart = 0 segmentStart = 0
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
for i in range(segmentLength): for i in range(segmentLength):
height = 0 height = 0
if (i % 2): if (i % 2):
height = 1 height = 1
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1 + height])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.]) baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
if (i % 2): if (i % 2):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
baseOrientation=baseOrientation)
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
width = 4 width = 4
for j in range(width): for j in range(width):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=stoneId,
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
link_Masses = [1] link_Masses = [1]
linkCollisionShapeIndices = [colBoxId] linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1] linkVisualShapeIndices = [-1]
@@ -96,26 +93,44 @@ axis=[[1,0,0]]
baseOrientation = [0, 0, 0, 1] baseOrientation = [0, 0, 0, 1]
for i in range(segmentLength): for i in range(segmentLength):
boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0) p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId)) print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)): for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10 targetVelocity = 10
if (i % 2): if (i % 2):
targetVelocity = -10 targetVelocity = -10
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100) p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1 segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1): while (1):
camData = p.getDebugVisualizerCamera() camData = p.getDebugVisualizerCamera()
viewMat = camData[2] viewMat = camData[2]
projMat = camData[3] projMat = camData[3]
p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
p.stepSimulation() p.stepSimulation()
#print(keys) #print(keys)
time.sleep(0.01) time.sleep(0.01)

181
examples/pybullet/examples/createMultiBodyBatch.py
View File

@@ -6,7 +6,6 @@ cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
@@ -22,111 +21,109 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0] shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1] meshScale = [0.1, 0.1, 0.1]
vertices=[ vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000,-1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000,1.000000,1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000,-1.000000,-1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000,1.000000,-1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,-1.000000], [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000,1.000000,1.000000], [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
]
normals=[ normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000,0.000000,1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
[-1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000]
]
uvs=[ uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
[0.750000,0.250000], [0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
[1.000000,0.250000], [0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
[1.000000,0.000000], [0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
[0.750000,0.000000], [0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
[0.500000,0.250000], [0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
[0.250000,0.250000], indices = [
[0.250000,0.000000], 0,
[0.500000,0.000000], 1,
[0.500000,0.000000], 2,
[0.750000,0.000000], 0,
[0.750000,0.250000], 2,
[0.500000,0.250000], 3, #//ground face
[0.250000,0.500000], 6,
[0.250000,0.250000], 5,
[0.000000,0.250000], 4,
[0.000000,0.500000], 7,
[0.250000,0.500000], 6,
[0.250000,0.250000], 4, #//top face
[0.500000,0.250000], 10,
[0.500000,0.500000], 9,
[0.000000,0.000000], 8,
[0.000000,0.250000], 11,
[0.250000,0.250000], 10,
[0.250000,0.000000] 8,
12,
13,
14,
12,
14,
15,
18,
17,
16,
19,
18,
16,
20,
21,
22,
20,
22,
23
] ]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) #p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
batchPositions = [] batchPositions = []
for x in range(32): for x in range(32):
for y in range(32): for y in range(32):
for z in range(10): for z in range(10):
batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5]) batchPositions.append(
[x * meshScale[0] * 5.5, y * meshScale[1] * 5.5, (0.5 + z) * meshScale[2] * 2.5])
bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.syncBodyInfo() p.syncBodyInfo()
@@ -139,9 +136,7 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
while (1): while (1):
p.stepSimulation() p.stepSimulation()
#time.sleep(1./120.) #time.sleep(1./120.)
#p.getCameraImage(320,200) #p.getCameraImage(320,200)

37
examples/pybullet/examples/createMultiBodyLinks.py
View File

@@ -1,20 +1,18 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE) p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0) p.createMultiBody(0, 0)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
link_Masses = [1] link_Masses = [1]
linkCollisionShapeIndices = [colBoxId] linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1] linkVisualShapeIndices = [-1]
@@ -29,18 +27,38 @@ axis=[[0,0,1]]
for i in range(3): for i in range(3):
for j in range(3): for j in range(3):
for k in range(3): for k in range(3):
basePosition = [1+i*5*sphereRadius,1+j*5*sphereRadius,1+k*5*sphereRadius+1] basePosition = [
1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
]
baseOrientation = [0, 0, 0, 1] baseOrientation = [0, 0, 0, 1]
if (k & 2): if (k & 2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation) sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
baseOrientation)
else: else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) sphereUid = p.createMultiBody(mass,
colBoxId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
for joint in range(p.getNumJoints(sphereUid)): for joint in range(p.getNumJoints(sphereUid)):
p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10) p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
@@ -53,4 +71,3 @@ while (1):
print(keys) print(keys)
time.sleep(0.01) time.sleep(0.01)

64
examples/pybullet/examples/createObstacleCourse.py
View File

@@ -8,11 +8,10 @@ p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE) #p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0) #p.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); #p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
@@ -21,14 +20,13 @@ colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
#convex mesh from obj #convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj") stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
boxHalfLength = 0.5 boxHalfLength = 0.5
boxHalfWidth = 2.5 boxHalfWidth = 2.5
boxHalfHeight = 0.1 boxHalfHeight = 0.1
segmentLength = 5 segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
@@ -36,32 +34,44 @@ visualShapeId = -1
segmentStart = 0 segmentStart = 0
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
for i in range(segmentLength): for i in range(segmentLength):
height = 0 height = 0
if (i % 2): if (i % 2):
height = 1 height = 1
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1 + height])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.]) baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
if (i % 2): if (i % 2):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
baseOrientation=baseOrientation)
for i in range(segmentLength): for i in range(segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
width = 4 width = 4
for j in range(width): for j in range(width):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0]) p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=stoneId,
basePosition=[segmentStart, 0.5 * (i % 2) + j - width / 2., 0])
segmentStart = segmentStart - 1 segmentStart = segmentStart - 1
link_Masses = [1] link_Masses = [1]
linkCollisionShapeIndices = [colBoxId] linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1] linkVisualShapeIndices = [-1]
@@ -75,26 +85,44 @@ axis=[[1,0,0]]
baseOrientation = [0, 0, 0, 1] baseOrientation = [0, 0, 0, 1]
for i in range(segmentLength): for i in range(segmentLength):
boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0) p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0)
print(p.getNumJoints(boxId)) print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)): for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10 targetVelocity = 10
if (i % 2): if (i % 2):
targetVelocity = -10 targetVelocity = -10
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100) p.setJointMotorControl2(boxId,
joint,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=100)
segmentStart = segmentStart - 1.1 segmentStart = segmentStart - 1.1
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1): while (1):
camData = p.getDebugVisualizerCamera() camData = p.getDebugVisualizerCamera()
viewMat = camData[2] viewMat = camData[2]
projMat = camData[3] projMat = camData[3]
p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
p.stepSimulation() p.stepSimulation()
#print(keys) #print(keys)
time.sleep(0.01) time.sleep(0.01)

29
examples/pybullet/examples/createSphereMultiBodies.py
View File

@@ -5,28 +5,40 @@ useMaximalCoordinates = 0
p.connect(p.GUI) p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) #p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH) monastryId = concaveEnv = p.createCollisionShape(p.GEOM_MESH,
fileName="samurai_monastry.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963, 0, 0]) orn = p.getQuaternionFromEuler([1.5707963, 0, 0])
p.createMultiBody(0, monastryId, baseOrientation=orn) p.createMultiBody(0, monastryId, baseOrientation=orn)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
for i in range(5): for i in range(5):
for j in range(5): for j in range(5):
for k in range(5): for k in range(5):
if (k & 2): if (k & 2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) sphereUid = p.createMultiBody(
mass,
colSphereId,
visualShapeId, [-i * 2 * sphereRadius, j * 2 * sphereRadius, k * 2 * sphereRadius + 1],
useMaximalCoordinates=useMaximalCoordinates)
else: else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) sphereUid = p.createMultiBody(
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) mass,
colBoxId,
visualShapeId, [-i * 2 * sphereRadius, j * 2 * sphereRadius, k * 2 * sphereRadius + 1],
useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
@@ -35,4 +47,3 @@ while (1):
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
#print(keys) #print(keys)
time.sleep(0.01) time.sleep(0.01)

199
examples/pybullet/examples/createTexturedMeshVisualShape.py
View File

@@ -2,24 +2,36 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX, mouseY): def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos rayFrom = camPos
oneOverWidth = float(1) / float(width) oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height) oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight] dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayToCenter = [
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], ]
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] rayTo = [
rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
@@ -37,99 +49,94 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0] shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1] meshScale = [0.1, 0.1, 0.1]
vertices=[ vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000,-1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000,1.000000,1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000,-1.000000,-1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000,1.000000,-1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,-1.000000], [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000,1.000000,1.000000], [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
]
normals=[ normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000,0.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000,0.000000,1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000,0.000000,-1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[-1.000000,0.000000,0.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
[-1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[1.000000,0.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,-1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000],
[0.000000,1.000000,0.000000]
]
uvs=[ uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
[0.750000,0.250000], [0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
[1.000000,0.250000], [0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
[1.000000,0.000000], [0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
[0.750000,0.000000], [0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
[0.500000,0.250000], [0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
[0.250000,0.250000], indices = [
[0.250000,0.000000], 0,
[0.500000,0.000000], 1,
[0.500000,0.000000], 2,
[0.750000,0.000000], 0,
[0.750000,0.250000], 2,
[0.500000,0.250000], 3, #//ground face
[0.250000,0.500000], 6,
[0.250000,0.250000], 5,
[0.000000,0.250000], 4,
[0.000000,0.500000], 7,
[0.250000,0.500000], 6,
[0.250000,0.250000], 4, #//top face
[0.500000,0.250000], 10,
[0.500000,0.500000], 9,
[0.000000,0.000000], 8,
[0.000000,0.250000], 11,
[0.250000,0.250000], 10,
[0.250000,0.000000] 8,
12,
13,
14,
12,
14,
15,
18,
17,
16,
19,
18,
16,
20,
21,
22,
20,
22,
23
] ]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) #visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") #visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
vertices=vertices,
collisionFramePosition=shift,
meshScale=meshScale)
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
@@ -137,7 +144,13 @@ rangex = 1
rangey = 1 rangey = 1
for i in range(rangex): for i in range(rangex):
for j in range(rangey): for j in range(rangey):
bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) bodyUid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId) p.stopStateLogging(logId)

46
examples/pybullet/examples/createVisualShape.py
View File

@@ -2,25 +2,37 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX, mouseY): def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos rayFrom = camPos
oneOverWidth = float(1) / float(width) oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height) oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight] dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayToCenter = [
rayTo = [rayToCenter[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
rayToCenter[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], ]
rayToCenter[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] rayTo = [
rayToCenter[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayToCenter[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayToCenter[2] - 0.5 * horizon[2] +
0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
@@ -38,14 +50,28 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0] shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) fileName="duck.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="duck_vhacd.obj",
collisionFramePosition=shift,
meshScale=meshScale)
rangex = 5 rangex = 5
rangey = 5 rangey = 5
for i in range(rangex): for i in range(rangex):
for j in range(rangey): for j in range(rangey):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)

53
examples/pybullet/examples/createVisualShapeArray.py
View File

@@ -2,24 +2,36 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX, mouseY): def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos rayFrom = camPos
oneOverWidth = float(1) / float(width) oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height) oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight] dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayToCenter = [
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], ]
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] rayTo = [
rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
@@ -40,15 +52,34 @@ shift2 = [0,0,0]
meshScale = [0.1, 0.1, 0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck.obj",""], visualFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck_vhacd.obj",""], collisionFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
fileNames=["duck.obj", ""],
visualFramePositions=[
shift1,
shift2,
],
meshScales=[meshScale, meshScale])
collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
fileNames=["duck_vhacd.obj", ""],
collisionFramePositions=[
shift1,
shift2,
],
meshScales=[meshScale, meshScale])
rangex = 2 rangex = 2
rangey = 2 rangey = 2
for i in range(rangex): for i in range(rangex):
for j in range(rangey): for j in range(rangey):
mb = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i*2)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*4,1], useMaximalCoordinates=False) mb = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i * 2) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 4, 1],
useMaximalCoordinates=False)
p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 1]) p.changeVisualShape(mb, -1, rgbaColor=[1, 1, 1, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId) p.stopStateLogging(logId)

6
examples/pybullet/examples/debugDrawItems.py
View File

@@ -6,7 +6,11 @@ if (cid<0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf") kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugText("tip", [0, 0, 0.1],
textColorRGB=[1, 0, 0],
textSize=1.5,
parentObjectUniqueId=kuka,
parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=kuka, parentLinkIndex=6)

1
examples/pybullet/examples/dumpLog.py
View File

@@ -7,6 +7,7 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose=True): def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')

4
examples/pybullet/examples/dumpVrLog.py
View File

@@ -7,6 +7,7 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose=True): def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
@@ -90,7 +91,8 @@ for record in log:
# indices of buttons that are released # indices of buttons that are released
buttonReleasedIndices = [] buttonReleasedIndices = []
buttonIndex = 0 buttonIndex = 0
for packedButtonIndex in range(firstPackedButtonIndex, firstPackedButtonIndex+numPackedButtons): for packedButtonIndex in range(firstPackedButtonIndex,
firstPackedButtonIndex + numPackedButtons):
for packButtonShift in range(numGroupedButtons): for packButtonShift in range(numGroupedButtons):
buttonEvent = buttonMask & record[packedButtonIndex] buttonEvent = buttonMask & record[packedButtonIndex]
if buttonEvent & 1: if buttonEvent & 1:

18
examples/pybullet/examples/eglRenderTest.py
View File

@@ -1,4 +1,3 @@
import pybullet as p import pybullet as p
import time import time
import pkgutil import pkgutil
@@ -24,7 +23,6 @@ pitch = -10.0
roll = 0 roll = 0
upAxisIndex = 2 upAxisIndex = 2
while (p.isConnected()): while (p.isConnected()):
for yaw in range(0, 360, 10): for yaw in range(0, 360, 10):
start = time.time() start = time.time()
@@ -33,15 +31,23 @@ while (p.isConnected()):
print("stepSimulation %f" % (stop - start)) print("stepSimulation %f" % (stop - start))
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0] #viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
projectionMatrix = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0] upAxisIndex)
projectionMatrix = [
1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0,
-1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0
]
start = time.time() start = time.time()
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, shadow=1,lightDirection=[1,1,1]) img_arr = p.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix=viewMatrix,
projectionMatrix=projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1])
stop = time.time() stop = time.time()
print("renderImage %f" % (stop - start)) print("renderImage %f" % (stop - start))
#time.sleep(.1) #time.sleep(.1)
#print("img_arr=",img_arr) #print("img_arr=",img_arr)
p.unloadPlugin(plugin) p.unloadPlugin(plugin)

30
examples/pybullet/examples/experimentalCcdSphereRadius.py
View File

@@ -4,42 +4,45 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0) p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
terrain_mass = 0 terrain_mass = 0
terrain_visual_shape_id = -1 terrain_visual_shape_id = -1
terrain_position = [0, 0, 0] terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1] terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape( terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
shapeType=p.GEOM_MESH,
fileName="terrain.obj", fileName="terrain.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
p.GEOM_CONCAVE_INTERNAL_EDGE,
meshScale=[0.5, 0.5, 0.5]) meshScale=[0.5, 0.5, 0.5])
p.createMultiBody( p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation) terrain_position, terrain_orientation)
useMaximalCoordinates = True useMaximalCoordinates = True
sphereRadius = 0.005 sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
for i in range(5): for i in range(5):
for j in range(5): for j in range(5):
for k in range(5): for k in range(5):
#if (k&2): #if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) sphereUid = p.createMultiBody(
mass,
colSphereId,
visualShapeId, [-i * 5 * sphereRadius, j * 5 * sphereRadius, k * 2 * sphereRadius + 1],
useMaximalCoordinates=useMaximalCoordinates)
#else: #else:
# sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) # sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) p.changeDynamics(sphereUid,
-1,
spinningFriction=0.001,
rollingFriction=0.001,
linearDamping=0.0)
p.changeDynamics(sphereUid, -1, ccdSweptSphereRadius=0.002) p.changeDynamics(sphereUid, -1, ccdSweptSphereRadius=0.002)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
pts = p.getContactPoints() pts = p.getContactPoints()
@@ -48,4 +51,3 @@ print(pts)
while (p.isConnected()): while (p.isConnected()):
time.sleep(1. / 240.) time.sleep(1. / 240.)
p.stepSimulation() p.stepSimulation()

2
examples/pybullet/examples/externalTorqueControlledSphere.py
View File

@@ -14,7 +14,6 @@ p.changeVisualShape(sphere,-1,rgbaColor=[1,0,0,1])
forward = 0 forward = 0
turn = 0 turn = 0
forwardVec = [2, 0, 0] forwardVec = [2, 0, 0]
cameraDistance = 1 cameraDistance = 1
cameraYaw = 35 cameraYaw = 35
@@ -29,7 +28,6 @@ while (1):
camInfo = p.getDebugVisualizerCamera() camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5] camForward = camInfo[5]
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
for k, v in keys.items(): for k, v in keys.items():

1
examples/pybullet/examples/fileIOPlugin.py
View File

@@ -12,7 +12,6 @@ if (fileIO>=0):
else: else:
print("fileIOPlugin is disabled.") print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False) p.setPhysicsEngineParameter(enableFileCaching=False)
while (1): while (1):

1
examples/pybullet/examples/forcetorquesensor.py
View File

@@ -23,4 +23,3 @@ print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0, 0)) print(p.getJointState(0, 0))
p.disconnect() p.disconnect()

6
examples/pybullet/examples/frictionCone.py
View File

@@ -10,7 +10,6 @@ plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinat
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
velocity = 1 velocity = 1
num = 40 num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
@@ -24,7 +23,9 @@ for i in range (num):
x = velocity * math.sin(2. * 3.1415 * float(i) / num) x = velocity * math.sin(2. * 3.1415 * float(i) / num)
y = velocity * math.cos(2. * 3.1415 * float(i) / num) y = velocity * math.cos(2. * 3.1415 * float(i) / num)
print("velocity=", x, y) print("velocity=", x, y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates) sphere = p.loadURDF("sphere_small_zeroinertia.urdf",
flags=p.URDF_USE_INERTIA_FROM_FILE,
useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere, -1, lateralFriction=0.02) p.changeDynamics(sphere, -1, lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10) #p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere, -1, linearDamping=0) p.changeDynamics(sphere, -1, linearDamping=0)
@@ -41,6 +42,5 @@ for i in range (num):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1): while (1):
time.sleep(0.01) time.sleep(0.01)

3
examples/pybullet/examples/getAABB.py
View File

@@ -8,6 +8,7 @@ else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
r2d2 = p.loadURDF("r2d2.urdf") r2d2 = p.loadURDF("r2d2.urdf")
def drawAABB(aabb): def drawAABB(aabb):
f = [aabbMin[0], aabbMin[1], aabbMin[2]] f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0], aabbMin[1], aabbMin[2]] t = [aabbMax[0], aabbMin[1], aabbMin[2]]
@@ -53,6 +54,7 @@ def drawAABB(aabb):
t = [aabbMax[0], aabbMax[1], aabbMin[2]] t = [aabbMax[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1]) p.addUserDebugLine(f, t, [1, 1, 1])
aabb = p.getAABB(r2d2) aabb = p.getAABB(r2d2)
aabbMin = aabb[0] aabbMin = aabb[0]
aabbMax = aabb[1] aabbMax = aabb[1]
@@ -72,7 +74,6 @@ for i in range (p.getNumJoints(r2d2)):
if (draw == 1): if (draw == 1):
drawAABB(aabb) drawAABB(aabb)
while (1): while (1):
a = 0 a = 0
p.stepSimulation() p.stepSimulation()

48
examples/pybullet/examples/getCameraImageTest.py
View File

@@ -3,11 +3,9 @@ import numpy as np
import pybullet as p import pybullet as p
import time import time
direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0") direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin") #egl = p.loadPlugin("eglRendererPlugin")
p.loadURDF('plane.urdf') p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf", [0, 0, 1]) p.loadURDF("r2d2.urdf", [0, 0, 1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025]) p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
@@ -25,7 +23,12 @@ view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer # Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL) images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255. rgb_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images[3], [width, height]) depth_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl) depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
@@ -33,14 +36,17 @@ seg_opengl = np.reshape(images[4], [width, height])*1./255.
time.sleep(1) time.sleep(1)
# Get depth values using Tiny renderer # Get depth values using Tiny renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER) images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height]) depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny) depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255. rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255. seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
bearStartPos1 = [-3.3, 0, 0] bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0]) bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
@@ -53,13 +59,34 @@ for b in range (p.getNumBodies()):
for j in range(p.getNumJoints(b)): for j in range(p.getNumJoints(b)):
p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId) p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId)
viewMat = [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0] viewMat = [
projMat = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] 0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat) 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
]
projMat = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
0.0, 0.0, -0.02000020071864128, 0.0
]
images = p.getCameraImage(width,
height,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
flags=p.ER_USE_PROJECTIVE_TEXTURE,
projectiveTextureView=viewMat,
projectiveTextureProj=projMat)
proj_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255. proj_opengl = np.reshape(images[2], (height, width, 4)) * 1. / 255.
time.sleep(1) time.sleep(1)
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_TINY_RENDERER, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat) images = p.getCameraImage(width,
height,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_TINY_RENDERER,
flags=p.ER_USE_PROJECTIVE_TEXTURE,
projectiveTextureView=viewMat,
projectiveTextureProj=projMat)
proj_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255. proj_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
# Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane # Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
@@ -96,5 +123,4 @@ plt.imshow(proj_tiny)
plt.title('Proj Tiny') plt.title('Proj Tiny')
plt.subplots_adjust(hspace=0.7) plt.subplots_adjust(hspace=0.7)
plt.show() plt.show()

29
examples/pybullet/examples/getClosestPoints.py
View File

@@ -11,12 +11,18 @@ obA=-1
obB = -1 obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1]) obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB ) obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3 lineWidth = 3
colorRGB = [1, 0, 0] colorRGB = [1, 0, 0]
lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0) lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0 pitch = 0
yaw = 0 yaw = 0
@@ -33,7 +39,14 @@ while (p.isConnected()):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB) p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery): if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) pts = p.getClosestPoints(bodyA=-1,
bodyB=-1,
distance=100,
collisionShapeA=geom,
collisionShapeB=geomBox,
collisionShapePositionA=[0.5, 0, 1],
collisionShapePositionB=basePositionB,
collisionShapeOrientationB=baseOrientationB)
#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) #pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else: else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100) pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
@@ -44,10 +57,14 @@ while (p.isConnected()):
#print("distance=",distance) #print("distance=",distance)
ptA = pts[0][5] ptA = pts[0][5]
ptB = pts[0][6] ptB = pts[0][6]
p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId); p.addUserDebugLine(lineFromXYZ=ptA,
lineToXYZ=ptB,
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0,
replaceItemUniqueId=lineId)
#time.sleep(1./240.) #time.sleep(1./240.)
#removeCollisionShape is optional: #removeCollisionShape is optional:
#only use removeCollisionShape if the collision shape is not used to create a body #only use removeCollisionShape if the collision shape is not used to create a body
#and if you want to keep on creating new collision shapes for different queries (not recommended) #and if you want to keep on creating new collision shapes for different queries (not recommended)

1
examples/pybullet/examples/getTextureUid.py
View File

@@ -16,4 +16,3 @@ p.changeVisualShape(plane,-1,textureUniqueId=curTexUid)
for i in range(100): for i in range(100):
p.getCameraImage(320, 200) p.getCameraImage(320, 200)

2
examples/pybullet/examples/grpcClient.py
View File

@@ -1,4 +1,3 @@
import pybullet as p import pybullet as p
usePort = True usePort = True
@@ -16,4 +15,3 @@ if (id<0):
print("Connected to GRPC") print("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf") r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2)) print("numJoints = ", p.getNumJoints(r2d2))

15
examples/pybullet/examples/hand.py
View File

@@ -32,12 +32,18 @@ thumbId = 3
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
def getSerialOrNone(portname): def getSerialOrNone(portname):
try: try:
return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) return serial.Serial(port=portname,
baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
except: except:
return None return None
def convertSensor(x, fingerIndex): def convertSensor(x, fingerIndex):
minV = minVarray[fingerIndex] minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex] maxV = maxVarray[fingerIndex]
@@ -54,6 +60,7 @@ def convertSensor(x, fingerIndex):
b = (v - minV) / float(maxV - minV) b = (v - minV) / float(maxV - minV)
return (b) return (b)
ser = None ser = None
portindex = 0 portindex = 0
while (ser is None and portindex < 30): while (ser is None and portindex < 30):
@@ -69,7 +76,11 @@ while (ser is None and portindex < 30):
portindex = portindex + 1 portindex = portindex + 1
if (ser is None): if (ser is None):
ser = serial.Serial(port = "/dev/cu.usbmodem1421",baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) ser = serial.Serial(port="/dev/cu.usbmodem1421",
baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
pi = 3.141592 pi = 3.141592
if (ser is not None and ser.isOpen()): if (ser is not None and ser.isOpen()):

316
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -21,7 +21,10 @@ from pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p) explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p) stablePD = PDControllerStableMultiDof(p)
p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) p.resetDebugVisualizerCamera(cameraDistance=7,
cameraYaw=-94,
cameraPitch=-14,
cameraTargetPosition=[0.31, 0.03, -1.16])
import pybullet_data import pybullet_data
p.setTimeOut(10000) p.setTimeOut(10000)
@@ -43,7 +46,6 @@ path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03]) #p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path) print("path = ", path)
with open(path, 'r') as f: with open(path, 'r') as f:
@@ -54,62 +56,95 @@ print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames']) numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames) print("#frames = ", numFrames)
frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0) frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
erpId = p.addUserDebugParameter("erp", 0, 1, 0.2) erpId = p.addUserDebugParameter("erp", 0, 1, 0.2)
kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2) kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000) forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000)
jointTypes = [
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
]
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]] startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) p.addUserDebugText("Stable PD",
p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) [startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]],
p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) [0, 0, 0])
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) p.addUserDebugText("Spherical Drive",
[startLocations[1][0], startLocations[1][1] + 1, startLocations[1][2]],
[0, 0, 0])
p.addUserDebugText("Explicit PD",
[startLocations[2][0], startLocations[2][1] + 1, startLocations[2][2]],
[0, 0, 0])
p.addUserDebugText("Kinematic",
[startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]],
[0, 0, 0])
humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid = p.loadURDF("humanoid/humanoid.urdf",
humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) startLocations[0],
humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) globalScaling=0.25,
humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid2 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[1],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid3 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[2],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[3],
globalScaling=0.25,
useFixedBase=False,
flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1]) humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) startLocations[0], [0, 0, 0, 1])
humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) humanoid2_fix = p.createConstraint(humanoid2, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) startLocations[1], [0, 0, 0, 1])
humanoid3_fix = p.createConstraint(humanoid3, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
startLocations[2], [0, 0, 0, 1])
humanoid3_fix = p.createConstraint(humanoid4, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
startLocations[3], [0, 0, 0, 1])
startPose = [
2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229,
0.009485003066, -0.04756001538, -0.004475001447, 1, 0, 0, 0, 0.9649395871, 0.02436898957,
-0.05755497537, 0.2549218909, -0.249116, 0.9993661511, 0.009952001505, 0.03265400494,
0.01009800153, 0.9854981188, -0.06440700776, 0.09324301124, -0.1262970152, 0.170571,
0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699, -0.391532, 0.9828788495,
0.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188,
0.105854014, 0.581348
]
startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, startVel = [
1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0,
0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952,
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] -0.4505575061, 0, -1.725374735, -0.5052852598, -0.8555179722, -0.2221173515, 0, -0.1837617357,
0.00171895706, 0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0,
startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952, -4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535, 0.1313680236,
-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -0.7236998534, 0, -0.5278312973
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] ]
p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
index0 = 7 index0 = 7
for j in range(p.getNumJoints(humanoid)): for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j) ji = p.getJointInfo(humanoid, j)
targetPosition = [0] targetPosition = [0]
jointType = ji[2] jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL): if (jointType == p.JOINT_SPHERICAL):
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] targetPosition = [
startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0]
]
targetVel = [startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2]] targetVel = [startVel[index0 + 0], startVel[index0 + 1], startVel[index0 + 2]]
index0 += 4 index0 += 4
print("spherical position: ", targetPosition) print("spherical position: ", targetPosition)
@@ -125,18 +160,36 @@ for j in range (p.getNumJoints(humanoid)):
p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
for j in range(p.getNumJoints(humanoid)): for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j) ji = p.getJointInfo(humanoid, j)
targetPosition = [0] targetPosition = [0]
jointType = ji[2] jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL): if (jointType == p.JOINT_SPHERICAL):
targetPosition = [0, 0, 0, 1] targetPosition = [0, 0, 0, 1]
p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0]) p.setJointMotorControlMultiDof(humanoid,
p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[31,31,31]) j,
p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[1,1,1]) p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[0, 0, 0])
p.setJointMotorControlMultiDof(humanoid3,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[31, 31, 31])
p.setJointMotorControlMultiDof(humanoid4,
j,
p.POSITION_CONTROL,
targetPosition,
targetVelocity=[0, 0, 0],
positionGain=0,
velocityGain=1,
force=[1, 1, 1])
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
@@ -147,8 +200,6 @@ for j in range (p.getNumJoints(humanoid)):
print("joint[", j, "].type=", jointTypes[ji[2]]) print("joint[", j, "].type=", jointTypes[ji[2]])
print("joint[", j, "].name=", ji[1]) print("joint[", j, "].name=", ji[1])
jointIds = [] jointIds = []
paramIds = [] paramIds = []
for j in range(p.getNumJoints(humanoid)): for j in range(p.getNumJoints(humanoid)):
@@ -191,16 +242,19 @@ leftElbow=13
import time import time
kpOrg = [
kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300] 0, 0, 0, 0, 0, 0, 0, 1000, 1000, 1000, 1000, 100, 100, 100, 100, 500, 500, 500, 500, 500, 400,
kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30] 400, 400, 400, 400, 400, 400, 400, 300, 500, 500, 500, 500, 500, 400, 400, 400, 400, 400, 400,
400, 400, 300
]
kdOrg = [
0, 0, 0, 0, 0, 0, 0, 100, 100, 100, 100, 10, 10, 10, 10, 50, 50, 50, 50, 50, 40, 40, 40, 40,
40, 40, 40, 40, 30, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40, 40, 30
]
once = True once = True
p.getCameraImage(320, 200) p.getCameraImage(320, 200)
while (p.isConnected()): while (p.isConnected()):
if useGUI: if useGUI:
@@ -236,9 +290,11 @@ while (p.isConnected()):
basePos1Start = [frameData[1], frameData[2], frameData[3]] basePos1Start = [frameData[1], frameData[2], frameData[3]]
basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]] basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), basePos1 = [
basePos1Start[0] + frameFraction * (basePos1End[0] - basePos1Start[0]),
basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]), basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2])
]
baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]] baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]] baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction) baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction)
@@ -254,7 +310,6 @@ while (p.isConnected()):
basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1) basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn) p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn)
chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]] chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]] chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction) chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
@@ -269,19 +324,26 @@ while (p.isConnected()):
rightKneeRotStart = [frameData[20]] rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]] rightKneeRotEnd = [frameDataNext[20]]
rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] rightKneeRot = [
rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0])
]
rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]] rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]] rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]]
rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, frameFraction) rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, frameFraction)
rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]] rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] rightShoulderRotEnd = [
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25]
]
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart, rightShoulderRotEnd,
frameFraction)
rightElbowRotStart = [frameData[29]] rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]] rightElbowRotEnd = [frameDataNext[29]]
rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] rightElbowRot = [
rightElbowRotStart[0] + frameFraction * (rightElbowRotEnd[0] - rightElbowRotStart[0])
]
leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]] leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]]
leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]] leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]]
@@ -300,7 +362,9 @@ while (p.isConnected()):
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction) leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction)
leftElbowRotStart = [frameData[43]] leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]] leftElbowRotEnd = [frameDataNext[43]]
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] leftElbowRot = [
leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
]
if (0): #if (once): if (0): #if (once):
p.resetJointStateMultiDof(humanoid, chest, chestRot) p.resetJointStateMultiDof(humanoid, chest, chestRot)
@@ -328,41 +392,26 @@ while (p.isConnected()):
totalDofs += dof totalDofs += dof
jointIndicesAll = [ jointIndicesAll = [
chest, chest, neck, rightHip, rightKnee, rightAnkle, rightShoulder, rightElbow, leftHip, leftKnee,
neck, leftAnkle, leftShoulder, leftElbow
rightHip,
rightKnee,
rightAnkle,
rightShoulder,
rightElbow,
leftHip,
leftKnee,
leftAnkle,
leftShoulder,
leftElbow
] ]
basePos, baseOrn = p.getBasePositionAndOrientation(humanoid) basePos, baseOrn = p.getBasePositionAndOrientation(humanoid)
pose = [ basePos[0],basePos[1],basePos[2], pose = [
baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3], basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
chestRot[0],chestRot[1],chestRot[2],chestRot[3], chestRot[0], chestRot[1], chestRot[2], chestRot[3], neckRot[0], neckRot[1], neckRot[2],
neckRot[0],neckRot[1],neckRot[2],neckRot[3], neckRot[3], rightHipRot[0], rightHipRot[1], rightHipRot[2], rightHipRot[3],
rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3], rightKneeRot[0], rightAnkleRot[0], rightAnkleRot[1], rightAnkleRot[2], rightAnkleRot[3],
rightKneeRot[0],
rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
rightShoulderRot[0], rightShoulderRot[1], rightShoulderRot[2], rightShoulderRot[3], rightShoulderRot[0], rightShoulderRot[1], rightShoulderRot[2], rightShoulderRot[3],
rightElbowRot[0], rightElbowRot[0], leftHipRot[0], leftHipRot[1], leftHipRot[2], leftHipRot[3],
leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3], leftKneeRot[0], leftAnkleRot[0], leftAnkleRot[1], leftAnkleRot[2], leftAnkleRot[3],
leftKneeRot[0],
leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3], leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3],
leftElbowRot[0] ] leftElbowRot[0]
]
#print("pose=") #print("pose=")
#for po in pose: #for po in pose:
# print(po) # print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid, taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices=jointIndicesAll, jointIndices=jointIndicesAll,
desiredPositions=pose, desiredPositions=pose,
@@ -387,32 +436,105 @@ while (p.isConnected()):
jointIndex = jointIndicesAll[index] jointIndex = jointIndicesAll[index]
if jointDofCounts[index] == 4: if jointDofCounts[index] == 4:
p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]]) p.setJointMotorControlMultiDof(
p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]]) humanoid,
jointIndex,
p.TORQUE_CONTROL,
force=[taus[dofIndex + 0], taus[dofIndex + 1], taus[dofIndex + 2]])
p.setJointMotorControlMultiDof(
humanoid3,
jointIndex,
p.TORQUE_CONTROL,
force=[taus3[dofIndex + 0], taus3[dofIndex + 1], taus3[dofIndex + 2]])
if jointDofCounts[index] == 1: if jointDofCounts[index] == 1:
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]]) p.setJointMotorControlMultiDof(humanoid,
p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]]) jointIndex,
controlMode=p.TORQUE_CONTROL,
force=[taus[dofIndex]])
p.setJointMotorControlMultiDof(humanoid3,
jointIndex,
controlMode=p.TORQUE_CONTROL,
force=[taus3[dofIndex]])
dofIndex += jointDofCounts[index] dofIndex += jointDofCounts[index]
#print("len(taus)=",len(taus)) #print("len(taus)=",len(taus))
#print("taus=",taus) #print("taus=",taus)
p.setJointMotorControlMultiDof(humanoid2,
p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce]) chest,
p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce]) p.POSITION_CONTROL,
p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce]) targetPosition=chestRot,
p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce]) positionGain=kp,
p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce]) force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,
p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce]) neck,
p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce]) p.POSITION_CONTROL,
p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce]) targetPosition=neckRot,
p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce]) positionGain=kp,
p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce]) force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce]) p.setJointMotorControlMultiDof(humanoid2,
rightHip,
p.POSITION_CONTROL,
targetPosition=rightHipRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
rightKnee,
p.POSITION_CONTROL,
targetPosition=rightKneeRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
rightAnkle,
p.POSITION_CONTROL,
targetPosition=rightAnkleRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
rightShoulder,
p.POSITION_CONTROL,
targetPosition=rightShoulderRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
rightElbow,
p.POSITION_CONTROL,
targetPosition=rightElbowRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
leftHip,
p.POSITION_CONTROL,
targetPosition=leftHipRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
leftKnee,
p.POSITION_CONTROL,
targetPosition=leftKneeRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
leftAnkle,
p.POSITION_CONTROL,
targetPosition=leftAnkleRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
leftShoulder,
p.POSITION_CONTROL,
targetPosition=leftShoulderRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
leftElbow,
p.POSITION_CONTROL,
targetPosition=leftElbowRot,
positionGain=kp,
force=[maxForce])
kinematicHumanoid4 = True kinematicHumanoid4 = True
if (kinematicHumanoid4): if (kinematicHumanoid4):

12
examples/pybullet/examples/humanoid_benchmark.py
View File

@@ -10,12 +10,17 @@ p.loadURDF("plane.urdf")
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0] ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923]) p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] [0.710965, 0.218117, 0.519402, -0.420923])
jointPositions = [
-0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000,
-0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000,
0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000,
-0.007374, 0.000000
]
for jointIndex in range(p.getNumJoints(ob)): for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex]) p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#first let the humanoid fall #first let the humanoid fall
#p.setRealTimeSimulation(1) #p.setRealTimeSimulation(1)
#time.sleep(5) #time.sleep(5)
@@ -33,4 +38,3 @@ p.stopStateLogging(logId)
print("ended benchmark") print("ended benchmark")
print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName) print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)

15
examples/pybullet/examples/humanoid_knee_position_control.py
View File

@@ -18,8 +18,6 @@ p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml") obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0] human = obUids[0]
for i in range(p.getNumJoints(human)): for i in range(p.getNumJoints(human)):
p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500) p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500)
@@ -29,7 +27,6 @@ maxForceId = p.addUserDebugParameter("maxForce",0,500,10)
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1) kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10) maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
kneeJointIndex = 11 kneeJointIndex = 11
kneeJointIndexLeft = 18 kneeJointIndexLeft = 18
@@ -37,10 +34,18 @@ while (1):
time.sleep(0.01) time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId) kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId) maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce) p.setJointMotorControl2(human,
kneeJointIndex,
p.POSITION_CONTROL,
targetPosition=kneeAngleTarget,
force=maxForce)
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId) kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
maxForceLeft = p.readUserDebugParameter(maxForceLeftId) maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
p.setJointMotorControl2(human,kneeJointIndexLeft,p.POSITION_CONTROL,targetPosition=kneeAngleTargetLeft,force=maxForceLeft) p.setJointMotorControl2(human,
kneeJointIndexLeft,
p.POSITION_CONTROL,
targetPosition=kneeAngleTargetLeft,
force=maxForceLeft)
if (useRealTime == 0): if (useRealTime == 0):
p.stepSimulation() p.stepSimulation()

15
examples/pybullet/examples/ik_end_effector_orientation.py
View File

@@ -23,8 +23,19 @@ while 1:
pos = [0, 0, 1.26] pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14]) orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
for i in range(numJoints): for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
sleep(0.05) sleep(0.05)

1
examples/pybullet/examples/integrate.py
View File

@@ -10,4 +10,3 @@ for i in range (frequency):
p.stepSimulation() p.stepSimulation()
pos, orn = p.getBasePositionAndOrientation(cube) pos, orn = p.getBasePositionAndOrientation(cube)
print(pos) print(pos)

24
examples/pybullet/examples/internalEdge.py
View File

@@ -4,33 +4,32 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
if (1): if (1):
box_collision_shape_id = p.createCollisionShape( box_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
shapeType=p.GEOM_BOX,
halfExtents=[0.01, 0.01, 0.055]) halfExtents=[0.01, 0.01, 0.055])
box_mass = 0.1 box_mass = 0.1
box_visual_shape_id = -1 box_visual_shape_id = -1
box_position = [0, 0.1, 1] box_position = [0, 0.1, 1]
box_orientation = [0, 0, 0, 1] box_orientation = [0, 0, 0, 1]
p.createMultiBody( p.createMultiBody(box_mass,
box_mass, box_collision_shape_id, box_visual_shape_id, box_collision_shape_id,
box_position, box_orientation, useMaximalCoordinates=True) box_visual_shape_id,
box_position,
box_orientation,
useMaximalCoordinates=True)
terrain_mass = 0 terrain_mass = 0
terrain_visual_shape_id = -1 terrain_visual_shape_id = -1
terrain_position = [0, 0, 0] terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1] terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape( terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
shapeType=p.GEOM_MESH,
fileName="terrain.obj", fileName="terrain.obj",
flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, flags=p.GEOM_FORCE_CONCAVE_TRIMESH |
p.GEOM_CONCAVE_INTERNAL_EDGE,
meshScale=[0.5, 0.5, 0.5]) meshScale=[0.5, 0.5, 0.5])
p.createMultiBody( p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation) terrain_position, terrain_orientation)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
pts = p.getContactPoints() pts = p.getContactPoints()
@@ -39,4 +38,3 @@ print(pts)
while (p.isConnected()): while (p.isConnected()):
time.sleep(1. / 240.) time.sleep(1. / 240.)
p.stepSimulation() p.stepSimulation()

27
examples/pybullet/examples/inverse_dynamics.py
View File

@@ -19,25 +19,33 @@ bullet.setTimeStep(delta_t)
# Switch between URDF with/without FIXED joints # Switch between URDF with/without FIXED joints
with_fixed_joints = True with_fixed_joints = True
if with_fixed_joints: if with_fixed_joints:
id_revolute_joints = [0, 3] id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf", id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True) robot_base,
robot_orientation,
useFixedBase=True)
else: else:
id_revolute_joints = [0, 1] id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf", id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True) robot_base,
robot_orientation,
useFixedBase=True)
bullet.changeDynamics(id_robot, -1, linearDamping=0, angularDamping=0) bullet.changeDynamics(id_robot, -1, linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot, 0, linearDamping=0, angularDamping=0) bullet.changeDynamics(id_robot, 0, linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot, 1, linearDamping=0, angularDamping=0) bullet.changeDynamics(id_robot, 1, linearDamping=0, angularDamping=0)
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"] jointTypeNames = [
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED",
"JOINT_POINT2POINT", "JOINT_GEAR"
]
# Disable the motors for torque control: # Disable the motors for torque control:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0]) bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.VELOCITY_CONTROL,
forces=[0.0, 0.0])
# Target Positions: # Target Positions:
start = 0.0 start = 0.0
@@ -60,7 +68,6 @@ for s in range(steps):
q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s]) q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s])
q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s]) q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
q_pos = [[0.] * steps, [0.] * steps] q_pos = [[0.] * steps, [0.] * steps]
q_vel = [[0.] * steps, [0.] * steps] q_vel = [[0.] * steps, [0.] * steps]
q_tor = [[0.] * steps, [0.] * steps] q_tor = [[0.] * steps, [0.] * steps]
@@ -105,7 +112,10 @@ for i in range(len(t)):
print(torque) print(torque)
# Set the Joint Torques: # Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]]) bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.TORQUE_CONTROL,
forces=[torque[0], torque[1]])
# Step Simulation # Step Simulation
bullet.stepSimulation() bullet.stepSimulation()
@@ -149,7 +159,6 @@ if plot:
plt.pause(0.01) plt.pause(0.01)
while (1): while (1):
bullet.stepSimulation() bullet.stepSimulation()
time.sleep(0.01) time.sleep(0.01)

40
examples/pybullet/examples/inverse_kinematics.py
View File

@@ -14,7 +14,6 @@ numJoints = p.getNumJoints(kukaId)
if (numJoints != 7): if (numJoints != 7):
exit() exit()
#lower limits for null space #lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space #upper limits for null space
@@ -48,7 +47,10 @@ p.setRealTimeSimulation(useRealTimeSimulation)
trailDuration = 15 trailDuration = 15
while 1: while 1:
p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(320,
200,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
if (useRealTimeSimulation): if (useRealTimeSimulation):
dt = datetime.now() dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi t = (dt.second / 60.) * 2. * math.pi
@@ -65,18 +67,42 @@ while 1:
if (useNullSpace == 1): if (useNullSpace == 1):
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
jr, rp)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp)
else: else:
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100,
residualThreshold=.01)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
solver=ikSolver)
if (useSimulation): if (useSimulation):
for i in range(numJoints): for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
else: else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation) #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints): for i in range(numJoints):

48
examples/pybullet/examples/inverse_kinematics_husky_kuka.py
View File

@@ -8,20 +8,21 @@ clid = p.connect(p.SHARED_MEMORY)
if (clid < 0): if (clid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659]) husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
[0.002328, -0.000984, 0.996491, 0.083659])
for i in range(p.getNumJoints(husky)): for i in range(p.getNumJoints(husky)):
print(p.getJointInfo(husky, i)) print(p.getJointInfo(husky, i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564, 0.120208, 0.002327,
-0.000988, 0.996491, 0.083659)
ob = kukaId ob = kukaId
jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001] jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
for jointIndex in range(p.getNumJoints(ob)): for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex]) p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#put kuka on top of husky #put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1]) cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5],
[0, 0, 0, 1])
baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3]) baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
baseorn = [0, 0, 0, 1] baseorn = [0, 0, 0, 1]
@@ -33,7 +34,6 @@ numJoints = p.getNumJoints(kukaId)
if (numJoints != 7): if (numJoints != 7):
exit() exit()
#lower limits for null space #lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space #upper limits for null space
@@ -68,6 +68,7 @@ basepos =[0,0,0]
ang = 0 ang = 0
ang = 0 ang = 0
def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter): def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False closeEnough = False
iter = 0 iter = 0
@@ -122,7 +123,11 @@ while 1:
baseorn = p.getQuaternionFromEuler([0, 0, ang]) baseorn = p.getQuaternionFromEuler([0, 0, ang])
for i in range(len(wheels)): for i in range(len(wheels)):
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000) p.setJointMotorControl2(husky,
wheels[i],
p.VELOCITY_CONTROL,
targetVelocity=wheelVelocities[i],
force=1000)
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1]) #p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
if (useRealTimeSimulation): if (useRealTimeSimulation):
t = time.time() #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.') t = time.time() #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
@@ -141,20 +146,39 @@ while 1:
if (useNullSpace == 1): if (useNullSpace == 1):
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
jr, rp)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp)
else: else:
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
else: else:
threshold = 0.001 threshold = 0.001
maxIter = 100 maxIter = 100
jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter) jointPoses = accurateCalculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos,
threshold, maxIter)
if (useSimulation): if (useSimulation):
for i in range(numJoints): for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=1,velocityGain=0.1) p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=1,
velocityGain=0.1)
else: else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation) #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints): for i in range(numJoints):

8
examples/pybullet/examples/inverse_kinematics_pole.py
View File

@@ -12,7 +12,6 @@ sawyerId = p.loadURDF("pole.urdf",[0,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3 sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId) numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents #joint damping coefficents
@@ -41,7 +40,12 @@ while 1:
for i in range(1): for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)] pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver, maxNumIterations=100) jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation) #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints): for i in range(numJoints):

18
examples/pybullet/examples/jacobian.py
View File

@@ -8,6 +8,7 @@ def getJointStates(robot):
joint_torques = [state[3] for state in joint_states] joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot): def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
@@ -17,18 +18,24 @@ def getMotorJointStates(robot):
joint_torques = [state[3] for state in joint_states] joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3): def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot) num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints zero_vec = [0.0] * num_joints
if len(position) == num_joints: if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL, p.setJointMotorControlArray(robot,
targetPositions=position, targetVelocities=zero_vec, range(num_joints),
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints) p.POSITION_CONTROL,
targetPositions=position,
targetVelocities=zero_vec,
positionGains=[kp] * num_joints,
velocityGains=[kv] * num_joints)
else: else:
print("Not setting torque. " print("Not setting torque. "
"Expected torque vector of " "Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque))) "length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(robot, jacobian, vector): def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0] result = [0.0, 0.0, 0.0]
i = 0 i = 0
@@ -69,7 +76,10 @@ p.stepSimulation()
pos, vel, torq = getJointStates(kukaId) pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId) mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) result = p.getLinkState(kukaId,
kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link. # Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.

2
examples/pybullet/examples/jointFrictionAndMotor.py
View File

@@ -10,7 +10,6 @@ for j in range (p.getNumJoints(door)):
p.changeDynamics(door, j, linearDamping=0, angularDamping=0) p.changeDynamics(door, j, linearDamping=0, angularDamping=0)
print(p.getJointInfo(door, j)) print(p.getJointInfo(door, j))
frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10) frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5) torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
@@ -23,4 +22,3 @@ while (1):
p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque) p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
p.stepSimulation() p.stepSimulation()
time.sleep(0.01) time.sleep(0.01)

11
examples/pybullet/examples/jointFrictionDamping.py
View File

@@ -5,7 +5,10 @@ p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.25]) p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur)) print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6,cameraTargetPosition=[-0.064,.621,-0.2]) p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1 motorJointId = 1
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0) p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
@@ -18,7 +21,11 @@ p.setRealTimeSimulation(1)
while (1): while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider) angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce) p.setJointMotorControl2(minitaur,
motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping) p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01) time.sleep(0.01)

3
examples/pybullet/examples/kuka_grasp_block_playback.py
View File

@@ -41,6 +41,7 @@ def readLogFile(filename, verbose = True):
return log return log
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
@@ -58,6 +59,7 @@ print(recordNum)
print('item num:'), print('item num:'),
print(itemNum) print(itemNum)
def Step(stepIndex): def Step(stepIndex):
for objectId in range(objectNum): for objectId in range(objectNum):
record = log[stepIndex * objectNum + objectId] record = log[stepIndex * objectNum + objectId]
@@ -80,4 +82,3 @@ while True:
Step(stepIndex) Step(stepIndex)
p.stepSimulation() p.stepSimulation()
Step(stepIndex) Step(stepIndex)

26
examples/pybullet/examples/kuka_with_cube.py
View File

@@ -70,18 +70,36 @@ while 1:
if (useNullSpace == 1): if (useNullSpace == 1):
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul,
jr, rp)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
lowerLimits=ll,
upperLimits=ul,
jointRanges=jr,
restPoses=rp)
else: else:
if (useOrientation == 1): if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos) jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos)
if (useSimulation): if (useSimulation):
for i in range(numJoints): for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
else: else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation) #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints): for i in range(numJoints):

2
examples/pybullet/examples/kuka_with_cube_playback.py
View File

@@ -10,6 +10,7 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose=True): def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
@@ -49,6 +50,7 @@ def readLogFile(filename, verbose = True):
return log return log
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])

2
examples/pybullet/examples/loadingBench.py
View File

@@ -24,5 +24,3 @@ p.getCameraImage(320,200)
while (1): while (1):
p.stepSimulation() p.stepSimulation()

4
examples/pybullet/examples/manyspheres.py
View File

@@ -20,7 +20,8 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
for i in range(10): for i in range(10):
for j in range(10): for j in range(10):
for k in range(10): for k in range(10):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True) ob = p.loadURDF("sphere_1cm.urdf", [0.02 * i, 0.02 * j, 0.2 + 0.02 * k],
useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
@@ -32,4 +33,3 @@ while True:
gravZ = p.readUserDebugParameter(gravZid) gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX, gravY, gravZ) p.setGravity(gravX, gravY, gravZ)
time.sleep(0.01) time.sleep(0.01)

30
examples/pybullet/examples/mimicJointConstraint.py
View File

@@ -10,20 +10,38 @@ for i in range(p.getNumJoints(wheelA)):
print(p.getJointInfo(wheelA, i)) print(p.getJointInfo(wheelA, i))
p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
c = p.createConstraint(wheelA,
c = p.createConstraint(wheelA,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) 1,
wheelA,
3,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000) p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(wheelA,2,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(wheelA,
2,
wheelA,
4,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(wheelA,1,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(wheelA,
1,
wheelA,
4,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while (1): while (1):
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
time.sleep(0.01) time.sleep(0.01)
#p.removeConstraint(c) #p.removeConstraint(c)

127
examples/pybullet/examples/minitaur.py
View File

@@ -1,7 +1,9 @@
import pybullet as p import pybullet as p
import numpy as np import numpy as np
class Minitaur: class Minitaur:
def __init__(self, urdfRootPath=''): def __init__(self, urdfRootPath=''):
self.urdfRootPath = urdfRootPath self.urdfRootPath = urdfRootPath
self.reset() self.reset()
@@ -26,7 +28,6 @@ class Minitaur:
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint']) self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint']) self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
def reset(self): def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, 0, 0, .2) self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, 0, 0, .2)
self.kp = 1 self.kp = 1
@@ -38,10 +39,14 @@ class Minitaur:
self.buildJointNameToIdDict() self.buildJointNameToIdDict()
self.buildMotorIdList() self.buildMotorIdList()
def setMotorAngleById(self, motorId, desiredAngle): def setMotorAngleById(self, motorId, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce) p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=motorId,
controlMode=p.POSITION_CONTROL,
targetPosition=desiredAngle,
positionGain=self.kp,
velocityGain=self.kd,
force=self.maxForce)
def setMotorAngleByName(self, motorName, desiredAngle): def setMotorAngleByName(self, motorName, desiredAngle):
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle) self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
@@ -52,50 +57,104 @@ class Minitaur:
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length) kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
#left front leg #left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['motor_front_leftL_joint'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle) self.motorDir[0] * halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_front_leftL_link'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle) self.motorDir[0] * kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(self.quadruped, self.jointNameToId['motor_front_leftR_joint'],
self.motorDir[1] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_leftR_link'],
self.motorDir[1] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_front_leftR_link'], self.quadruped,
self.jointNameToId['knee_front_leftL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0] * halfpi) self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0] * halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1] * halfpi) self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) jointIndex=self.jointNameToId['knee_front_leftL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#left back leg #left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['motor_back_leftL_joint'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle) self.motorDir[2] * halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_back_leftL_link'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle) self.motorDir[2] * kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(self.quadruped, self.jointNameToId['motor_back_leftR_joint'],
self.motorDir[3] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_leftR_link'],
self.motorDir[3] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_back_leftR_link'], self.quadruped,
self.jointNameToId['knee_back_leftL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_back_leftL_joint', self.motorDir[2] * halfpi) self.setMotorAngleByName('motor_back_leftL_joint', self.motorDir[2] * halfpi)
self.setMotorAngleByName('motor_back_leftR_joint', self.motorDir[3] * halfpi) self.setMotorAngleByName('motor_back_leftR_joint', self.motorDir[3] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) jointIndex=self.jointNameToId['knee_back_leftL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right front leg #right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['motor_front_rightL_joint'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle) self.motorDir[4] * halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_front_rightL_link'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle) self.motorDir[4] * kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(self.quadruped, self.jointNameToId['motor_front_rightR_joint'],
self.motorDir[5] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_rightR_link'],
self.motorDir[5] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_front_rightR_link'],
self.quadruped, self.jointNameToId['knee_front_rightL_link'],
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_front_rightL_joint', self.motorDir[4] * halfpi) self.setMotorAngleByName('motor_front_rightL_joint', self.motorDir[4] * halfpi)
self.setMotorAngleByName('motor_front_rightR_joint', self.motorDir[5] * halfpi) self.setMotorAngleByName('motor_front_rightR_joint', self.motorDir[5] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) jointIndex=self.jointNameToId['knee_front_rightL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right back leg #right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightL_joint'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle) self.motorDir[6] * halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi) p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightL_link'],
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle) self.motorDir[6] * kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightR_joint'],
self.motorDir[7] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightR_link'],
self.motorDir[7] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_back_rightR_link'], self.quadruped,
self.jointNameToId['knee_back_rightL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_back_rightL_joint', self.motorDir[6] * halfpi) self.setMotorAngleByName('motor_back_rightL_joint', self.motorDir[6] * halfpi)
self.setMotorAngleByName('motor_back_rightR_joint', self.motorDir[7] * halfpi) self.setMotorAngleByName('motor_back_rightR_joint', self.motorDir[7] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) p.setJointMotorControl2(bodyIndex=self.quadruped,
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce) jointIndex=self.jointNameToId['knee_back_rightL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
def getBasePosition(self): def getBasePosition(self):
position, orientation = p.getBasePositionAndOrientation(self.quadruped) position, orientation = p.getBasePositionAndOrientation(self.quadruped)

25
examples/pybullet/examples/minitaur_evaluate.py
View File

@@ -15,6 +15,7 @@ def current_position():
position = minitaur.getBasePosition() position = minitaur.getBasePosition()
return np.asarray(position) return np.asarray(position)
def is_fallen(): def is_fallen():
global minitaur global minitaur
orientation = minitaur.getBaseOrientation() orientation = minitaur.getBaseOrientation()
@@ -22,13 +23,16 @@ def is_fallen():
localUp = rotMat[6:] localUp = rotMat[6:]
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0 return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
def evaluate_desired_motorAngle_8Amplitude8Phase(i, params): def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
nMotors = 8 nMotors = 8
speed = 0.35 speed = 0.35
for jthMotor in range(nMotors): for jthMotor in range(nMotors):
joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57 joint_values[jthMotor] = math.sin(i * speed +
params[nMotors + jthMotor]) * params[jthMotor] * +1.57
return joint_values return joint_values
def evaluate_desired_motorAngle_2Amplitude4Phase(i, params): def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
speed = 0.35 speed = 0.35
phaseDiff = params[2] phaseDiff = params[2]
@@ -43,6 +47,7 @@ def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
joint_values = [a0, a1, a2, a3, a4, a5, a6, a7] joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
return joint_values return joint_values
def evaluate_desired_motorAngle_hop(i, params): def evaluate_desired_motorAngle_hop(i, params):
amplitude = params[0] amplitude = params[0]
speed = params[1] speed = params[1]
@@ -52,13 +57,20 @@ def evaluate_desired_motorAngle_hop(i, params):
return joint_values return joint_values
evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase evaluate_func_map[
evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase 'evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
evaluate_func_map[
'evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
def evaluate_params(evaluateFunc,
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0): params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation') print('start evaluation')
beforeTime = time.time() beforeTime = time.time()
p.resetSimulation() p.resetSimulation()
@@ -95,5 +107,6 @@ def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep
final_distance = np.linalg.norm(start_position - current_position()) final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime elapsedTime = time.time() - beforeTime
print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime) print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn return finalReturn

13
examples/pybullet/examples/minitaur_test.py
View File

@@ -10,18 +10,27 @@ import time
import math import math
import numpy as np import numpy as np
def main(unused_args): def main(unused_args):
timeStep = 0.01 timeStep = 0.01
c = p.connect(p.SHARED_MEMORY) c = p.connect(p.SHARED_MEMORY)
if (c < 0): if (c < 0):
c = p.connect(p.GUI) c = p.connect(p.GUI)
params = [0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583, 6.2406418167980595, 4.189869754607539] params = [
0.1903581461951056, 0.0006732219568880068, 0.05018085615283363, 3.219916795483583,
6.2406418167980595, 4.189869754607539
]
evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase' evaluate_func = 'evaluate_desired_motorAngle_2Amplitude4Phase'
energy_weight = 0.01 energy_weight = 0.01
finalReturn = evaluate_params(evaluateFunc = evaluate_func, params=params, objectiveParams=[energy_weight], timeStep=timeStep, sleepTime=timeStep) finalReturn = evaluate_params(evaluateFunc=evaluate_func,
params=params,
objectiveParams=[energy_weight],
timeStep=timeStep,
sleepTime=timeStep)
print(finalReturn) print(finalReturn)
main(0) main(0)

10
examples/pybullet/examples/motorMaxVelocity.py
View File

@@ -4,7 +4,15 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
cartpole = p.loadURDF("cartpole.urdf") cartpole = p.loadURDF("cartpole.urdf")
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5) p.setJointMotorControl2(cartpole,
1,
p.POSITION_CONTROL,
targetPosition=1000,
targetVelocity=0,
force=1000,
positionGain=1,
velocityGain=0,
maxVelocity=0.5)
while (1): while (1):
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
js = p.getJointState(cartpole, 1) js = p.getJointState(cartpole, 1)

21
examples/pybullet/examples/otherPhysicsEngine.py
View File

@@ -11,7 +11,9 @@ if usePhysX:
else: else:
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.,
numSolverIterations=4,
minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath()) p.setAdditionalSearchPath(pd.getDataPath())
@@ -33,7 +35,10 @@ for i in range (int(num*50)):
num = (radius * 2 * math.pi) / 0.08 num = (radius * 2 * math.pi) / 0.08
radius += 0.05 / float(num) radius += 0.05 / float(num)
orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)]) orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03] pos = [
radius * math.cos(2 * math.pi * (i / float(num))),
radius * math.sin(2 * math.pi * (i / float(num))), 0.03
]
sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates) sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
numDominoes += 1 numDominoes += 1
@@ -43,7 +48,6 @@ sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaxi
print("numDominoes=", numDominoes) print("numDominoes=", numDominoes)
#for j in range (20): #for j in range (20):
# for i in range (100): # for i in range (100):
# if (i<99): # if (i<99):
@@ -52,17 +56,14 @@ print("numDominoes=",numDominoes)
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0]) # orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates) # sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
print("loaded!") print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000) #p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf", [0, 0, -11]) door = p.loadURDF("door.urdf", [0, 0, -11])
p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door)) print("numJoints = ", p.getNumJoints(door))
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
position_control = True position_control = True
@@ -90,7 +91,13 @@ while (1):
prevTime = curTime prevTime = curTime
if position_control: if position_control:
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001) p.setJointMotorControl2(door,
1,
p.POSITION_CONTROL,
targetPosition=angle,
positionGain=10.1,
velocityGain=1,
force=11.001)
else: else:
p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011) p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
#contacts = p.getContactPoints() #contacts = p.getContactPoints()

85
examples/pybullet/examples/pdControl.py
View File

@@ -1,4 +1,3 @@
import pybullet as p import pybullet as p
from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerExplicit import PDControllerExplicit from pdControllerExplicit import PDControllerExplicit
@@ -6,7 +5,6 @@ from pdControllerStable import PDControllerStable
import time import time
useMaximalCoordinates = False useMaximalCoordinates = False
p.connect(p.GUI) p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates) pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
@@ -17,7 +15,6 @@ pole4 = p.loadURDF("cartpole.urdf", [0,3,0], useMaximalCoordinates=useMaximalCoo
exPD = PDControllerExplicitMultiDof(p) exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p) sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)): for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors #disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0) p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
@@ -27,7 +24,6 @@ for i in range (p.getNumJoints(pole2)):
#print("joint",i,"=",p.getJointInfo(pole2,i)) #print("joint",i,"=",p.getJointInfo(pole2,i))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01) timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2) desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0) desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
@@ -37,10 +33,22 @@ maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
textColor = [1, 1, 1] textColor = [1, 1, 1]
shift = 0.05 shift = 0.05
p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,parentLinkIndex=1) p.addUserDebugText("explicit PD", [shift, 0, .1],
p.addUserDebugText("explicit PD plugin", [shift,0,-.1],textColor,parentObjectUniqueId=pole2,parentLinkIndex=1) textColor,
p.addUserDebugText("stablePD", [shift,0,.1],textColor,parentObjectUniqueId=pole4,parentLinkIndex=1) parentObjectUniqueId=pole,
p.addUserDebugText("position constraint", [shift,0,-.1],textColor,parentObjectUniqueId=pole3,parentLinkIndex=1) parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0) desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0) desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
@@ -50,7 +58,6 @@ maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
pd = p.loadPlugin("pdControlPlugin") pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
useRealTimeSim = False useRealTimeSim = False
@@ -59,7 +66,6 @@ p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001 timeStep = 0.001
while p.isConnected(): while p.isConnected():
#p.getCameraImage(320,200) #p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId) timeStep = p.readUserDebugParameter(timeStepId)
@@ -80,33 +86,64 @@ while p.isConnected():
basePos, baseOrn = p.getBasePositionAndOrientation(pole) basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7 baseDof = 7
taus = exPD.computePD(pole, [0,1], [basePos[0],basePos[1],basePos[2],baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],desiredPosCart,desiredPosPole], taus = exPD.computePD(pole, [0, 1], [
[0,0,0,0,0,0,0,desiredVelCart,desiredVelPole], [0,0,0,0,0,0,0,kpCart,kpPole], [0,0,0,0,0,0,0,kdCart,kdPole],[0,0,0,0,0,0,0,maxForceCart,maxForcePole], timeStep) basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
desiredPosCart, desiredPosPole
], [0, 0, 0, 0, 0, 0, 0, desiredVelCart, desiredVelPole], [0, 0, 0, 0, 0, 0, 0, kpCart, kpPole],
[0, 0, 0, 0, 0, 0, 0, kdCart, kdPole],
[0, 0, 0, 0, 0, 0, 0, maxForceCart, maxForcePole], timeStep)
for j in [0, 1]: for j in [0, 1]:
p.setJointMotorControlMultiDof(pole, j, controlMode=p.TORQUE_CONTROL, force=[taus[j+baseDof]]) p.setJointMotorControlMultiDof(pole,
j,
controlMode=p.TORQUE_CONTROL,
force=[taus[j + baseDof]])
#p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus) #p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
if (pd >= 0): if (pd >= 0):
link = 0 link = 0
p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart) p.setJointMotorControl2(bodyUniqueId=pole2,
jointIndex=link,
controlMode=p.PD_CONTROL,
targetPosition=desiredPosCart,
targetVelocity=desiredVelCart,
force=maxForceCart,
positionGain=kpCart,
velocityGain=kdCart)
link = 1 link = 1
p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole) p.setJointMotorControl2(bodyUniqueId=pole2,
jointIndex=link,
controlMode=p.PD_CONTROL,
targetPosition=desiredPosPole,
targetVelocity=desiredVelPole,
force=maxForcePole,
positionGain=kpPole,
velocityGain=kdPole)
taus = sPD.computePD(pole4, [0, 1], [desiredPosCart, desiredPosPole],
[desiredVelCart, desiredVelPole], [kpCart, kpPole], [kdCart, kdPole],
[maxForceCart, maxForcePole], timeStep)
taus = sPD.computePD(pole4, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
#p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus) #p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
for j in [0, 1]: for j in [0, 1]:
p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]]) p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
p.setJointMotorControl2(pole3,
p.setJointMotorControl2(pole3,0, p.POSITION_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, positionGain=timeStep*(kpCart/150.), velocityGain=0.5, force=maxForceCart) 0,
p.setJointMotorControl2(pole3,1, p.POSITION_CONTROL, targetPosition=desiredPosPole, targetVelocity=desiredVelPole, positionGain=timeStep*(kpPole/150.), velocityGain=0.5, force=maxForcePole) p.POSITION_CONTROL,
targetPosition=desiredPosCart,
targetVelocity=desiredVelCart,
positionGain=timeStep * (kpCart / 150.),
velocityGain=0.5,
force=maxForceCart)
p.setJointMotorControl2(pole3,
1,
p.POSITION_CONTROL,
targetPosition=desiredPosPole,
targetVelocity=desiredVelPole,
positionGain=timeStep * (kpPole / 150.),
velocityGain=0.5,
force=maxForcePole)
if (not useRealTimeSim): if (not useRealTimeSim):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

25
examples/pybullet/examples/pdControllerExplicit.py
View File

@@ -2,16 +2,20 @@ import numpy as np
class PDControllerExplicitMultiDof(object): class PDControllerExplicitMultiDof(object):
def __init__(self, pb): def __init__(self, pb):
self._pb = pb self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId) numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId) curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]] q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId) baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0] qdot1 = [
baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
]
qError = [0, 0, 0, 0, 0, 0, 0] qError = [0, 0, 0, 0, 0, 0, 0]
qIndex = 7 qIndex = 7
qdotIndex = 7 qdotIndex = 7
@@ -33,7 +37,10 @@ class PDControllerExplicitMultiDof(object):
qIndex += 1 qIndex += 1
qdotIndex += 1 qdotIndex += 1
if len(js[0]) == 4: if len(js[0]) == 4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] desiredPos = [
desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredPositions[qIndex + 3]
]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos) axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0] jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew qdot1 += jointVelNew
@@ -41,7 +48,10 @@ class PDControllerExplicitMultiDof(object):
qError.append(axis[1]) qError.append(axis[1])
qError.append(axis[2]) qError.append(axis[2])
qError.append(0) qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]] desiredVel = [
desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
desiredVelocities[qdotIndex + 2]
]
zeroAccelerations += [0., 0., 0., 0.] zeroAccelerations += [0., 0., 0., 0.]
qIndex += 4 qIndex += 4
qdotIndex += 4 qdotIndex += 4
@@ -60,12 +70,13 @@ class PDControllerExplicitMultiDof(object):
return forces return forces
class PDControllerExplicit(object): class PDControllerExplicit(object):
def __init__(self, pb): def __init__(self, pb):
self._pb = pb self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId) numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices) jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = [] q1 = []
@@ -85,5 +96,3 @@ class PDControllerExplicit(object):
maxF = np.array(maxForces) maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF) forces = np.clip(forces, -maxF, maxF)
return forces return forces

30
examples/pybullet/examples/pdControllerStable.py
View File

@@ -1,12 +1,13 @@
import numpy as np import numpy as np
class PDControllerStableMultiDof(object): class PDControllerStableMultiDof(object):
def __init__(self, pb): def __init__(self, pb):
self._pb = pb self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId) numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId) curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
@@ -16,7 +17,9 @@ class PDControllerStableMultiDof(object):
#qdot1 = [0,0,0, 0,0,0,0] #qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId) baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0] qdot1 = [
baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
]
qError = [0, 0, 0, 0, 0, 0, 0] qError = [0, 0, 0, 0, 0, 0, 0]
qIndex = 7 qIndex = 7
@@ -39,7 +42,10 @@ class PDControllerStableMultiDof(object):
qIndex += 1 qIndex += 1
qdotIndex += 1 qdotIndex += 1
if len(js[0]) == 4: if len(js[0]) == 4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] desiredPos = [
desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredPositions[qIndex + 3]
]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos) axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0] jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew qdot1 += jointVelNew
@@ -47,7 +53,10 @@ class PDControllerStableMultiDof(object):
qError.append(axis[1]) qError.append(axis[1])
qError.append(axis[2]) qError.append(axis[2])
qError.append(0) qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]] desiredVel = [
desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
desiredVelocities[qdotIndex + 2]
]
zeroAccelerations += [0., 0., 0., 0.] zeroAccelerations += [0., 0., 0., 0.]
qIndex += 4 qIndex += 4
qdotIndex += 4 qdotIndex += 4
@@ -58,7 +67,6 @@ class PDControllerStableMultiDof(object):
qdotdesired = np.array(desiredVelocities) qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired - qdot qdoterr = qdotdesired - qdot
Kp = np.diagflat(kps) Kp = np.diagflat(kps)
Kd = np.diagflat(kds) Kd = np.diagflat(kds)
@@ -72,10 +80,8 @@ class PDControllerStableMultiDof(object):
#np.savetxt("pb_d.csv", d, delimiter=",") #np.savetxt("pb_d.csv", d, delimiter=",")
#np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",") #np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1, flags=1) M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1, flags=1)
M2 = np.array(M1) M2 = np.array(M1)
#np.savetxt("M2.csv", M2, delimiter=",") #np.savetxt("M2.csv", M2, delimiter=",")
@@ -83,11 +89,8 @@ class PDControllerStableMultiDof(object):
#np.savetxt("pbM_tKd.csv",M, delimiter=",") #np.savetxt("pbM_tKd.csv",M, delimiter=",")
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1) c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
c = np.array(c1) c = np.array(c1)
#np.savetxt("pbC.csv",c, delimiter=",") #np.savetxt("pbC.csv",c, delimiter=",")
A = M A = M
@@ -102,12 +105,13 @@ class PDControllerStableMultiDof(object):
return forces return forces
class PDControllerStable(object): class PDControllerStable(object):
def __init__(self, pb): def __init__(self, pb):
self._pb = pb self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId) numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices) jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = [] q1 = []

53
examples/pybullet/examples/pointCloudFromCameraImage.py
View File

@@ -1,20 +1,23 @@
import pybullet as p import pybullet as p
import math import math
import numpy as np import numpy as np
p.connect(p.GUI) p.connect(p.GUI)
plane = p.loadURDF("plane100.urdf") plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf", [0, 0, 1]) cube = p.loadURDF("cube.urdf", [0, 0, 1])
def getRayFromTo(mouseX, mouseY): def getRayFromTo(mouseX, mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]) lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
invLen = farPlane * 1. / lenFwd invLen = farPlane * 1. / lenFwd
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos rayFrom = camPos
@@ -23,24 +26,34 @@ def getRayFromTo(mouseX,mouseY):
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight] dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayToCenter = [
ortho=[- 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
]
ortho = [
-0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] - float(mouseY) * dVer[0],
-0.5 * horizon[1] + 0.5 * vertical[1] + float(mouseX) * dHor[1] - float(mouseY) * dVer[1], -0.5 * horizon[1] + 0.5 * vertical[1] + float(mouseX) * dHor[1] - float(mouseY) * dVer[1],
- 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] -0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
rayTo = [rayFrom[0]+rayForward[0] +ortho[0], rayTo = [
rayFrom[1]+rayForward[1] +ortho[1], rayFrom[0] + rayForward[0] + ortho[0], rayFrom[1] + rayForward[1] + ortho[1],
rayFrom[2]+rayForward[2] +ortho[2]] rayFrom[2] + rayForward[2] + ortho[2]
]
lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2]) lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2])
alpha = math.atan(lenOrtho / farPlane) alpha = math.atan(lenOrtho / farPlane)
return rayFrom, rayTo, alpha return rayFrom, rayTo, alpha
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]] )
camPos = [
camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
camTarget[2] - dist * camForward[2]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]) lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
rayForward[2] * rayForward[2])
oneOverWidth = float(1) / float(width) oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height) oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
@@ -103,7 +116,11 @@ for w in range(0,imgW,stepX):
depth /= math.cos(alpha) depth /= math.cos(alpha)
newTo = (depth / l) * vec + rf newTo = (depth / l) * vec + rf
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0]) p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
mb = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = newTo, useMaximalCoordinates=True) mb = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=newTo,
useMaximalCoordinates=True)
color = rgbBuffer[h, w] color = rgbBuffer[h, w]
color = [color[0] / 255., color[1] / 255., color[2] / 255., 1] color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.changeVisualShape(mb, -1, rgbaColor=color) p.changeVisualShape(mb, -1, rgbaColor=color)
@@ -117,7 +134,3 @@ print("ready\n")
#p.removeBody(cube) #p.removeBody(cube)
while (1): while (1):
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)

1
examples/pybullet/examples/profileTiming.py
View File

@@ -9,4 +9,3 @@ logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
while (time.time() < t): while (time.time() < t):
p.submitProfileTiming("pythontest") p.submitProfileTiming("pythontest")
p.stopStateLogging(logId) p.stopStateLogging(logId)

8
examples/pybullet/examples/projective_texture.py
View File

@@ -16,7 +16,6 @@ textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1 useRealTimeSimulation = 1
if (useRealTimeSimulation): if (useRealTimeSimulation):
@@ -29,7 +28,12 @@ while 1:
projMat = camera[3] projMat = camera[3]
#An example of setting the view matrix for the projective texture. #An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1]) #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat) p.getCameraImage(300,
300,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
flags=p.ER_USE_PROJECTIVE_TEXTURE,
projectiveTextureView=viewMat,
projectiveTextureProj=projMat)
p.setGravity(0, 0, 0) p.setGravity(0, 0, 0)
else: else:
p.stepSimulation() p.stepSimulation()

339
examples/pybullet/examples/quadruped.py
View File

@@ -12,9 +12,9 @@ def drawInertiaBox(parentUid, parentLinkIndex, color):
Ixx = inertia[0] Ixx = inertia[0]
Iyy = inertia[1] Iyy = inertia[1]
Izz = inertia[2] Izz = inertia[2]
boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass); boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass); boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass); boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ] halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]], pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
@@ -26,21 +26,80 @@ def drawInertiaBox(parentUid, parentLinkIndex, color):
[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[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[0],
p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[4],
p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 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[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4 + 0],
p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[4 + 1],
p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 1,
parentObjectUniqueId=parentUid,
p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4 + 1],
p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[4 + 3],
p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 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 toeConstraint = True
@@ -64,7 +123,6 @@ kp = 1
kd = .5 kd = .5
maxKneeForce = 1000 maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId < 0): if (physId < 0):
p.connect(p.GUI) p.connect(p.GUI)
@@ -74,7 +132,10 @@ angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0]) orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn) p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000) p.setTimeOut(4000000)
p.setGravity(0, 0, 0) p.setGravity(0, 0, 0)
@@ -82,7 +143,11 @@ p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4]) orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_USE_IMPLICIT_CYLINDER) quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
@@ -90,7 +155,6 @@ for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i) jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link'] knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
@@ -116,9 +180,6 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0]) #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] motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
@@ -139,34 +200,48 @@ p.changeDynamics(quadruped,-1,localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints): #for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001]) # p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0]) drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0]) #drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range(nJoints): for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0]) drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates): if (useMaximalCoordinates):
steps = 400 steps = 400
for aa in range(steps): 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_leftL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps) motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps) motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps) motordir[2] * 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_leftR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps) 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_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps) p.setJointMotorControl2(quadruped, knee_front_leftL_link, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps) motordir[0] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps) p.setJointMotorControl2(quadruped, knee_front_leftR_link, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps) motordir[1] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps) p.setJointMotorControl2(quadruped, knee_back_leftL_link, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps) motordir[2] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps) p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_back_rightR_link,p.POSITION_CONTROL,motordir[7]*kneeangle*float(aa)/steps) motordir[3] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL,
motordir[4] * (kneeangle) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL,
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL,
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL,
motordir[7] * kneeangle * float(aa) / steps)
p.stepSimulation() p.stepSimulation()
#time.sleep(fixedTimeStep) #time.sleep(fixedTimeStep)
@@ -194,58 +269,108 @@ else:
#p.getNumJoints(1) #p.getNumJoints(1)
if (toeConstraint): if (toeConstraint):
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce) p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce) p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce) p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_back_rightR_link, quadruped, knee_back_rightL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce) p.changeConstraint(cid, maxForce=maxKneeForce)
if (1): if (1):
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_rightL_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
legnumbering = [ legnumbering = [
motor_front_leftL_joint, motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
motor_front_leftR_joint, motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
motor_back_leftL_joint, motor_back_rightL_joint, motor_back_rightR_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): for i in range(8):
print(legnumbering[i]) print(legnumbering[i])
#use the Minitaur leg numbering #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,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) jointIndex=legnumbering[0],
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) controlMode=p.POSITION_CONTROL,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) targetPosition=motordir[0] * 1.57,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) positionGain=kp,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) velocityGain=kd,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*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 #stand still
p.setRealTimeSimulation(useRealTime) p.setRealTimeSimulation(useRealTime)
t = 0.0 t = 0.0
t_end = t + 15 t_end = t + 15
ref_time = time.time() ref_time = time.time()
@@ -264,11 +389,6 @@ print(quadruped)
p.saveWorld("quadru.py") p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped]) logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
#jump #jump
t = 0.0 t = 0.0
t_end = t + 100 t_end = t + 100
@@ -282,17 +402,64 @@ while (1):
t = t + fixedTimeStep t = t + fixedTimeStep
if (True): if (True):
target = math.sin(t*speed)*jump_amp+1.57; 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,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce) jointIndex=legnumbering[0],
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce) controlMode=p.POSITION_CONTROL,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce) targetPosition=motordir[0] * target,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce) positionGain=kp,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce) velocityGain=kd,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce) force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*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): if (useRealTime == 0):
p.stepSimulation() p.stepSimulation()
time.sleep(fixedTimeStep) time.sleep(fixedTimeStep)

80
examples/pybullet/examples/quadruped_playback.py
View File

@@ -10,6 +10,7 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose=True): def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
@@ -52,9 +53,10 @@ def readLogFile(filename, verbose = True):
return log return log
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
log = readLogFile("LOG00076.TXT"); log = readLogFile("LOG00076.TXT")
recordNum = len(log) recordNum = len(log)
print('record num:'), print('record num:'),
@@ -72,8 +74,6 @@ maxForce = 3.5
kp = .05 kp = .05
kd = .5 kd = .5
quadruped = 1 quadruped = 1
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
@@ -106,21 +106,71 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
motorDir = [1, 1, 1, 1, 1, 1, 1, 1]; motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] legnumbering = [
motor_front_leftR_joint, motor_front_leftL_joint, motor_back_leftR_joint,
motor_back_leftL_joint, motor_front_rightR_joint, motor_front_rightL_joint,
motor_back_rightR_joint, motor_back_rightL_joint
]
for record in log: for record in log:
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce) jointIndex=legnumbering[0],
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce) controlMode=p.POSITION_CONTROL,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce) targetPosition=motorDir[0] * record[7],
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce) positionGain=kp,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce) velocityGain=kd,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce) force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[1] * record[8],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[2] * record[9],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[3] * record[10],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[4] * record[11],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[5] * record[12],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[6] * record[13],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[7] * record[14],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setGravity(0.000000, 0.000000, -10.000000) p.setGravity(0.000000, 0.000000, -10.000000)
p.stepSimulation() p.stepSimulation()
p.stepSimulation() p.stepSimulation()
sleep(0.01) sleep(0.01)

36
examples/pybullet/examples/quadruped_setup_playback.py
View File

@@ -1,20 +1,42 @@
import pybullet as p import pybullet as p
p.connect(p.SHARED_MEMORY) p.connect(p.SHARED_MEMORY)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,-.300000,0.000000,0.000000,0.000000,1.000000)] objects = [
objects = [p.loadURDF("quadruped/minitaur.urdf", [-0.000046,-0.000068,0.200774],[-0.000701,0.000387,-0.000252,1.000000],useFixedBase=False)] p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000)
]
objects = [
p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774],
[-0.000701, 0.000387, -0.000252, 1.000000],
useFixedBase=False)
]
ob = objects[0] ob = objects[0]
jointPositions=[ 0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000, -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193, 0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517 ] jointPositions = [
0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)): for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji]) p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1,3,1,6,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000) p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1,16,1,19,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000) p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1,9,1,12,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid2, maxForce=500.000000) p.changeConstraint(cid2, maxForce=500.000000)
cid3 = p.createConstraint(1,22,1,25,p.JOINT_POINT2POINT,[0.000000,0.000000,0.000000],[0.000000,0.005000,0.200000],[0.000000,0.010000,0.200000],[0.000000,0.000000,0.000000,1.000000],[0.000000,0.000000,0.000000,1.000000]) cid3 = p.createConstraint(1, 22, 1, 25, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid3, maxForce=500.000000) p.changeConstraint(cid3, maxForce=500.000000)
p.setGravity(0.000000, 0.000000, 0.000000) p.setGravity(0.000000, 0.000000, 0.000000)
p.stepSimulation() p.stepSimulation()

6
examples/pybullet/examples/racecar.py
View File

@@ -47,7 +47,11 @@ while (True):
#print(targetVelocity) #print(targetVelocity)
for wheel in wheels: for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=maxForce)
for steer in steering: for steer in steering:
p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle) p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle)

80
examples/pybullet/examples/racecar_differential.py
View File

@@ -26,31 +26,85 @@ wheels = [8,15]
print("----------------") print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
9,
car,
11,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000) p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
10,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
9,
car,
13,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
16,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000) p.changeConstraint(c, gearRatio=1, maxForce=10000)
c = p.createConstraint(car,
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) 16,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
17,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000) p.changeConstraint(c, gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
1,
car,
18,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000) p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
3,
car,
19,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000) p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, maxForce=10000)
steering = [0, 2] steering = [0, 2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0) targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
@@ -63,7 +117,11 @@ while (True):
#print(targetVelocity) #print(targetVelocity)
for wheel in wheels: for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) p.setJointMotorControl2(car,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=maxForce)
for steer in steering: for steer in steering:
p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle) p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle)

4
examples/pybullet/examples/renderPlugin.py
View File

@@ -1,7 +1,7 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin") plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
"_testPlugin")
print("plugin=", plugin) print("plugin=", plugin)
p.loadURDF("r2d2.urdf") p.loadURDF("r2d2.urdf")

26
examples/pybullet/examples/rendertest.py
View File

@@ -29,6 +29,7 @@ import matplotlib.pyplot as plt
class BulletSim(): class BulletSim():
def __init__(self, connection_mode, *argv): def __init__(self, connection_mode, *argv):
self.connection_mode = connection_mode self.connection_mode = connection_mode
self.argv = argv self.argv = argv
@@ -57,6 +58,7 @@ class BulletSim():
def __exit__(self, *_, **__): def __exit__(self, *_, **__):
pybullet.disconnect() pybullet.disconnect()
def test(num_runs=300, shadow=1, log=True, plot=False): def test(num_runs=300, shadow=1, log=True, plot=False):
if log: if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings") logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
@@ -69,17 +71,21 @@ def test(num_runs=300, shadow=1, log=True, plot=False):
image = plt.imshow(img, interpolation='none', animated=True, label="blah") image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca() ax = plt.gca()
times = np.zeros(num_runs) times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0, 360, 10)) yaw_gen = itertools.cycle(range(0, 360, 10))
for i, yaw in zip(range(num_runs), yaw_gen): for i, yaw in zip(range(num_runs), yaw_gen):
pybullet.stepSimulation() pybullet.stepSimulation()
start = time.time() start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
aspect = pixelWidth / pixelHeight; roll, upAxisIndex)
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); aspect = pixelWidth / pixelHeight
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
projectionMatrix, shadow=shadow,lightDirection=[1,1,1], img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=shadow,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER) #renderer=pybullet.ER_TINY_RENDERER)
stop = time.time() stop = time.time()
@@ -110,10 +116,8 @@ def test(num_runs=300, shadow=1, log=True, plot=False):
return mean_time return mean_time
if __name__ == "__main__": if __name__ == "__main__":
res = [] res = []
with BulletSim(pybullet.DIRECT): with BulletSim(pybullet.DIRECT):
@@ -121,9 +125,10 @@ if __name__ == "__main__":
mean_time = test(log=False, plot=True) mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time)) res.append(("tiny", mean_time))
with BulletSim(pybullet.DIRECT): with BulletSim(pybullet.DIRECT):
plugin_fn = os.path.join(pybullet.__file__.split("bullet3")[0],"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so") plugin_fn = os.path.join(
pybullet.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin") plugin = pybullet.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0: if plugin < 0:
print("\nPlugin Failed to load!\n") print("\nPlugin Failed to load!\n")
@@ -143,4 +148,3 @@ if __name__ == "__main__":
print("back nenv fps fps_tot") print("back nenv fps fps_tot")
for r in res: for r in res:
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1])) print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))

29
examples/pybullet/examples/rendertest_sync.py
View File

@@ -30,7 +30,9 @@ fov = 60
class TestEnv(gym.Env): class TestEnv(gym.Env):
def __init__(self,
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug') renderer='tiny', # ('tiny', 'egl', 'debug')
): ):
self.action_space = spaces.Discrete(2) self.action_space = spaces.Discrete(2)
@@ -47,7 +49,9 @@ class TestEnv(gym.Env):
p.connect(p.DIRECT, options=optionstring) p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin": if self._renderer == "plugin":
plugin_fn = os.path.join(p.__file__.split("bullet3")[0],"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so") plugin_fn = os.path.join(
p.__file__.split("bullet3")[0],
"bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so")
plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin") plugin = p.loadPlugin(plugin_fn, "_tinyRendererPlugin")
if plugin < 0: if plugin < 0:
print("\nPlugin Failed to load! Try installing via `pip install -e .`\n") print("\nPlugin Failed to load! Try installing via `pip install -e .`\n")
@@ -71,7 +75,6 @@ class TestEnv(gym.Env):
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
def __del__(self): def __del__(self):
p.disconnect() p.disconnect()
@@ -82,11 +85,16 @@ class TestEnv(gym.Env):
p.stepSimulation() p.stepSimulation()
start = time.time() start = time.time()
yaw = next(self.iter) yaw = next(self.iter)
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
aspect = pixelWidth / pixelHeight; upAxisIndex)
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); aspect = pixelWidth / pixelHeight
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix, projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
projectionMatrix, shadow=1,lightDirection=[1,1,1], img_arr = p.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=p.ER_BULLET_HARDWARE_OPENGL) renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER) #renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2] self._observation = img_arr[2]
@@ -95,8 +103,11 @@ class TestEnv(gym.Env):
def seed(self, seed=None): def seed(self, seed=None):
pass pass
def train(env_id, num_timesteps=300, seed=0, num_env=2, renderer='tiny'): def train(env_id, num_timesteps=300, seed=0, num_env=2, renderer='tiny'):
def make_env(rank): def make_env(rank):
def _thunk(): def _thunk():
if env_id == "TestEnv": if env_id == "TestEnv":
env = TestEnv(renderer=renderer) #gym.make(env_id) env = TestEnv(renderer=renderer) #gym.make(env_id)
@@ -107,7 +118,9 @@ def train(env_id, num_timesteps=300, seed=0,num_env=2,renderer='tiny'):
gym.logger.setLevel(logging.WARN) gym.logger.setLevel(logging.WARN)
# only clip rewards when not evaluating # only clip rewards when not evaluating
return env return env
return _thunk return _thunk
set_global_seeds(seed) set_global_seeds(seed)
env = SubprocVecEnv([make_env(i) for i in range(num_env)]) env = SubprocVecEnv([make_env(i) for i in range(num_env)])

98
examples/pybullet/examples/reset_dynamic_info.py
View File

@@ -11,14 +11,17 @@ p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=1.0)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
def drawInertiaBox(parentUid, parentLinkIndex): def drawInertiaBox(parentUid, parentLinkIndex):
mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA) mass, frictionCoeff, inertia = p.getDynamicsInfo(bodyUniqueId=parentUid,
linkIndex=parentLinkIndex,
flags=p.DYNAMICS_INFO_REPORT_INERTIA)
Ixx = inertia[0] Ixx = inertia[0]
Iyy = inertia[1] Iyy = inertia[1]
Izz = inertia[2] Izz = inertia[2]
boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass); boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass); boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass); boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ] halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]], pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
@@ -31,22 +34,80 @@ def drawInertiaBox(parentUid, parentLinkIndex):
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]] [-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
color = [1, 0, 0] color = [1, 0, 0]
p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[0],
p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[1],
p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 1,
parentObjectUniqueId=parentUid,
p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],
p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[3],
p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
1,
p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentObjectUniqueId=parentUid,
p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],
p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 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)
drawInertiaBox(cubeId, -1) drawInertiaBox(cubeId, -1)
@@ -59,7 +120,6 @@ while 1:
mass1, frictionCoeff1 = p.getDynamicsInfo(bodyUniqueId=planeId, linkIndex=-1) mass1, frictionCoeff1 = p.getDynamicsInfo(bodyUniqueId=planeId, linkIndex=-1)
mass2, frictionCoeff2 = p.getDynamicsInfo(bodyUniqueId=cubeId, linkIndex=-1) mass2, frictionCoeff2 = p.getDynamicsInfo(bodyUniqueId=cubeId, linkIndex=-1)
print(mass1, frictionCoeff1) print(mass1, frictionCoeff1)
print(mass2, frictionCoeff2) print(mass2, frictionCoeff2)
time.sleep(1. / 240.) time.sleep(1. / 240.)

2
examples/pybullet/examples/robotcontrol.py
View File

@@ -26,5 +26,3 @@ for step in range (400):
p.getContactPoints(husky) p.getContactPoints(husky)
p.disconnect() p.disconnect()

1
examples/pybullet/examples/rollPitchYaw.py
View File

@@ -23,4 +23,3 @@ while True:
orn = p.getQuaternionFromEuler([roll, pitch, yaw]) orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn) p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used #p.stepSimulation()#not really necessary for this demo, no physics used

6
examples/pybullet/examples/satCollision.py
View File

@@ -4,7 +4,10 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableSAT=1) p.setPhysicsEngineParameter(enableSAT=1)
p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("cube_concave.urdf", [0, 0, -25],
globalScaling=50,
useFixedBase=True,
flags=p.URDF_INITIALIZE_SAT_FEATURES)
p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("cube.urdf", [0, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
@@ -13,4 +16,3 @@ while (p.isConnected()):
pts = p.getContactPoints() pts = p.getContactPoints()
#print("num contacts = ", len(pts)) #print("num contacts = ", len(pts))
time.sleep(1. / 240.) time.sleep(1. / 240.)

7
examples/pybullet/examples/saveRestoreState.py
View File

@@ -10,6 +10,7 @@ verbose = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
def setupWorld(): def setupWorld():
p.resetSimulation() p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
@@ -37,6 +38,7 @@ def dumpStateToFile(file):
file.write(txtLinVel) file.write(txtLinVel)
file.write(txtAngVel) file.write(txtAngVel)
def compareFiles(file1, file2): def compareFiles(file1, file2):
diff = difflib.unified_diff( diff = difflib.unified_diff(
file1.readlines(), file1.readlines(),
@@ -53,6 +55,7 @@ def compareFiles(file1,file2):
else: else:
print("OK, files are identical") print("OK, files are identical")
setupWorld() setupWorld()
for i in range(numSteps): for i in range(numSteps):
p.stepSimulation() p.stepSimulation()
@@ -69,7 +72,6 @@ if verbose:
for i in range(numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("saveFile.txt", "w") file = open("saveFile.txt", "w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
@@ -96,7 +98,6 @@ if verbose:
for i in range(numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("restoreFile.txt", "w") file = open("restoreFile.txt", "w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
@@ -113,7 +114,6 @@ if verbose:
for i in range(numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("restoreFile2.txt", "w") file = open("restoreFile2.txt", "w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
@@ -134,4 +134,3 @@ p.stopStateLogging(logId)
#while (p.getConnectionInfo()["isConnected"]): #while (p.getConnectionInfo()["isConnected"]):
# time.sleep(1) # time.sleep(1)

1
examples/pybullet/examples/segmask_linkindex.py
View File

@@ -38,5 +38,4 @@ while (1):
linkIndex = (pixel >> 24) - 1 linkIndex = (pixel >> 24) - 1
print("obUid=", obUid, "linkIndex=", linkIndex) print("obUid=", obUid, "linkIndex=", linkIndex)
p.stepSimulation() p.stepSimulation()

20
examples/pybullet/examples/shiftCenterOfMass.py
View File

@@ -12,11 +12,23 @@ collisionShift = [0,0,0]
inertiaShift = [0, 0, -0.5] inertiaShift = [0, 0, -0.5]
meshScale = [1, 1, 1] meshScale = [1, 1, 1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale) fileName="cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=visualShift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=collisionShift,
meshScale=meshScale)
p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False) p.createMultiBody(baseMass=1,
baseInertialFramePosition=inertiaShift,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 1],
useMaximalCoordinates=False)
while (1): while (1):
p.stepSimulation() p.stepSimulation()
time.sleep(1. / 240.) time.sleep(1. / 240.)

1
examples/pybullet/examples/signedDistanceField.py
View File

@@ -12,4 +12,3 @@ p.setTimeStep(timeStep)
while (1): while (1):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

6
examples/pybullet/examples/sleeping.py
View File

@@ -12,12 +12,13 @@ p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordina
r2d2 = -1 r2d2 = -1
for k in range(5): for k in range(5):
for i in range(5): for i in range(5):
r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags) r2d2 = p.loadURDF("r2d2.urdf", [k * 2, i * 2, 1],
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards #enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING) #p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range(p.getNumJoints(r2d2)): for j in range(p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0) p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
print("r2d2=", r2d2) print("r2d2=", r2d2)
@@ -31,4 +32,3 @@ while p.isConnected():
time.sleep(timestep) time.sleep(timestep)
#force the object to wake up #force the object to wake up
p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP) p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)

32
examples/pybullet/examples/snake.py
View File

@@ -2,11 +2,9 @@ import pybullet as p
import time import time
import math import math
# This simple snake logic is from some 15 year old Havok C++ demo # This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert! # Thanks to Michael Ewert!
p.connect(p.GUI) p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE) plane = p.createCollisionShape(p.GEOM_PLANE)
@@ -15,7 +13,8 @@ p.createMultiBody(0,plane)
useMaximalCoordinates = True useMaximalCoordinates = True
sphereRadius = 0.25 sphereRadius = 0.25
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) #colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
@@ -45,7 +44,22 @@ for i in range (36):
basePosition = [0, 0, 1] basePosition = [0, 0, 1]
baseOrientation = [0, 0, 0, 1] baseOrientation = [0, 0, 0, 1]
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis, useMaximalCoordinates=useMaximalCoordinates) sphereUid = p.createMultiBody(mass,
colBoxId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
@@ -115,12 +129,14 @@ while (1):
targetPos *= 1.0 / (1.0 - m_steering) targetPos *= 1.0 / (1.0 - m_steering)
#set our motor #set our motor
p.setJointMotorControl2(sphereUid,joint,p.POSITION_CONTROL,targetPosition=targetPos+m_steering,force=30) p.setJointMotorControl2(sphereUid,
joint,
p.POSITION_CONTROL,
targetPosition=targetPos + m_steering,
force=30)
#wave keeps track of where the wave is in time #wave keeps track of where the wave is in time
m_waveFront += dt/m_wavePeriod * m_waveLength; m_waveFront += dt / m_wavePeriod * m_waveLength
p.stepSimulation() p.stepSimulation()
time.sleep(dt) time.sleep(dt)

5
examples/pybullet/examples/switchConstraintSolver.py
View File

@@ -3,10 +3,10 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001) p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG,
globalCFM=0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
radius = 0.025 radius = 0.025
distance = 1.86 distance = 1.86
@@ -28,4 +28,3 @@ p.setGravity(0,0,-10)
while (p.isConnected()): while (p.isConnected()):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

5
examples/pybullet/examples/test.py
View File

@@ -6,7 +6,6 @@ print ("current_dir=" + currentdir)
parentdir = os.path.join(currentdir, "../gym") parentdir = os.path.join(currentdir, "../gym")
os.sys.path.insert(0, parentdir) os.sys.path.insert(0, parentdir)
import pybullet import pybullet
import pybullet_data import pybullet_data
@@ -21,7 +20,8 @@ obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"))
posX = 0 posX = 0
posY = 3 posY = 3
posZ = 2 posZ = 2
obj2 = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"kuka_iiwa/model.urdf"),posX,posY,posZ) obj2 = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(), "kuka_iiwa/model.urdf"), posX,
posY, posZ)
#query the number of joints of the object #query the number of joints of the object
numJoints = pybullet.getNumJoints(obj) numJoints = pybullet.getNumJoints(obj)
@@ -44,4 +44,3 @@ pybullet.resetSimulation()
#disconnect from the physics server #disconnect from the physics server
pybullet.disconnect() pybullet.disconnect()

4
examples/pybullet/examples/testPlugin.py
View File

@@ -1,7 +1,7 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll","_testPlugin") plugin = p.loadPlugin("D:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll",
"_testPlugin")
print("plugin=", plugin) print("plugin=", plugin)
p.executePluginCommand(plugin, "r2d2.urdf", [1, 2, 3], [50.0, 3.3]) p.executePluginCommand(plugin, "r2d2.urdf", [1, 2, 3], [50.0, 3.3])

19
examples/pybullet/examples/testrender.py
View File

@@ -1,7 +1,5 @@
#testrender.py is a bit slower than testrender_np.py: pixels are copied from C to Python one by one #testrender.py is a bit slower than testrender_np.py: pixels are copied from C to Python one by one
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import pybullet import pybullet
import time import time
@@ -14,7 +12,6 @@ img = [[1,2,3]*50]*100#np.random.rand(200, 320)
image = plt.imshow(img, interpolation='none', animated=True, label="blah") image = plt.imshow(img, interpolation='none', animated=True, label="blah")
ax = plt.gca() ax = plt.gca()
pybullet.connect(pybullet.DIRECT) pybullet.connect(pybullet.DIRECT)
#pybullet.loadPlugin("eglRendererPlugin") #pybullet.loadPlugin("eglRendererPlugin")
@@ -43,10 +40,17 @@ while(1):
for yaw in range(0, 360, 10): for yaw in range(0, 360, 10):
pybullet.stepSimulation() pybullet.stepSimulation()
start = time.time() start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
aspect = pixelWidth / pixelHeight; roll, upAxisIndex)
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); aspect = pixelWidth / pixelHeight
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time() stop = time.time()
print("renderImage %f" % (stop - start)) print("renderImage %f" % (stop - start))
@@ -61,7 +65,6 @@ while(1):
#plt.imshow(rgb,interpolation='none') #plt.imshow(rgb,interpolation='none')
#reshape is needed #reshape is needed
np_img_arr = np.reshape(rgb, (h, w, 4)) np_img_arr = np.reshape(rgb, (h, w, 4))
np_img_arr = np_img_arr * (1. / 255.) np_img_arr = np_img_arr * (1. / 255.)

16
examples/pybullet/examples/testrender_egl.py
View File

@@ -51,10 +51,17 @@ while (1):
pybullet.stepSimulation() pybullet.stepSimulation()
start = time.time() start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
aspect = pixelWidth / pixelHeight; roll, upAxisIndex)
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); aspect = pixelWidth / pixelHeight
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time() stop = time.time()
print("renderImage %f" % (stop - start)) print("renderImage %f" % (stop - start))
@@ -82,7 +89,6 @@ while (1):
plt.pause(0.01) plt.pause(0.01)
#image.draw() #image.draw()
main_stop = time.time() main_stop = time.time()
print("Total time %f" % (main_stop - main_start)) print("Total time %f" % (main_stop - main_start))

18
examples/pybullet/examples/testrender_np.py
View File

@@ -1,4 +1,3 @@
#make sure to compile pybullet with PYBULLET_USE_NUMPY enabled #make sure to compile pybullet with PYBULLET_USE_NUMPY enabled
#otherwise use testrender.py (slower but compatible without numpy) #otherwise use testrender.py (slower but compatible without numpy)
#you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU) #you can also use GUI mode, for faster OpenGL rendering (instead of TinyRender CPU)
@@ -8,7 +7,6 @@ import matplotlib.pyplot as plt
import pybullet import pybullet
import time import time
plt.ion() plt.ion()
img = np.random.rand(200, 320) img = np.random.rand(200, 320)
@@ -44,10 +42,17 @@ while (1):
pybullet.stepSimulation() pybullet.stepSimulation()
start = time.time() start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
aspect = pixelWidth / pixelHeight; roll, upAxisIndex)
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); aspect = pixelWidth / pixelHeight
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, shadow=1,lightDirection=[1,1,1],renderer=pybullet.ER_BULLET_HARDWARE_OPENGL) projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
stop = time.time() stop = time.time()
print("renderImage %f" % (stop - start)) print("renderImage %f" % (stop - start))
@@ -75,7 +80,6 @@ while (1):
plt.pause(0.01) plt.pause(0.01)
#image.draw() #image.draw()
main_stop = time.time() main_stop = time.time()
print("Total time %f" % (main_stop - main_start)) print("Total time %f" % (main_stop - main_start))

5
examples/pybullet/examples/transparent.py
View File

@@ -15,5 +15,8 @@ while (1):
blue = p.readUserDebugParameter(blueSlider) blue = p.readUserDebugParameter(blueSlider)
alpha = p.readUserDebugParameter(alphaSlider) alpha = p.readUserDebugParameter(alphaSlider)
p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha]) p.changeVisualShape(sphereUid, -1, rgbaColor=[red, green, blue, alpha])
p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(320,
200,
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
time.sleep(0.01) time.sleep(0.01)

17
examples/pybullet/examples/urdfEditor.py
View File

@@ -2,10 +2,6 @@ import pybullet as p
import time import time
from pybullet_utils import urdfEditor from pybullet_utils import urdfEditor
########################################## ##########################################
org2 = p.connect(p.DIRECT) org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY) org = p.connect(p.SHARED_MEMORY)
@@ -18,8 +14,6 @@ p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf #door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org) mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb, physicsClientId=org)): for i in range(p.getNumJoints(mb, physicsClientId=org)):
p.setJointMotorControl2(mb, i, p.VELOCITY_CONTROL, force=0, physicsClientId=org) p.setJointMotorControl2(mb, i, p.VELOCITY_CONTROL, force=0, physicsClientId=org)
@@ -47,18 +41,19 @@ if (1):
parser2.initializeFromBulletBody(obUid, physicsClientId=gui) parser2.initializeFromBulletBody(obUid, physicsClientId=gui)
parser2.saveUrdf("test2.urdf") parser2.saveUrdf("test2.urdf")
for i in range(p.getNumJoints(obUid, physicsClientId=gui)): for i in range(p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui) p.setJointMotorControl2(obUid,
i,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=1,
physicsClientId=gui)
print(p.getJointInfo(obUid, i, physicsClientId=gui)) print(p.getJointInfo(obUid, i, physicsClientId=gui))
parser = 0 parser = 0
p.setRealTimeSimulation(1, physicsClientId=gui) p.setRealTimeSimulation(1, physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]): while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui) p.stepSimulation(physicsClientId=gui)
time.sleep(0.01) time.sleep(0.01)

9
examples/pybullet/examples/userData.py
View File

@@ -2,15 +2,12 @@ import pybullet as pb
import time import time
from pybullet_utils import bullet_client from pybullet_utils import bullet_client
server = bullet_client.BulletClient(connection_mode=pb.SHARED_MEMORY_SERVER) server = bullet_client.BulletClient(connection_mode=pb.SHARED_MEMORY_SERVER)
print("Connecting to bullet server") print("Connecting to bullet server")
CONNECTION_METHOD = pb.SHARED_MEMORY CONNECTION_METHOD = pb.SHARED_MEMORY
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
PLANE_PATH = "plane.urdf" PLANE_PATH = "plane.urdf"
client.loadURDF(PLANE_PATH) client.loadURDF(PLANE_PATH)
@@ -52,19 +49,18 @@ client.addUserData(plane_id, "MyKey1", "MyNewValue")
print("Cached overridden data") print("Cached overridden data")
print(client.getUserData(MyKey1)) print(client.getUserData(MyKey1))
print("Disconnecting") print("Disconnecting")
del client del client
print("Reconnecting") print("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD) client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print("Synced overridden data") print("Synced overridden data")
print(client.getUserData(MyKey1)) print(client.getUserData(MyKey1))
print("Getting user data ID") print("Getting user data ID")
print ("Retrieved ID: %s, ID retrieved from addUserData: %s" % (client.getUserDataId(plane_id, "MyKey2"), MyKey2)) print("Retrieved ID: %s, ID retrieved from addUserData: %s" %
(client.getUserDataId(plane_id, "MyKey2"), MyKey2))
print("Removing user data") print("Removing user data")
client.removeUserData(MyKey2) client.removeUserData(MyKey2)
@@ -107,4 +103,3 @@ print ("Retrieving user data")
print(client.getUserData(MyKey1)) print(client.getUserData(MyKey1))
print(client.getUserData(MyKey3)) print(client.getUserData(MyKey3))
print(client.getUserData(MyKey4)) print(client.getUserData(MyKey4))

8
examples/pybullet/examples/vrEvent.py
View File

@@ -78,7 +78,11 @@ while True:
lenSqr = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2] lenSqr = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]
ptDistThreshold = 0.01 ptDistThreshold = 0.01
if (lenSqr > (ptDistThreshold * ptDistThreshold)): if (lenSqr > (ptDistThreshold * ptDistThreshold)):
p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]]) p.addUserDebugLine(e[POSITION], prevPosition[e[CONTROLLER_ID]], colors[e[CONTROLLER_ID]],
widths[e[CONTROLLER_ID]])
#p.loadURDF("cube_small.urdf",e[1]) #p.loadURDF("cube_small.urdf",e[1])
colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]] colors[e[CONTROLLER_ID]] = [
1 - colors[e[CONTROLLER_ID]][0], 1 - colors[e[CONTROLLER_ID]][1],
1 - colors[e[CONTROLLER_ID]][2]
]
prevPosition[e[CONTROLLER_ID]] = e[POSITION] prevPosition[e[CONTROLLER_ID]] = e[POSITION]

161
examples/pybullet/examples/vr_kitchen_setup_vrSyncPython.py
View File

@@ -2,22 +2,28 @@ import pybullet as p
import time import time
#p.connect(p.UDP,"192.168.86.100") #p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
meshScale = [.1, .1, .01] meshScale = [.1, .1, .01]
shift = [0, 0, 0] shift = [0, 0, 0]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="marble_cube.obj", rgbaColor=[1,1,1,1], specularColor=[1,1,1], visualFramePosition=shift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="marble_cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[1, 1, 1],
visualFramePosition=shift,
meshScale=meshScale)
#collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale) #collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
collisionShapeId = -1 collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,1,0], useMaximalCoordinates=True) uiCube = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 1, 0],
useMaximalCoordinates=True)
textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963]) textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963])
numLines = 1 numLines = 1
@@ -29,7 +35,10 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] #objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
kitchenObj = p.loadSDF("kitchens/1.sdf") kitchenObj = p.loadSDF("kitchens/1.sdf")
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] #objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
1.000000)
]
pr2_gripper = objects[0] pr2_gripper = objects[0]
print("pr2_gripper=") print("pr2_gripper=")
print(pr2_gripper) print(pr2_gripper)
@@ -39,55 +48,105 @@ for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex]) p.resetJointState(pr2_gripper, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(pr2_gripper, jointIndex, p.POSITION_CONTROL, targetPosition=0, force=0) p.setJointMotorControl2(pr2_gripper, jointIndex, p.POSITION_CONTROL, targetPosition=0, force=0)
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000]) pr2_cid = p.createConstraint(pr2_gripper, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.2, 0, 0],
[0.500000, 0.300006, 0.700000])
print("pr2_cid") print("pr2_cid")
print(pr2_cid) print(pr2_cid)
pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) pr2_cid2 = p.createConstraint(pr2_gripper,
0,
pr2_gripper,
2,
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(pr2_cid2, gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3) p.changeConstraint(pr2_cid2, gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3)
objects = [
p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000, -0.200000, 0.600000, 0.000000, 0.000000,
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)] 0.000000, 1.000000)
]
kuka = objects[0] kuka = objects[0]
jointPositions = [-0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001] jointPositions = [-0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001]
for jointIndex in range(p.getNumJoints(kuka)): for jointIndex in range(p.getNumJoints(kuka)):
p.resetJointState(kuka, jointIndex, jointPositions[jointIndex]) p.resetJointState(kuka, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0) p.setJointMotorControl2(kuka, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex], 0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)] objects = [
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)] p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.700000, 0.000000, 0.000000, 0.000000,
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)] 1.000000)
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.800000, 0.000000, 0.000000, 0.000000,
1.000000)
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.900000, 0.000000, 0.000000, 0.000000,
1.000000)
]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf") objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0] kuka_gripper = objects[0]
print("kuka gripper=") print("kuka gripper=")
print(kuka_gripper) print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970]) p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036],
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ] [-0.000000, 0.964531, -0.000002, -0.263970])
jointPositions = [
0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000
]
for jointIndex in range(p.getNumJoints(kuka_gripper)): for jointIndex in range(p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex]) p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0) p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex],
0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05],
[0, 0, 0])
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) objects = [
p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107,
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] 0.707107)
]
#objects = [p.loadURDF("textures/table2.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)] #objects = [p.loadURDF("textures/table2.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 0.707107)
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] ]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] p.loadURDF("jenga/jenga.urdf", 1.200000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] 0.707107)
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] ]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.100000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
0.707107)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 1.000000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
0.707107)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 0.900000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
0.707107)
]
objects = [
p.loadURDF("jenga/jenga.urdf", 0.800000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
0.707107)
]
#objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)] #objects = [p.loadURDF("teddy_vhacd.urdf", 1.050000,-0.500000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)] objects = [
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)] p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000, 0.000000, 0.707107,
objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.000000,0.707107,0.707107)] 0.707107)
]
objects = [
p.loadURDF("sphere_small.urdf", 0.850000, -0.400000, 0.700000, 0.000000, 0.000000, 0.707107,
0.707107)
]
objects = [
p.loadURDF("duck_vhacd.urdf", 0.850000, -0.400000, 0.900000, 0.000000, 0.000000, 0.707107,
0.707107)
]
#bjects = p.loadSDF("kiva_shelf/model.sdf") #bjects = p.loadSDF("kiva_shelf/model.sdf")
#ob = objects[0] #ob = objects[0]
#p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000]) #p.resetBasePositionAndOrientation(ob,[0.000000,1.000000,1.204500],[0.000000,0.000000,0.000000,1.000000])
@@ -108,7 +167,6 @@ objects = [p.loadURDF("duck_vhacd.urdf", 0.850000,-0.400000,0.900000,0.000000,0.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0, 0, -10) p.setGravity(0, 0, -10)
##show this for 10 seconds ##show this for 10 seconds
@@ -136,8 +194,6 @@ while (uiControllerId<0):
print("Using uiControllerId=" + str(uiControllerId)) print("Using uiControllerId=" + str(uiControllerId))
controllerId = -1 controllerId = -1
print("waiting for VR picking controller trigger") print("waiting for VR picking controller trigger")
@@ -164,7 +220,13 @@ if (once):
spacing = 0.01 spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011] textPos = [.1 - (i + 1) * spacing, .1, 0.011]
text = "ABCDEFGH\nIJKLMNOPQRSTUVWXYZ\n01234567890abcdefg\n" * 10 text = "ABCDEFGH\nIJKLMNOPQRSTUVWXYZ\n01234567890abcdefg\n" * 10
lines[i] = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.01, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1) lines[i] = p.addUserDebugText(text,
textColorRGB=[0, 0, 0],
textSize=0.01,
textPosition=textPos,
textOrientation=textOrn,
parentObjectUniqueId=uiCube,
parentLinkIndex=-1)
if (once): if (once):
once = 0 once = 0
@@ -179,15 +241,20 @@ while (1):
frameNr = frameNr + 1 frameNr = frameNr + 1
for i in range(numLines): for i in range(numLines):
spacing = 0.01 spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011] textPos = [.1 - (i + 1) * spacing, .1, 0.011]
text = "Frame:" + str(frameNr) + "\nObject UID:" + objectInfo text = "Frame:" + str(frameNr) + "\nObject UID:" + objectInfo
textUid = p.addUserDebugText(text,textColorRGB=[0,0,0], textSize = 0.02, textPosition = textPos,textOrientation = textOrn,parentObjectUniqueId=uiCube, parentLinkIndex = -1, replaceItemUniqueId = lines[i]) textUid = p.addUserDebugText(text,
textColorRGB=[0, 0, 0],
textSize=0.02,
textPosition=textPos,
textOrientation=textOrn,
parentObjectUniqueId=uiCube,
parentLinkIndex=-1,
replaceItemUniqueId=lines[i])
lines[i] = textUid lines[i] = textUid
#keep the gripper centered/symmetric #keep the gripper centered/symmetric
b = p.getJointState(pr2_gripper, 2)[0] b = p.getJointState(pr2_gripper, 2)[0]
p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
@@ -215,16 +282,20 @@ while (1):
hitObjectUid = hit[0][0] hitObjectUid = hit[0][0]
linkIndex = hit[0][1] linkIndex = hit[0][1]
if (hitObjectUid >= 0): if (hitObjectUid >= 0):
objectInfo = str(hitObjectUid)+" Link Index="+str(linkIndex)+"\nBase Name:"+p.getBodyInfo(hitObjectUid)[0].decode()+"\nBody Info:"+p.getBodyInfo(hitObjectUid)[1].decode() objectInfo = str(hitObjectUid) + " Link Index=" + str(
linkIndex) + "\nBase Name:" + p.getBodyInfo(hitObjectUid)[0].decode(
) + "\nBody Info:" + p.getBodyInfo(hitObjectUid)[1].decode()
else: else:
objectInfo = "None" objectInfo = "None"
if e[CONTROLLER_ID] == controllerId: # To make sure we only get the value for one of the remotes if e[
CONTROLLER_ID] == controllerId: # To make sure we only get the value for one of the remotes
#sync the vr pr2 gripper with the vr controller position #sync the vr pr2 gripper with the vr controller position
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500) p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
relPosTarget = 1 - e[ANALOG] relPosTarget = 1 - e[ANALOG]
#open/close the gripper, based on analogue #open/close the gripper, based on analogue
p.changeConstraint(pr2_cid2,gearRatio=1, erp=1, relativePositionTarget=relPosTarget, maxForce=3) p.changeConstraint(pr2_cid2,
gearRatio=1,
erp=1,
relativePositionTarget=relPosTarget,
maxForce=3)

Some files were not shown because too many files have changed in this diff Show More