AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-04 17:40:06 +00:00
Code Issues 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
Show Stats 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
header = """/* Copyright (C) 2006 Charlie C
*
* This software is provided 'as-is', without any express or implied
@ -27,14 +26,17 @@ dtList = dump.DataTypeList
out = "../BlenderSerialize/autogenerated/"
spaces = 4
def addSpaces(file, space):
for i in range(0, space):
file.write(" ")
def write(file, spaces, string):
addSpaces(file, spaces)
file.write(string)
###################################################################################
blender = open(out + "blender.h", 'w')
blender.write(header)
@ -69,7 +71,6 @@ blenderC.write("}\n")
blenderC.write("#endif//__BLENDERCOMMON_H__")
blenderC.close()
for dt in dtList:
fp = open(out + dt.filename + ".h", 'w')
@ -80,7 +81,6 @@ for dt in dtList:
fp.write("#define __%s__H__\n" % strUpper)
fp.write("\n\n")
fp.write("// -------------------------------------------------- //\n")
fp.write("#include \"blender_Common.h\"\n")
@ -93,7 +93,6 @@ for dt in dtList:
addSpaces(fp, 4)
fp.write("// ---------------------------------------------- //\n")
write(fp, 4, "class %s\n" % dt.name)
write(fp, 4, "{\n")
@ -101,11 +100,8 @@ for dt in dtList:
for i in dt.dataTypes:
write(fp, 8, i + ";\n")
write(fp, 4, "};\n")
fp.write("}\n")
fp.write("\n\n")
fp.write("#endif//__%s__H__\n" % strUpper)
fp.close()

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

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

97
data/dinnerware/generate.py
View File

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

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

@ -14,14 +14,11 @@ maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1
p.connect(p.GUI)
amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5)
timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1)
jointTypeNames = {}
jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE"
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_PLANAR_REFLECTION, 1)
jointNamesToIndex = {}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision, j)
jointInfoName = jointInfo[1].decode("utf-8")
@ -48,7 +42,6 @@ for j in range(p.getNumJoints(vision)):
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_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)
relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0)
motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos)
motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos)
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)
relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0)
motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos)
motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos)
motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft)
@ -107,25 +98,46 @@ camYaw = -2
camPitch = -16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
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])
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])
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)
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)
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.setRealTimeSimulation(1)
for i in range(1):
#while (1):
@ -135,43 +147,108 @@ for i in range (1):
erp_rf = p.readUserDebugParameter(erp_rf_Id)
relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
#motC_rf
p.setJointMotorControl2(vision,motor_front_rightR_joint,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.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce)
p.setJointMotorControl2(vision,
motor_front_rightR_joint,
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.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)
motB_lf = p.readUserDebugParameter(motB_lf_Id)
motC_lf = p.readUserDebugParameter(motC_lf_Id)
erp_lf = p.readUserDebugParameter(erp_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,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)
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.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)
motB_rb = p.readUserDebugParameter(motB_rb_Id)
motC_rb = p.readUserDebugParameter(motC_rb_Id)
erp_rb = p.readUserDebugParameter(erp_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,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)
p.setJointMotorControl2(vision,
motor_back_rightR_joint,
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.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)
motB_lb = p.readUserDebugParameter(motB_lb_Id)
motC_lb = p.readUserDebugParameter(motC_lb_Id)
erp_lb = p.readUserDebugParameter(erp_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,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.setJointMotorControl2(vision,
motor_back_leftL_joint,
p.POSITION_CONTROL,
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)
time.sleep(1. / 240.)
@ -196,25 +273,69 @@ while (1):
motB_lf = -(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,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_rightR_joint,
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,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_back_rightR_joint,
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,motor_back_rightR_joint,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,motor_back_leftL_joint,p.POSITION_CONTROL,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.setJointMotorControl2(vision,
motor_back_leftL_joint,
p.POSITION_CONTROL,
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.setGravity(0, 0, -10)
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):
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+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
for i in range(NUM_VERTS_X - 1):
@ -55,5 +56,3 @@ for i in range (totalTriangles):
print(" "),
print(gGroundIndices[i * 3 + 2]),
print(" ")

1
data/threecubes/threecubes.py
View File

@ -5,4 +5,3 @@ p.loadSDF("newsdf.sdf")
while (1):
p.getCameraImage(320, 240, flags=p.ER_NO_SEGMENTATION_MASK)
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)
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]
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]
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
]
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()

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))
else:
writer.write("/>%s" % (newl))
# replace minidom's function with ours
xml.dom.minidom.Element.writexml = fixed_writexml
class Table:
def __init__(self, parent=None):
self.parent = parent
self.table = {}
@ -120,6 +123,7 @@ class Table:
class QuickLexer(object):
def __init__(self, **res):
self.str = ""
self.top = None
@ -196,6 +200,7 @@ def child_nodes(elt):
yield c
c = c.nextSibling
all_includes = []
# 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)
except Exception as e:
raise XacroException(
"included file \"%s\" generated an error during XML parsing: %s"
% (filename, str(e)))
"included file \"%s\" generated an error during XML parsing: %s" %
(filename, str(e)))
except IOError as e:
raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e)))
@ -332,8 +337,8 @@ def grab_properties(doc):
break
if has_bad:
sys.stderr.write('Property names may not have whitespace, ' +
'"{", "}", or "$" : "' + name + '"')
sys.stderr.write('Property names may not have whitespace, ' + '"{", "}", or "$" : "' +
name + '"')
else:
table[name] = value
@ -453,6 +458,7 @@ def eval_expr(lex, symbols):
def eval_text(text, symbols):
def handle_expr(s):
lex = QuickLexer(IGNORE=r"\s+",
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:
block = block.nextSibling
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)
scoped[param] = block
block = block.nextSibling
@ -590,7 +597,9 @@ def eval_all(root, macros, symbols):
elif value == 'false': keep = False
else: keep = float(value)
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 keep:
for e in list(child_nodes(node)):
@ -630,12 +639,14 @@ def print_usage(exit_code=0):
def set_substitution_args_context(context={}):
substitution_args_context['arg'] = context
def open_output(output_filename):
if output_filename is None:
return sys.stdout
else:
return open(output_filename, 'w')
def main():
try:
opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes'])
@ -689,11 +700,14 @@ def main():
print()
else:
eval_self_contained(doc)
banner = [xml.dom.minidom.Comment(c) for c in
[" %s " % ('=' * 83),
banner = [
xml.dom.minidom.Comment(c) for c in [
" %s " % ('=' * 83),
" | This document was autogenerated by xacro from %-30s | " % args[0],
" | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "",
" %s " % ('=' * 83)]]
" %s " % ('=' * 83)
]
]
first = doc.firstChild
for comment in banner:
doc.insertBefore(comment, first)
@ -701,5 +715,6 @@ def main():
open_output(output_filename).write(doc.toprettyxml(indent=' '))
print()
if __name__ == '__main__':
main()

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

@ -2,42 +2,43 @@ import re
if (__name__ == "__main__"):
# Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
HeadAttributes=HeadMatch.group(1);
FullDocumentHead=HeadMatch.group(2);
BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
BodyAttributes=BodyMatch.group(1);
FullPreviewBody=BodyMatch.group(2);
ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
ReplacementList=[
("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
HeadAttributes = HeadMatch.group(1)
FullDocumentHead = HeadMatch.group(2)
BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
BodyAttributes = BodyMatch.group(1)
FullPreviewBody = BodyMatch.group(2)
ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
];
("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
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
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):
FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
1) + "\",\"" + Match.group(2) + "\");\r\n"
open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
# Concatenate all the scripts together
SourceFileList = [
"PrepareHTML.full.js",
"SetMarkdeepMode.js",
"markdeep.min.js",
"DisplayMarkdeepOutput.js",
"PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.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]);
OutputFile=open("MarkdeepUtility.js","wb");
OutputFile.write(OutputCode.encode("utf-8"));
OutputFile.close();
print("Done.");
]
OutputCode = "\r\n\r\n".join([
"// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
for SourceFile in SourceFileList
])
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__"):
# Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
HeadAttributes=HeadMatch.group(1);
FullDocumentHead=HeadMatch.group(2);
BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
BodyAttributes=BodyMatch.group(1);
FullPreviewBody=BodyMatch.group(2);
ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
ReplacementList=[
("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
HeadAttributes = HeadMatch.group(1)
FullDocumentHead = HeadMatch.group(2)
BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
BodyAttributes = BodyMatch.group(1)
FullPreviewBody = BodyMatch.group(2)
ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
];
("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
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
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):
FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
1) + "\",\"" + Match.group(2) + "\");\r\n"
open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
# Concatenate all the scripts together
SourceFileList = [
"PrepareHTML.full.js",
"SetMarkdeepMode.js",
"markdeep.min.js",
"DisplayMarkdeepOutput.js",
"PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.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]);
OutputFile=open("MarkdeepUtility.js","wb");
OutputFile.write(OutputCode.encode("utf-8"));
OutputFile.close();
print("Done.");
]
OutputCode = "\r\n\r\n".join([
"// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
for SourceFile in SourceFileList
])
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?
MJCF_COLORS_FROM_FILE = 512
def run():
print("grpc.insecure_channel")
channel = grpc.insecure_channel('localhost:6667')
@ -18,61 +19,77 @@ def run():
response = 0
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("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("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)
bodyUniqueId = response.urdfStatus.bodyUniqueId
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("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("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("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("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("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)
i = 0
while (True):
i = i + 1
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("TerminateServerCommand")
#response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
#print("PyBullet client received: " , response.statusType)
if __name__ == '__main__':
run()

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

@ -2,24 +2,36 @@ import pybullet as p
import time
import math
def getRayFromTo(mouseX, mouseY):
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]]
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]
]
farPlane = 10000
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]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[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]]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[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
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
@ -39,14 +51,28 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
#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)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
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
rangey = 3
for i in range(rangex):
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.stopStateLogging(logId)
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("r2d2.urdf", [3, 3, 1])
rayFrom = []
rayTo = []
rayIds = []
@ -24,7 +23,6 @@ numRays = 1024
rayLen = 13
rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0]
@ -32,7 +30,10 @@ replaceLines = True
for i in range(numRays):
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):
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
else:
@ -53,9 +54,6 @@ for i in range (numSteps):
#for i in range (10):
# p.removeAllUserDebugItems()
if (useGui):
if (not replaceLines):
p.removeAllUserDebugItems()
@ -63,7 +61,6 @@ for i in range (numSteps):
for i in range(numRays):
hitObjectUid = results[i][0]
if (hitObjectUid < 0):
hitPosition = [0, 0, 0]
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")
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf",
cubeStartPos,
cubeStartOrientation)
botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
#disable the default velocity motors
#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
colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
@ -40,4 +39,3 @@ for i in range (100000):
p.stopStateLogging(logId)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,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.stopStateLogging(logId)
p.resetSimulation();
p.resetSimulation()
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin")
while (p.isConnected()):
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[2] += pt[11][2] * pt[10] + pt[13][2] * pt[12]
print("totalNormalForce=", totalNormalForce)
print("totalLateralFrictionForce=", totalLateralFrictionForce)

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

@ -8,48 +8,33 @@ p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0)
#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)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh:
vertices=[ [-0.246350,-0.246483,-0.000624],
[-0.151407, -0.176325, 0.172867],
[ -0.246350, 0.249205, -0.000624],
[ -0.151407, 0.129477, 0.172867],
[ 0.249338, -0.246483, -0.000624],
[ 0.154395, -0.176325, 0.172867],
[ 0.249338, 0.249205, -0.000624],
[ 0.154395, 0.129477, 0.172867]
vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
[-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
[0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
[0.249338, 0.249205, -0.000624], [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
]
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
stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
@ -57,32 +42,44 @@ visualShapeId = -1
segmentStart = 0
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
for i in range(segmentLength):
height = 0
if (i % 2):
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
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
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
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):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
width = 4
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
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
@ -96,26 +93,44 @@ axis=[[1,0,0]]
baseOrientation = [0, 0, 0, 1]
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)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
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
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
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()
p.stepSimulation()
#print(keys)
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):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1. / 120.)
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]
meshScale = [0.1, 0.1, 0.1]
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]
]
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]]
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],
[-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],
[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]
]
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],
[-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],
[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=[
[0.750000,0.250000],
[1.000000,0.250000],
[1.000000,0.000000],
[0.750000,0.000000],
[0.500000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000],
[0.500000,0.000000],
[0.500000,0.000000],
[0.750000,0.000000],
[0.750000,0.250000],
[0.500000,0.250000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.000000,0.250000],
[0.000000,0.500000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.500000,0.250000],
[0.500000,0.500000],
[0.000000,0.000000],
[0.000000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000]
uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
[0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
[0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
[0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
[0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
[0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
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
]
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)
#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)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
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)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
batchPositions = []
for x in range(32):
for y in range(32):
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.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]]
currentColor = 0
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)

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

@ -1,20 +1,18 @@
import pybullet as p
import time
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0, 0)
sphereRadius = 0.05
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
visualShapeId = -1
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
@ -29,18 +27,38 @@ axis=[[0,0,1]]
for i in range(3):
for j 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]
if (k & 2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation)
sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
baseOrientation)
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)):
p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
@ -53,4 +71,3 @@ while (1):
print(keys)
time.sleep(0.01)

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

@ -8,11 +8,10 @@ p.resetSimulation()
#p.createCollisionShape(p.GEOM_PLANE)
#p.createMultiBody(0,0)
#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)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
@ -21,14 +20,13 @@ colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH, fileName="stone.obj")
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
@ -36,32 +34,44 @@ visualShapeId = -1
segmentStart = 0
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
for i in range(segmentLength):
height = 0
if (i % 2):
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
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
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
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):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
width = 4
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
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
@ -75,26 +85,44 @@ axis=[[1,0,0]]
baseOrientation = [0, 0, 0, 1]
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)
print(p.getNumJoints(boxId))
for joint in range(p.getNumJoints(boxId)):
targetVelocity = 10
if (i % 2):
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
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
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()
p.stepSimulation()
#print(keys)
time.sleep(0.01)

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

@ -5,28 +5,40 @@ useMaximalCoordinates = 0
p.connect(p.GUI)
#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])
p.createMultiBody(0, monastryId, baseOrientation=orn)
sphereRadius = 0.05
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
visualShapeId = -1
for i in range(5):
for j in range(5):
for k in range(5):
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:
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)
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.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
@ -35,4 +47,3 @@ while (1):
keys = p.getKeyboardEvents()
#print(keys)
time.sleep(0.01)

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

@ -2,24 +2,36 @@ import pybullet as p
import time
import math
def getRayFromTo(mouseX, mouseY):
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]]
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]
]
farPlane = 10000
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]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[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]]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[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
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
@ -37,99 +49,94 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
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]
]
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]]
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],
[-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],
[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]
]
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],
[-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],
[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=[
[0.750000,0.250000],
[1.000000,0.250000],
[1.000000,0.000000],
[0.750000,0.000000],
[0.500000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000],
[0.500000,0.000000],
[0.500000,0.000000],
[0.750000,0.000000],
[0.750000,0.250000],
[0.500000,0.250000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.000000,0.250000],
[0.000000,0.500000],
[0.250000,0.500000],
[0.250000,0.250000],
[0.500000,0.250000],
[0.500000,0.500000],
[0.000000,0.000000],
[0.000000,0.250000],
[0.250000,0.250000],
[0.250000,0.000000]
uvs = [[0.750000, 0.250000], [1.000000, 0.250000], [1.000000, 0.000000], [0.750000, 0.000000],
[0.500000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000], [0.500000, 0.000000],
[0.500000, 0.000000], [0.750000, 0.000000], [0.750000, 0.250000], [0.500000, 0.250000],
[0.250000, 0.500000], [0.250000, 0.250000], [0.000000, 0.250000], [0.000000, 0.500000],
[0.250000, 0.500000], [0.250000, 0.250000], [0.500000, 0.250000], [0.500000, 0.500000],
[0.000000, 0.000000], [0.000000, 0.250000], [0.250000, 0.250000], [0.250000, 0.000000]]
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
]
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)
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_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")
@ -137,7 +144,13 @@ rangex = 1
rangey = 1
for i in range(rangex):
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.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)

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

@ -2,25 +2,37 @@ import pybullet as p
import time
import math
def getRayFromTo(mouseX, mouseY):
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]]
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]
]
farPlane = 10000
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]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[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]]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[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
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
@ -38,14 +50,28 @@ p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
#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)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
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
rangey = 5
for i in range(rangex):
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.stopStateLogging(logId)
p.setGravity(0, 0, -10)

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

@ -2,24 +2,36 @@ import pybullet as p
import time
import math
def getRayFromTo(mouseX, mouseY):
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]]
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]
]
farPlane = 10000
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]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[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]]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[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
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
@ -40,15 +52,34 @@ shift2 = [0,0,0]
meshScale = [0.1, 0.1, 0.1]
#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])
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])
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])
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
rangey = 2
for i in range(rangex):
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.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)

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

@ -6,7 +6,11 @@ if (cid<0):
p.connect(p.GUI)
p.loadURDF("plane.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, 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)

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

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

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

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

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

@ -1,4 +1,3 @@
import pybullet as p
import time
import pkgutil
@ -24,7 +23,6 @@ pitch = -10.0
roll = 0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range(0, 360, 10):
start = time.time()
@ -33,15 +31,23 @@ while (p.isConnected()):
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 = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, 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]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
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()
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()
print("renderImage %f" % (stop - start))
#time.sleep(.1)
#print("img_arr=",img_arr)
p.unloadPlugin(plugin)

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

@ -4,42 +4,45 @@ import time
p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
terrain_mass = 0
terrain_visual_shape_id = -1
terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape(
shapeType=p.GEOM_MESH,
terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
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])
p.createMultiBody(
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation)
useMaximalCoordinates = True
sphereRadius = 0.005
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
visualShapeId = -1
for i in range(5):
for j in range(5):
for k in range(5):
#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:
# 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.setGravity(0, 0, -10)
pts = p.getContactPoints()
@ -48,4 +51,3 @@ print(pts)
while (p.isConnected()):
time.sleep(1. / 240.)
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
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
@ -29,7 +28,6 @@ while (1):
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
for k, v in keys.items():

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

@ -12,7 +12,6 @@ if (fileIO>=0):
else:
print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False)
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))
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)
velocity = 1
num = 40
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)
y = velocity * math.cos(2. * 3.1415 * float(i) / num)
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,rollingFriction=10)
p.changeDynamics(sphere, -1, linearDamping=0)
@ -41,6 +42,5 @@ for i in range (num):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1):
time.sleep(0.01)

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

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

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

@ -3,11 +3,9 @@ import numpy as np
import pybullet as p
import time
direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin")
p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf", [0, 0, 1])
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)
# 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.
depth_buffer_opengl = np.reshape(images[3], [width, height])
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)
# 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_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
@ -53,13 +59,34 @@ for b in range (p.getNumBodies()):
for j in range(p.getNumJoints(b)):
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]
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)
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
]
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.
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.
# 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.subplots_adjust(hspace=0.7)
plt.show()

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

@ -11,12 +11,18 @@ obA=-1
obB = -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
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
yaw = 0
@ -33,7 +39,14 @@ while (p.isConnected()):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
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)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
@ -44,10 +57,14 @@ while (p.isConnected()):
#print("distance=",distance)
ptA = pts[0][5]
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.)
#removeCollisionShape is optional:
#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)

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

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

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

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

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

@ -32,12 +32,18 @@ thumbId = 3
p.setRealTimeSimulation(1)
def getSerialOrNone(portname):
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:
return None
def convertSensor(x, fingerIndex):
minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex]
@ -54,6 +60,7 @@ def convertSensor(x, fingerIndex):
b = (v - minV) / float(maxV - minV)
return (b)
ser = None
portindex = 0
while (ser is None and portindex < 30):
@ -69,7 +76,11 @@ while (ser is None and portindex < 30):
portindex = portindex + 1
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
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)
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
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_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path)
with open(path, 'r') as f:
@ -54,62 +56,95 @@ print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames)
frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
erpId = p.addUserDebugParameter("erp", 0, 1, 0.2)
kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
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]]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][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])
p.addUserDebugText("Stable PD",
[startLocations[0][0], startLocations[0][1] + 1, startLocations[0][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)
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 = p.loadURDF("humanoid/humanoid.urdf",
startLocations[0],
globalScaling=0.25,
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])
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],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])
humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
startLocations[0], [0, 0, 0, 1])
humanoid2_fix = p.createConstraint(humanoid2, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
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,
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]
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,
-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,
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
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,
-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,
-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(humanoid2, startLocations[1], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
index0 = 7
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
targetPosition = [0]
jointType = ji[2]
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]]
index0 += 4
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(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
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(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])
p.setJointMotorControlMultiDof(humanoid,
j,
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):
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, "].name=", ji[1])
jointIds = []
paramIds = []
for j in range(p.getNumJoints(humanoid)):
@ -191,16 +242,19 @@ leftElbow=13
import time
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]
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]
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
]
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
p.getCameraImage(320, 200)
while (p.isConnected()):
if useGUI:
@ -236,9 +290,11 @@ while (p.isConnected()):
basePos1Start = [frameData[1], frameData[2], frameData[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[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2])
]
baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction)
@ -254,7 +310,6 @@ while (p.isConnected()):
basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn)
chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
@ -269,19 +324,26 @@ while (p.isConnected()):
rightKneeRotStart = [frameData[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]]
rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]]
rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, frameFraction)
rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]]
rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
rightShoulderRotEnd = [
frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25]
]
rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart, rightShoulderRotEnd,
frameFraction)
rightElbowRotStart = [frameData[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]]
leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]]
@ -300,7 +362,9 @@ while (p.isConnected()):
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
leftElbowRot = [
leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
]
if (0): #if (once):
p.resetJointStateMultiDof(humanoid, chest, chestRot)
@ -328,41 +392,26 @@ while (p.isConnected()):
totalDofs += dof
jointIndicesAll = [
chest,
neck,
rightHip,
rightKnee,
rightAnkle,
rightShoulder,
rightElbow,
leftHip,
leftKnee,
leftAnkle,
leftShoulder,
leftElbow
chest, neck, rightHip, rightKnee, rightAnkle, rightShoulder, rightElbow, leftHip, leftKnee,
leftAnkle, leftShoulder, leftElbow
]
basePos, baseOrn = p.getBasePositionAndOrientation(humanoid)
pose = [ basePos[0],basePos[1],basePos[2],
baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
chestRot[0],chestRot[1],chestRot[2],chestRot[3],
neckRot[0],neckRot[1],neckRot[2],neckRot[3],
rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
rightKneeRot[0],
rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
pose = [
basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
chestRot[0], chestRot[1], chestRot[2], chestRot[3], neckRot[0], neckRot[1], neckRot[2],
neckRot[3], rightHipRot[0], rightHipRot[1], rightHipRot[2], rightHipRot[3],
rightKneeRot[0], rightAnkleRot[0], rightAnkleRot[1], rightAnkleRot[2], rightAnkleRot[3],
rightShoulderRot[0], rightShoulderRot[1], rightShoulderRot[2], rightShoulderRot[3],
rightElbowRot[0],
leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
leftKneeRot[0],
leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
rightElbowRot[0], leftHipRot[0], leftHipRot[1], leftHipRot[2], leftHipRot[3],
leftKneeRot[0], leftAnkleRot[0], leftAnkleRot[1], leftAnkleRot[2], leftAnkleRot[3],
leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3],
leftElbowRot[0] ]
leftElbowRot[0]
]
#print("pose=")
#for po in pose:
# print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices=jointIndicesAll,
desiredPositions=pose,
@ -387,32 +436,105 @@ while (p.isConnected()):
jointIndex = jointIndicesAll[index]
if jointDofCounts[index] == 4:
p.setJointMotorControlMultiDof(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]])
p.setJointMotorControlMultiDof(
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:
p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
p.setJointMotorControlMultiDof(humanoid3, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus3[dofIndex]])
p.setJointMotorControlMultiDof(humanoid,
jointIndex,
controlMode=p.TORQUE_CONTROL,
force=[taus[dofIndex]])
p.setJointMotorControlMultiDof(humanoid3,
jointIndex,
controlMode=p.TORQUE_CONTROL,
force=[taus3[dofIndex]])
dofIndex += jointDofCounts[index]
#print("len(taus)=",len(taus))
#print("taus=",taus)
p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,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])
p.setJointMotorControlMultiDof(humanoid2,
chest,
p.POSITION_CONTROL,
targetPosition=chestRot,
positionGain=kp,
force=[maxForce])
p.setJointMotorControlMultiDof(humanoid2,
neck,
p.POSITION_CONTROL,
targetPosition=neckRot,
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
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")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[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 ]
p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
[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)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
@ -33,4 +38,3 @@ p.stopStateLogging(logId)
print("ended benchmark")
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")
human = obUids[0]
for i in range(p.getNumJoints(human)):
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)
maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
kneeJointIndex = 11
kneeJointIndexLeft = 18
@ -37,10 +34,18 @@ while (1):
time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
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)
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):
p.stepSimulation()

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

@ -23,8 +23,19 @@ while 1:
pos = [0, 0, 1.26]
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):
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)

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

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

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

@ -4,33 +4,32 @@ import time
p.connect(p.GUI)
if (1):
box_collision_shape_id = p.createCollisionShape(
shapeType=p.GEOM_BOX,
box_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_BOX,
halfExtents=[0.01, 0.01, 0.055])
box_mass = 0.1
box_visual_shape_id = -1
box_position = [0, 0.1, 1]
box_orientation = [0, 0, 0, 1]
p.createMultiBody(
box_mass, box_collision_shape_id, box_visual_shape_id,
box_position, box_orientation, useMaximalCoordinates=True)
p.createMultiBody(box_mass,
box_collision_shape_id,
box_visual_shape_id,
box_position,
box_orientation,
useMaximalCoordinates=True)
terrain_mass = 0
terrain_visual_shape_id = -1
terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape(
shapeType=p.GEOM_MESH,
terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
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])
p.createMultiBody(
terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
p.createMultiBody(terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id,
terrain_position, terrain_orientation)
p.setGravity(0, 0, -10)
pts = p.getContactPoints()
@ -39,4 +38,3 @@ print(pts)
while (p.isConnected()):
time.sleep(1. / 240.)
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
with_fixed_joints = True
if with_fixed_joints:
id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
robot_base,
robot_orientation,
useFixedBase=True)
else:
id_revolute_joints = [0, 1]
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, 0, 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:
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:
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[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
q_pos = [[0.] * steps, [0.] * steps]
q_vel = [[0.] * steps, [0.] * steps]
q_tor = [[0.] * steps, [0.] * steps]
@ -105,7 +112,10 @@ for i in range(len(t)):
print(torque)
# 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
bullet.stepSimulation()
@ -149,7 +159,6 @@ if plot:
plt.pause(0.01)
while (1):
bullet.stepSimulation()
time.sleep(0.01)

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

@ -14,7 +14,6 @@ numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
@ -48,7 +47,10 @@ p.setRealTimeSimulation(useRealTimeSimulation)
trailDuration = 15
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):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
@ -65,18 +67,42 @@ while 1:
if (useNullSpace == 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:
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:
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:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
solver=ikSolver)
if (useSimulation):
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:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
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):
p.connect(p.GUI)
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)):
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
jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#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 = [0, 0, 0, 1]
@ -33,7 +34,6 @@ numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
@ -68,6 +68,7 @@ basepos =[0,0,0]
ang = 0
ang = 0
def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
@ -122,7 +123,11 @@ while 1:
baseorn = p.getQuaternionFromEuler([0, 0, ang])
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])
if (useRealTimeSimulation):
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 (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:
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:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
else:
threshold = 0.001
maxIter = 100
jointPoses = accurateCalculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos, threshold, maxIter)
jointPoses = accurateCalculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos,
threshold, maxIter)
if (useSimulation):
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:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
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.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
@ -41,7 +40,12 @@ while 1:
for i in range(1):
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)
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]
return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, 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]
return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
p.setJointMotorControlArray(robot,
range(num_joints),
p.POSITION_CONTROL,
targetPositions=position,
targetVelocities=zero_vec,
positionGains=[kp] * num_joints,
velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0]
i = 0
@ -69,7 +76,10 @@ p.stepSimulation()
pos, vel, torq = getJointStates(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
# 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.

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)
print(p.getJointInfo(door, j))
frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
@ -23,4 +22,3 @@ while (1):
p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
p.stepSimulation()
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])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
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
p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
@ -18,7 +21,11 @@ p.setRealTimeSimulation(1)
while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
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)
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
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
@ -58,6 +59,7 @@ print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
for objectId in range(objectNum):
record = log[stepIndex * objectNum + objectId]
@ -80,4 +82,3 @@ while True:
Step(stepIndex)
p.stepSimulation()
Step(stepIndex)

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

@ -70,18 +70,36 @@ while 1:
if (useNullSpace == 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:
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:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
else:
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos)
if (useSimulation):
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:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
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
from time import sleep
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
@ -49,6 +50,7 @@ def readLogFile(filename, verbose = True):
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
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):
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 j 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.setGravity(0, 0, -10)
@ -32,4 +33,3 @@ while True:
gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX, gravY, gravZ)
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))
p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
c = p.createConstraint(wheelA,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(wheelA,
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)
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)
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.setRealTimeSimulation(1)
while (1):
p.setGravity(0, 0, -10)
time.sleep(0.01)
#p.removeConstraint(c)

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

@ -1,7 +1,9 @@
import pybullet as p
import numpy as np
class Minitaur:
def __init__(self, urdfRootPath=''):
self.urdfRootPath = urdfRootPath
self.reset()
@ -26,7 +28,6 @@ class Minitaur:
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, 0, 0, .2)
self.kp = 1
@ -38,10 +39,14 @@ class Minitaur:
self.buildJointNameToIdDict()
self.buildMotorIdList()
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):
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
@ -52,50 +57,104 @@ class Minitaur:
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
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])
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_leftL_joint'],
self.motorDir[0] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_leftL_link'],
self.motorDir[0] * kneeangle)
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_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,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
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,
jointIndex=self.jointNameToId['knee_front_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
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])
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_leftL_joint'],
self.motorDir[2] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_leftL_link'],
self.motorDir[2] * kneeangle)
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_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,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
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,
jointIndex=self.jointNameToId['knee_back_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
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])
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_rightL_joint'],
self.motorDir[4] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_rightL_link'],
self.motorDir[4] * kneeangle)
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_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,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
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,
jointIndex=self.jointNameToId['knee_front_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
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])
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightL_joint'],
self.motorDir[6] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightL_link'],
self.motorDir[6] * kneeangle)
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_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,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
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,
jointIndex=self.jointNameToId['knee_back_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
def getBasePosition(self):
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()
return np.asarray(position)
def is_fallen():
global minitaur
orientation = minitaur.getBaseOrientation()
@ -22,13 +23,16 @@ def is_fallen():
localUp = rotMat[6:]
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
nMotors = 8
speed = 0.35
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
def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
speed = 0.35
phaseDiff = params[2]
@ -43,6 +47,7 @@ def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
return joint_values
def evaluate_desired_motorAngle_hop(i, params):
amplitude = params[0]
speed = params[1]
@ -52,13 +57,20 @@ def evaluate_desired_motorAngle_hop(i, params):
return joint_values
evaluate_func_map['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_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
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
@ -95,5 +107,6 @@ def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
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

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

@ -10,18 +10,27 @@ import time
import math
import numpy as np
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c < 0):
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'
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)
main(0)

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

@ -4,7 +4,15 @@ import time
p.connect(p.GUI)
cartpole = p.loadURDF("cartpole.urdf")
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):
p.setGravity(0, 0, -10)
js = p.getJointState(cartpole, 1)

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

@ -11,7 +11,9 @@ if usePhysX:
else:
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.setAdditionalSearchPath(pd.getDataPath())
@ -33,7 +35,10 @@ for i in range (int(num*50)):
num = (radius * 2 * math.pi) / 0.08
radius += 0.05 / 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)
numDominoes += 1
@ -43,7 +48,6 @@ sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaxi
print("numDominoes=", numDominoes)
#for j in range (20):
# for i in range (100):
# if (i<99):
@ -52,17 +56,14 @@ print("numDominoes=",numDominoes)
# 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)
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf", [0, 0, -11])
p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door))
p.setGravity(0, 0, -10)
position_control = True
@ -90,7 +91,13 @@ while (1):
prevTime = curTime
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:
p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=1, force=1011)
#contacts = p.getContactPoints()

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

@ -1,4 +1,3 @@
import pybullet as p
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerExplicit import PDControllerExplicit
@ -6,7 +5,6 @@ from pdControllerStable import PDControllerStable
import time
useMaximalCoordinates = False
p.connect(p.GUI)
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)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
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))
timeStepId = p.addUserDebugParameter("timeStep", 0.001, 0.1, 0.01)
desiredPosCartId = p.addUserDebugParameter("desiredPosCart", -10, 10, 2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart", -10, 10, 0)
@ -37,10 +33,22 @@ maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
textColor = [1, 1, 1]
shift = 0.05
p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,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)
p.addUserDebugText("explicit PD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole,
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)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
@ -50,7 +58,6 @@ maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
useRealTimeSim = False
@ -59,7 +66,6 @@ p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
@ -80,33 +86,64 @@ while p.isConnected():
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
baseDof = 7
taus = exPD.computePD(pole, [0,1], [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)
taus = exPD.computePD(pole, [0, 1], [
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]:
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)
if (pd >= 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
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)
for j in [0, 1]:
p.setJointMotorControlMultiDof(pole4, j, controlMode=p.TORQUE_CONTROL, force=[taus[j]])
p.setJointMotorControl2(pole3,0, 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)
p.setJointMotorControl2(pole3,
0,
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):
p.stepSimulation()
time.sleep(timeStep)

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

@ -2,16 +2,20 @@ import numpy as np
class PDControllerExplicitMultiDof(object):
def __init__(self, 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)
curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
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]
qIndex = 7
qdotIndex = 7
@ -33,7 +37,10 @@ class PDControllerExplicitMultiDof(object):
qIndex += 1
qdotIndex += 1
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)
jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew
@ -41,7 +48,10 @@ class PDControllerExplicitMultiDof(object):
qError.append(axis[1])
qError.append(axis[2])
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.]
qIndex += 4
qdotIndex += 4
@ -60,12 +70,13 @@ class PDControllerExplicitMultiDof(object):
return forces
class PDControllerExplicit(object):
def __init__(self, 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)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
@ -85,5 +96,3 @@ class PDControllerExplicit(object):
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF)
return forces

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

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

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

@ -1,20 +1,23 @@
import pybullet as p
import math
import numpy as np
p.connect(p.GUI)
plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf", [0, 0, 1])
def getRayFromTo(mouseX, mouseY):
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]]
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]
]
farPlane = 10000
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
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos
@ -23,24 +26,34 @@ def getRayFromTo(mouseX,mouseY):
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter=[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],
rayToCenter = [
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[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],
rayFrom[1]+rayForward[1] +ortho[1],
rayFrom[2]+rayForward[2] +ortho[2]]
rayTo = [
rayFrom[0] + rayForward[0] + ortho[0], rayFrom[1] + rayForward[1] + ortho[1],
rayFrom[2] + rayForward[2] + ortho[2]
]
lenOrtho = math.sqrt(ortho[0] * ortho[0] + ortho[1] * ortho[1] + ortho[2] * ortho[2])
alpha = math.atan(lenOrtho / farPlane)
return rayFrom, rayTo, alpha
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]]
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]
]
farPlane = 10000
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)
oneOverHeight = float(1) / float(height)
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)
newTo = (depth / l) * vec + rf
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 = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.changeVisualShape(mb, -1, rgbaColor=color)
@ -117,7 +134,3 @@ print("ready\n")
#p.removeBody(cube)
while (1):
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):
p.submitProfileTiming("pythontest")
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=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
@ -29,7 +28,12 @@ while 1:
projMat = camera[3]
#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])
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)
else:
p.stepSimulation()

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

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

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

@ -10,6 +10,7 @@ import os, fnmatch
import argparse
from time import sleep
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
@ -52,9 +53,10 @@ def readLogFile(filename, verbose = True):
return log
clid = p.connect(p.SHARED_MEMORY)
log = readLogFile("LOG00076.TXT");
log = readLogFile("LOG00076.TXT")
recordNum = len(log)
print('record num:'),
@ -72,8 +74,6 @@ maxForce = 3.5
kp = .05
kd = .5
quadruped = 1
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
@ -106,21 +106,71 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
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]
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
]
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, 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.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,
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.stepSimulation()
p.stepSimulation()
sleep(0.01)

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

@ -1,20 +1,42 @@
import pybullet as p
p.connect(p.SHARED_MEMORY)
objects = [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)]
objects = [
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]
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)):
p.resetJointState(ob, ji, jointPositions[ji])
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)
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)
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)
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.setGravity(0.000000, 0.000000, 0.000000)
p.stepSimulation()

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

@ -47,7 +47,11 @@ while (True):
#print(targetVelocity)
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:
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("----------------")
#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)
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)
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)
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)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
c = p.createConstraint(car,
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)
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)
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)
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)
steering = [0, 2]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
@ -63,7 +117,11 @@ while (True):
#print(targetVelocity)
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:
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
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)
p.loadURDF("r2d2.urdf")

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

@ -29,6 +29,7 @@ import matplotlib.pyplot as plt
class BulletSim():
def __init__(self, connection_mode, *argv):
self.connection_mode = connection_mode
self.argv = argv
@ -57,6 +58,7 @@ class BulletSim():
def __exit__(self, *_, **__):
pybullet.disconnect()
def test(num_runs=300, shadow=1, log=True, plot=False):
if log:
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")
ax = plt.gca()
times = np.zeros(num_runs)
yaw_gen = itertools.cycle(range(0, 360, 10))
for i, yaw in zip(range(num_runs), yaw_gen):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=shadow,lightDirection=[1,1,1],
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = pybullet.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=shadow,
lightDirection=[1, 1, 1],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
@ -110,10 +116,8 @@ def test(num_runs=300, shadow=1, log=True, plot=False):
return mean_time
if __name__ == "__main__":
res = []
with BulletSim(pybullet.DIRECT):
@ -121,9 +125,10 @@ if __name__ == "__main__":
mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time))
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")
if plugin < 0:
print("\nPlugin Failed to load!\n")
@ -143,4 +148,3 @@ if __name__ == "__main__":
print("back nenv fps fps_tot")
for r in res:
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):
def __init__(self,
def __init__(
self,
renderer='tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
@ -47,7 +49,9 @@ class TestEnv(gym.Env):
p.connect(p.DIRECT, options=optionstring)
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")
if plugin < 0:
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_RGB_BUFFER_PREVIEW, 0)
def __del__(self):
p.disconnect()
@ -82,11 +85,16 @@ class TestEnv(gym.Env):
p.stepSimulation()
start = time.time()
yaw = next(self.iter)
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix,
projectionMatrix, shadow=1,lightDirection=[1,1,1],
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
upAxisIndex)
aspect = pixelWidth / pixelHeight
projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
img_arr = p.getCameraImage(pixelWidth,
pixelHeight,
viewMatrix,
projectionMatrix,
shadow=1,
lightDirection=[1, 1, 1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2]
@ -95,8 +103,11 @@ class TestEnv(gym.Env):
def seed(self, seed=None):
pass
def train(env_id, num_timesteps=300, seed=0, num_env=2, renderer='tiny'):
def make_env(rank):
def _thunk():
if env_id == "TestEnv":
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)
# only clip rewards when not evaluating
return env
return _thunk
set_global_seeds(seed)
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.setRealTimeSimulation(0)
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]
Iyy = inertia[1]
Izz = inertia[2]
boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
@ -31,22 +34,80 @@ def drawInertiaBox(parentUid, parentLinkIndex):
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
color = [1, 0, 0]
p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[0],
pts[1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[0],
pts[4],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[5],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[6],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[7],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 0],
pts[4 + 1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 1],
pts[4 + 3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 3],
pts[4 + 2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 2],
pts[4 + 0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
drawInertiaBox(cubeId, -1)
@ -59,7 +120,6 @@ while 1:
mass1, frictionCoeff1 = p.getDynamicsInfo(bodyUniqueId=planeId, linkIndex=-1)
mass2, frictionCoeff2 = p.getDynamicsInfo(bodyUniqueId=cubeId, linkIndex=-1)
print(mass1, frictionCoeff1)
print(mass2, frictionCoeff2)
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.disconnect()

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

@ -23,4 +23,3 @@ while True:
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#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.setGravity(0, 0, -10)
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("duck_vhacd.urdf", [1, 0, 1], globalScaling=1, flags=p.URDF_INITIALIZE_SAT_FEATURES)
@ -13,4 +16,3 @@ while (p.isConnected()):
pts = p.getContactPoints()
#print("num contacts = ", len(pts))
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")
def setupWorld():
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
@ -37,6 +38,7 @@ def dumpStateToFile(file):
file.write(txtLinVel)
file.write(txtAngVel)
def compareFiles(file1, file2):
diff = difflib.unified_diff(
file1.readlines(),
@ -53,6 +55,7 @@ def compareFiles(file1,file2):
else:
print("OK, files are identical")
setupWorld()
for i in range(numSteps):
p.stepSimulation()
@ -69,7 +72,6 @@ if verbose:
for i in range(numSteps2):
p.stepSimulation()
file = open("saveFile.txt", "w")
dumpStateToFile(file)
file.close()
@ -96,7 +98,6 @@ if verbose:
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile.txt", "w")
dumpStateToFile(file)
file.close()
@ -113,7 +114,6 @@ if verbose:
for i in range(numSteps2):
p.stepSimulation()
file = open("restoreFile2.txt", "w")
dumpStateToFile(file)
file.close()
@ -134,4 +134,3 @@ p.stopStateLogging(logId)
#while (p.getConnectionInfo()["isConnected"]):
# time.sleep(1)

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

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

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

@ -12,11 +12,23 @@ collisionShift = [0,0,0]
inertiaShift = [0, 0, -0.5]
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)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
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):
p.stepSimulation()
time.sleep(1. / 240.)

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

@ -12,4 +12,3 @@ p.setTimeStep(timeStep)
while (1):
p.stepSimulation()
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
for k 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
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range(p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
print("r2d2=", r2d2)
@ -31,4 +32,3 @@ while p.isConnected():
time.sleep(timestep)
#force the object to 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 math
# This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert!
p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE)
@ -15,7 +13,8 @@ p.createMultiBody(0,plane)
useMaximalCoordinates = True
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.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
@ -45,7 +44,22 @@ for i in range (36):
basePosition = [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.setRealTimeSimulation(0)
@ -115,12 +129,14 @@ while (1):
targetPos *= 1.0 / (1.0 - m_steering)
#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
m_waveFront += dt/m_wavePeriod * m_waveLength;
m_waveFront += dt / m_wavePeriod * m_waveLength
p.stepSimulation()
time.sleep(dt)

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

@ -3,10 +3,10 @@ import time
p.connect(p.GUI)
#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.loadURDF("plane.urdf")
radius = 0.025
distance = 1.86
@ -28,4 +28,3 @@ p.setGravity(0,0,-10)
while (p.isConnected()):
p.stepSimulation()
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")
os.sys.path.insert(0, parentdir)
import pybullet
import pybullet_data
@ -21,7 +20,8 @@ obj = pybullet.loadURDF(os.path.join(pybullet_data.getDataPath(),"r2d2.urdf"))
posX = 0
posY = 3
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
numJoints = pybullet.getNumJoints(obj)
@ -44,4 +44,3 @@ pybullet.resetSimulation()
#disconnect from the physics server
pybullet.disconnect()

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

@ -1,7 +1,7 @@
import pybullet as p
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)
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
import matplotlib.pyplot as plt
import pybullet
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")
ax = plt.gca()
pybullet.connect(pybullet.DIRECT)
#pybullet.loadPlugin("eglRendererPlugin")
@ -43,10 +40,17 @@ while(1):
for yaw in range(0, 360, 10):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
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)
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
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()
print("renderImage %f" % (stop - start))
@ -61,7 +65,6 @@ while(1):
#plt.imshow(rgb,interpolation='none')
#reshape is needed
np_img_arr = np.reshape(rgb, (h, w, 4))
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()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
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)
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
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()
print("renderImage %f" % (stop - start))
@ -82,7 +89,6 @@ while (1):
plt.pause(0.01)
#image.draw()
main_stop = time.time()
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
#otherwise use testrender.py (slower but compatible without numpy)
#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 time
plt.ion()
img = np.random.rand(200, 320)
@ -44,10 +42,17 @@ while (1):
pybullet.stepSimulation()
start = time.time()
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
aspect = pixelWidth / pixelHeight;
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)
viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch,
roll, upAxisIndex)
aspect = pixelWidth / pixelHeight
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()
print("renderImage %f" % (stop - start))
@ -75,7 +80,6 @@ while (1):
plt.pause(0.01)
#image.draw()
main_stop = time.time()
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)
alpha = p.readUserDebugParameter(alphaSlider)
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)

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

@ -2,10 +2,6 @@ import pybullet as p
import time
from pybullet_utils import urdfEditor
##########################################
org2 = p.connect(p.DIRECT)
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
mb = p.loadURDF("r2d2.urdf", flags=p.URDF_USE_IMPLICIT_CYLINDER, physicsClientId=org)
for i in range(p.getNumJoints(mb, 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.saveUrdf("test2.urdf")
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))
parser = 0
p.setRealTimeSimulation(1, physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)

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

@ -2,15 +2,12 @@ import pybullet as pb
import time
from pybullet_utils import bullet_client
server = bullet_client.BulletClient(connection_mode=pb.SHARED_MEMORY_SERVER)
print("Connecting to bullet server")
CONNECTION_METHOD = pb.SHARED_MEMORY
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
PLANE_PATH = "plane.urdf"
client.loadURDF(PLANE_PATH)
@ -52,19 +49,18 @@ client.addUserData(plane_id, "MyKey1", "MyNewValue")
print("Cached overridden data")
print(client.getUserData(MyKey1))
print("Disconnecting")
del client
print("Reconnecting")
client = bullet_client.BulletClient(connection_mode=CONNECTION_METHOD)
print("Synced overridden data")
print(client.getUserData(MyKey1))
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")
client.removeUserData(MyKey2)
@ -107,4 +103,3 @@ print ("Retrieving user data")
print(client.getUserData(MyKey1))
print(client.getUserData(MyKey3))
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]
ptDistThreshold = 0.01
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])
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]

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

@ -2,22 +2,28 @@ import pybullet as p
import time
#p.connect(p.UDP,"192.168.86.100")
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.resetSimulation()
meshScale = [.1, .1, .01]
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 = -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])
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)]
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("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]
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.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)
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)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
objects = [
p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000, -0.200000, 0.600000, 0.000000, 0.000000,
0.000000, 1.000000)
]
kuka = objects[0]
jointPositions = [-0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001]
for jointIndex in range(p.getNumJoints(kuka)):
p.resetJointState(kuka, jointIndex, jointPositions[jointIndex])
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 = [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.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.700000, 0.000000, 0.000000, 0.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")
kuka_gripper = objects[0]
print("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-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 ]
p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036],
[-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)):
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,0.707107)]
objects = [
p.loadURDF("table/table.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,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 = [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("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000, 0.707107, 0.000000,
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 = [
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("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,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)]
objects = [
p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000, 0.000000, 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")
#ob = objects[0]
#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.setGravity(0, 0, -10)
##show this for 10 seconds
@ -136,8 +194,6 @@ while (uiControllerId<0):
print("Using uiControllerId=" + str(uiControllerId))
controllerId = -1
print("waiting for VR picking controller trigger")
@ -164,7 +220,13 @@ if (once):
spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011]
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):
once = 0
@ -179,15 +241,20 @@ while (1):
frameNr = frameNr + 1
for i in range(numLines):
spacing = 0.01
textPos = [.1 - (i + 1) * spacing, .1, 0.011]
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
#keep the gripper centered/symmetric
b = p.getJointState(pr2_gripper, 2)[0]
p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
@ -215,16 +282,20 @@ while (1):
hitObjectUid = hit[0][0]
linkIndex = hit[0][1]
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:
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
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
relPosTarget = 1 - e[ANALOG]
#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