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

add yapf style and apply yapf to format all Python files

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

5
.style.yapf Normal file
View File

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

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

@@ -1,6 +1,5 @@
import dump import dump
header = """/* Copyright (C) 2006 Charlie C header = """/* Copyright (C) 2006 Charlie C
* *
* This software is provided 'as-is', without any express or implied * This software is provided 'as-is', without any express or implied
@@ -27,27 +26,30 @@ dtList = dump.DataTypeList
out = "../BlenderSerialize/autogenerated/" out = "../BlenderSerialize/autogenerated/"
spaces = 4 spaces = 4
def addSpaces(file, space): def addSpaces(file, space):
for i in range(0, space): for i in range(0, space):
file.write(" ") file.write(" ")
def write(file, spaces, string): def write(file, spaces, string):
addSpaces(file, spaces) addSpaces(file, spaces)
file.write(string) file.write(string)
################################################################################### ###################################################################################
blender = open(out+"blender.h", 'w') blender = open(out + "blender.h", 'w')
blender.write(header) blender.write(header)
blender.write("#ifndef __BLENDER_H__\n") blender.write("#ifndef __BLENDER_H__\n")
blender.write("#define __BLENDER_H__\n") blender.write("#define __BLENDER_H__\n")
for dt in dtList: for dt in dtList:
blender.write("#include \"%s.h\"\n"%dt.filename) blender.write("#include \"%s.h\"\n" % dt.filename)
blender.write("#endif//__BLENDER_H__") blender.write("#endif//__BLENDER_H__")
blender.close() blender.close()
################################################################################### ###################################################################################
blenderC = open(out+"blender_Common.h", 'w') blenderC = open(out + "blender_Common.h", 'w')
blenderC.write(header) blenderC.write(header)
blenderC.write("#ifndef __BLENDERCOMMON_H__\n") blenderC.write("#ifndef __BLENDERCOMMON_H__\n")
blenderC.write("#define __BLENDERCOMMON_H__\n") blenderC.write("#define __BLENDERCOMMON_H__\n")
@@ -63,49 +65,43 @@ blenderC.write(strUnRes)
blenderC.write("namespace Blender {\n") blenderC.write("namespace Blender {\n")
for dt in dtList: for dt in dtList:
write(blenderC, 4, "class %s;\n"%dt.name) write(blenderC, 4, "class %s;\n" % dt.name)
blenderC.write("}\n") blenderC.write("}\n")
blenderC.write("#endif//__BLENDERCOMMON_H__") blenderC.write("#endif//__BLENDERCOMMON_H__")
blenderC.close() blenderC.close()
for dt in dtList: for dt in dtList:
fp = open(out+dt.filename+".h", 'w') fp = open(out + dt.filename + ".h", 'w')
fp.write(header)
strUpper = dt.filename.upper()
fp.write("#ifndef __%s__H__\n"%strUpper)
fp.write("#define __%s__H__\n"%strUpper)
fp.write("\n\n")
fp.write("// -------------------------------------------------- //\n")
fp.write("#include \"blender_Common.h\"\n")
for i in dt.includes: fp.write(header)
fp.write("#include \"%s\"\n"%i) strUpper = dt.filename.upper()
fp.write("\nnamespace Blender {\n") fp.write("#ifndef __%s__H__\n" % strUpper)
fp.write("\n\n") fp.write("#define __%s__H__\n" % strUpper)
fp.write("\n\n")
addSpaces(fp,4) fp.write("// -------------------------------------------------- //\n")
fp.write("// ---------------------------------------------- //\n") fp.write("#include \"blender_Common.h\"\n")
for i in dt.includes:
fp.write("#include \"%s\"\n" % i)
write(fp, 4, "class %s\n"%dt.name) fp.write("\nnamespace Blender {\n")
fp.write("\n\n")
write(fp, 4, "{\n") addSpaces(fp, 4)
write(fp, 4, "public:\n") fp.write("// ---------------------------------------------- //\n")
for i in dt.dataTypes:
write(fp, 8, i+";\n")
write(fp, 4, "class %s\n" % dt.name)
write(fp, 4, "};\n") write(fp, 4, "{\n")
fp.write("}\n") write(fp, 4, "public:\n")
fp.write("\n\n") for i in dt.dataTypes:
fp.write("#endif//__%s__H__\n"%strUpper) write(fp, 8, i + ";\n")
fp.close()
write(fp, 4, "};\n")
fp.write("}\n")
fp.write("\n\n")
fp.write("#endif//__%s__H__\n" % strUpper)
fp.close()

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

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

12
build3/cmake/FindLibPython.py
View File

@@ -15,11 +15,11 @@ print("exec_prefix:%s" % sys.exec_prefix)
print("major_version:%s" % str(sys.version_info[0])) print("major_version:%s" % str(sys.version_info[0]))
print("minor_version:%s" % str(sys.version_info[1])) print("minor_version:%s" % str(sys.version_info[1]))
print("patch_version:%s" % str(sys.version_info[2])) print("patch_version:%s" % str(sys.version_info[2]))
print("short_version:%s" % '.'.join(map(lambda x:str(x), sys.version_info[0:2]))) print("short_version:%s" % '.'.join(map(lambda x: str(x), sys.version_info[0:2])))
print("long_version:%s" % '.'.join(map(lambda x:str(x), sys.version_info[0:3]))) print("long_version:%s" % '.'.join(map(lambda x: str(x), sys.version_info[0:3])))
print("py_inc_dir:%s" % distutils.sysconfig.get_python_inc()) print("py_inc_dir:%s" % distutils.sysconfig.get_python_inc())
print("site_packages_dir:%s" % distutils.sysconfig.get_python_lib(plat_specific=1)) print("site_packages_dir:%s" % distutils.sysconfig.get_python_lib(plat_specific=1))
for e in distutils.sysconfig.get_config_vars ('LIBDIR'): for e in distutils.sysconfig.get_config_vars('LIBDIR'):
if e != None: if e != None:
print("py_lib_dir:%s" % e) print("py_lib_dir:%s" % e)
break break

439
data/dinnerware/generate.py
View File

@@ -1,252 +1,253 @@
from __future__ import print_function from __future__ import print_function
import numpy as np import numpy as np
class Obj: class Obj:
def __init__(self, fn):
self.ind_v = 0 def __init__(self, fn):
self.ind_vt = 0 self.ind_v = 0
self.ind_vn = 0 self.ind_vt = 0
self.fn = fn self.ind_vn = 0
self.out = open(fn + ".tmp", "w") self.fn = fn
self.out.write("mtllib dinnerware.mtl\n") self.out = open(fn + ".tmp", "w")
def __del__(self): self.out.write("mtllib dinnerware.mtl\n")
self.out.close()
import shutil def __del__(self):
shutil.move(self.fn + ".tmp", self.fn) self.out.close()
def push_v(self, v): import shutil
self.out.write("v %f %f %f\n" % (v[0],v[1],v[2])) shutil.move(self.fn + ".tmp", self.fn)
self.ind_v += 1
return self.ind_v def push_v(self, v):
def push_vt(self, vt): self.out.write("v %f %f %f\n" % (v[0], v[1], v[2]))
self.out.write("vt %f %f\n" % (vt[0],vt[1])) self.ind_v += 1
self.ind_vt += 1 return self.ind_v
return self.ind_vt
def push_vn(self, vn): def push_vt(self, vt):
vn /= np.linalg.norm(vn) self.out.write("vt %f %f\n" % (vt[0], vt[1]))
self.out.write("vn %f %f %f\n" % (vn[0],vn[1],vn[2])) self.ind_vt += 1
self.ind_vn += 1 return self.ind_vt
return self.ind_vn
def push_vn(self, vn):
vn /= np.linalg.norm(vn)
self.out.write("vn %f %f %f\n" % (vn[0], vn[1], vn[2]))
self.ind_vn += 1
return self.ind_vn
def convex_hull(points, vind, nind, tind, obj): def convex_hull(points, vind, nind, tind, obj):
"super ineffective" "super ineffective"
cnt = len(points) cnt = len(points)
for a in range(cnt): for a in range(cnt):
for b in range(a+1,cnt): for b in range(a + 1, cnt):
for c in range(b+1,cnt): for c in range(b + 1, cnt):
vec1 = points[a] - points[b] vec1 = points[a] - points[b]
vec2 = points[a] - points[c] vec2 = points[a] - points[c]
n = np.cross(vec1, vec2) n = np.cross(vec1, vec2)
n /= np.linalg.norm(n) n /= np.linalg.norm(n)
C = np.dot(n, points[a]) C = np.dot(n, points[a])
inner = np.inner(n, points) inner = np.inner(n, points)
pos = (inner <= C+0.0001).all() pos = (inner <= C + 0.0001).all()
neg = (inner >= C-0.0001).all() neg = (inner >= C - 0.0001).all()
if not pos and not neg: continue if not pos and not neg: continue
obj.out.write("f %i//%i %i//%i %i//%i\n" % ( obj.out.write("f %i//%i %i//%i %i//%i\n" %
(vind[a], nind[a], vind[b], nind[b], vind[c], nind[c]) ((vind[a], nind[a], vind[b], nind[b], vind[c], nind[c]) if
if (inner - C).sum() < 0 else (inner - C).sum() < 0 else
(vind[a], nind[a], vind[c], nind[c], vind[b], nind[b]) ) ) (vind[a], nind[a], vind[c], nind[c], vind[b], nind[b])))
#obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % ( #obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
# (vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c]) # (vind[a], tind[a], nind[a], vind[b], tind[b], nind[b], vind[c], tind[c], nind[c])
# if (inner - C).sum() < 0 else # if (inner - C).sum() < 0 else
# (vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) ) # (vind[a], tind[a], nind[a], vind[c], tind[c], nind[c], vind[b], tind[b], nind[b]) ) )
def test_convex_hull(): def test_convex_hull():
obj = Obj("convex_test.obj") obj = Obj("convex_test.obj")
vlist = np.random.uniform( low=-0.1, high=+0.1, size=(100,3) ) vlist = np.random.uniform(low=-0.1, high=+0.1, size=(100, 3))
nlist = vlist.copy() nlist = vlist.copy()
tlist = np.random.uniform( low=0, high=+1, size=(100,2) ) tlist = np.random.uniform(low=0, high=+1, size=(100, 2))
vind = [obj.push_v(xyz) for xyz in vlist] vind = [obj.push_v(xyz) for xyz in vlist]
nind = [obj.push_vn(xyz) for xyz in nlist] nind = [obj.push_vn(xyz) for xyz in nlist]
tind = [obj.push_vt(uv) for uv in tlist] tind = [obj.push_vt(uv) for uv in tlist]
convex_hull(vlist, vind, nind, tind, obj) convex_hull(vlist, vind, nind, tind, obj)
class Contour: class Contour:
def __init__(self):
self.vprev_vind = None
def f(self, obj, vlist_vind, vlist_tind, vlist_nind): def __init__(self):
cnt = len(vlist_vind) self.vprev_vind = None
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],
) )
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],
) )
def belt(self, obj, vlist, nlist, tlist): def f(self, obj, vlist_vind, vlist_tind, vlist_nind):
vlist_vind = [obj.push_v(xyz) for xyz in vlist] cnt = len(vlist_vind)
vlist_tind = [obj.push_vt(xyz) for xyz in tlist] for i1 in range(cnt):
vlist_nind = [obj.push_vn(xyz) for xyz in nlist] i2 = i1 - 1
if self.vprev_vind: obj.out.write("f %i/%i/%i %i/%i/%i %i/%i/%i\n" % (
self.f(obj, vlist_vind, vlist_tind, vlist_nind) vlist_vind[i2],
else: vlist_tind[i2],
self.first_vind = vlist_vind vlist_nind[i2],
self.first_tind = vlist_tind vlist_vind[i1],
self.first_nind = vlist_nind vlist_tind[i1],
self.vprev_vind = vlist_vind vlist_nind[i1],
self.vprev_tind = vlist_tind self.vprev_vind[i1],
self.vprev_nind = vlist_nind 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],
))
def belt(self, obj, vlist, nlist, tlist):
vlist_vind = [obj.push_v(xyz) for xyz in vlist]
vlist_tind = [obj.push_vt(xyz) for xyz in tlist]
vlist_nind = [obj.push_vn(xyz) for xyz in nlist]
if self.vprev_vind:
self.f(obj, vlist_vind, vlist_tind, vlist_nind)
else:
self.first_vind = vlist_vind
self.first_tind = vlist_tind
self.first_nind = vlist_nind
self.vprev_vind = vlist_vind
self.vprev_tind = vlist_tind
self.vprev_nind = vlist_nind
def finish(self, obj):
self.f(obj, self.first_vind, self.first_tind, self.first_nind)
def finish(self, obj):
self.f(obj, self.first_vind, self.first_tind, self.first_nind)
def test_contour(): def test_contour():
RAD1 = 2.0 RAD1 = 2.0
RAD2 = 1.5 RAD2 = 1.5
obj = Obj("torus.obj") obj = Obj("torus.obj")
obj.out.write("usemtl porcelain\n") obj.out.write("usemtl porcelain\n")
contour = Contour() contour = Contour()
for step in range(100): for step in range(100):
angle = step/100.0*2*np.pi angle = step / 100.0 * 2 * np.pi
belt_v = [] belt_v = []
belt_n = [] belt_n = []
belt_t = [] belt_t = []
for b in range(50): for b in range(50):
beta = b/50.0*2*np.pi beta = b / 50.0 * 2 * np.pi
r = RAD2*np.cos(beta) + RAD1 r = RAD2 * np.cos(beta) + RAD1
z = RAD2*np.sin(beta) z = RAD2 * np.sin(beta)
belt_v.append( np.array( [ belt_v.append(np.array([np.cos(angle) * r, np.sin(angle) * r, z]))
np.cos(angle)*r, belt_n.append(
np.sin(angle)*r, np.array([np.cos(angle) * np.cos(beta),
z] ) ) np.sin(angle) * np.cos(beta),
belt_n.append( np.array( [ np.sin(beta)]))
np.cos(angle)*np.cos(beta), belt_t.append((0, 0))
np.sin(angle)*np.cos(beta), contour.belt(obj, belt_v, belt_n, belt_t)
np.sin(beta)] ) ) contour.finish(obj)
belt_t.append( (0,0) )
contour.belt(obj, belt_v, belt_n, belt_t)
contour.finish(obj)
#test_convex_hull() #test_convex_hull()
#test_contour() #test_contour()
class RotationFigureParams: class RotationFigureParams:
pass pass
def generate_plate(p, obj, collision_prefix): def generate_plate(p, obj, collision_prefix):
contour = Contour() contour = Contour()
belt_vlist_3d_prev = None belt_vlist_3d_prev = None
for step in range(p.N_VIZ+1): for step in range(p.N_VIZ + 1):
angle = step/float(p.N_VIZ)*2*np.pi angle = step / float(p.N_VIZ) * 2 * np.pi
if step % p.COLLISION_EVERY == 0: if step % p.COLLISION_EVERY == 0:
vlist_3d = [] vlist_3d = []
for x,y in p.belt_simple: for x, y in p.belt_simple:
vlist_3d.append( [ vlist_3d.append([np.cos(angle) * x * 1.06, np.sin(angle) * x * 1.06, y])
np.cos(angle)*x*1.06, if belt_vlist_3d_prev:
np.sin(angle)*x*1.06, obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY))
y obj2.out.write("usemtl pan_tefal\n")
] ) vlist = np.array(vlist_3d + belt_vlist_3d_prev)
if belt_vlist_3d_prev: vlist[len(vlist_3d):] *= 1.01 # break points on one plane
obj2 = Obj(collision_prefix % (step / p.COLLISION_EVERY)) vlist[0, 0:2] += 0.01 * vlist[len(vlist_3d), 0:2]
obj2.out.write("usemtl pan_tefal\n") vlist[len(vlist_3d), 0:2] += 0.01 * vlist[0, 0:2]
vlist = np.array( vlist_3d + belt_vlist_3d_prev ) nlist = np.random.uniform(low=-1, high=+1, size=vlist.shape)
vlist[len(vlist_3d):] *= 1.01 # break points on one plane tlist = np.random.uniform(low=0, high=+1, size=(len(vlist), 2))
vlist[0,0:2] += 0.01*vlist[len(vlist_3d),0:2] vind = [obj2.push_v(xyz) for xyz in vlist]
vlist[len(vlist_3d),0:2] += 0.01*vlist[0,0:2] nind = [obj2.push_vn(xyz) for xyz in nlist]
nlist = np.random.uniform( low=-1, high=+1, size=vlist.shape ) convex_hull(vlist, vind, nind, None, obj2)
tlist = np.random.uniform( low=0, high=+1, size=(len(vlist),2) ) belt_vlist_3d_prev = vlist_3d
vind = [obj2.push_v(xyz) for xyz in vlist] if step == p.N_VIZ: break
nind = [obj2.push_vn(xyz) for xyz in nlist]
convex_hull(vlist, vind, nind, None, obj2)
belt_vlist_3d_prev = vlist_3d
if step==p.N_VIZ: break
belt_v = [] belt_v = []
belt_n = [] belt_n = []
belt_t = [] belt_t = []
for x,y,nx,ny in p.belt: for x, y, nx, ny in p.belt:
belt_v.append( np.array( [ belt_v.append(np.array([np.cos(angle) * x, np.sin(angle) * x, y]))
np.cos(angle)*x, belt_n.append(np.array([np.cos(angle) * nx, np.sin(angle) * nx, ny]))
np.sin(angle)*x, if ny - nx >= 0:
y 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))
belt_n.append( np.array( [ else:
np.cos(angle)*nx, belt_t.append((382.0 / 512 + np.cos(angle) * x / p.RAD_HIGH * 125 / 512,
np.sin(angle)*nx, (512 - 380.0) / 512 + np.sin(angle) * x / p.RAD_HIGH * 125 / 512))
ny contour.belt(obj, belt_v, belt_n, belt_t)
] ) )
if ny-nx >= 0: contour.finish(obj)
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,
(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(): def tefal():
p = RotationFigureParams() p = RotationFigureParams()
p.RAD_LOW = 0.240/2 p.RAD_LOW = 0.240 / 2
p.RAD_HIGH = 0.255/2 p.RAD_HIGH = 0.255 / 2
p.H = 0.075 p.H = 0.075
p.THICK = 0.005 p.THICK = 0.005
p.N_VIZ = 30 p.N_VIZ = 30
p.COLLISION_EVERY = 5 p.COLLISION_EVERY = 5
p.belt = [ p.belt = [
(p.RAD_HIGH-p.THICK, p.H, -1,0), # x y norm (p.RAD_HIGH - p.THICK, p.H, -1, 0), # x y norm
(p.RAD_HIGH , p.H, 0,1), (p.RAD_HIGH, p.H, 0, 1),
(p.RAD_HIGH+p.THICK, p.H, +1,0), (p.RAD_HIGH + p.THICK, p.H, +1, 0),
(p.RAD_LOW+p.THICK, p.THICK, +1,0), (p.RAD_LOW + p.THICK, p.THICK, +1, 0),
(p.RAD_LOW , 0, 0,-1), (p.RAD_LOW, 0, 0, -1),
( 0, 0, 0,-1), (0, 0, 0, -1),
( 0, p.THICK, 0,1), (0, p.THICK, 0, 1),
(p.RAD_LOW-p.THICK, p.THICK, 0,1), (p.RAD_LOW - p.THICK, p.THICK, 0, 1),
(p.RAD_LOW-p.THICK, 3*p.THICK,-1,0), (p.RAD_LOW - p.THICK, 3 * p.THICK, -1, 0),
] ]
p.belt.reverse() p.belt.reverse()
p.belt_simple = [ p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
(p.RAD_HIGH-p.THICK, p.H), (p.RAD_LOW - p.THICK, 0)]
(p.RAD_HIGH+p.THICK, p.H), obj = Obj("pan_tefal.obj")
(p.RAD_LOW , 0), obj.out.write("usemtl pan_tefal\n")
(p.RAD_LOW-p.THICK , 0) generate_plate(p, obj, "pan_tefal-collision%02i.obj")
]
obj = Obj("pan_tefal.obj")
obj.out.write("usemtl pan_tefal\n")
generate_plate(p, obj, "pan_tefal-collision%02i.obj")
def plate(): def plate():
p = RotationFigureParams() p = RotationFigureParams()
p.RAD_LOW = 0.110/2 p.RAD_LOW = 0.110 / 2
p.RAD_HIGH = 0.190/2 p.RAD_HIGH = 0.190 / 2
p.H = 0.060 p.H = 0.060
p.THICK = 0.003 p.THICK = 0.003
p.N_VIZ = 30 p.N_VIZ = 30
p.COLLISION_EVERY = 5 p.COLLISION_EVERY = 5
p.belt = [ p.belt = [
(p.RAD_HIGH-p.THICK, p.H, -0.9,0.5), # x y norm (p.RAD_HIGH - p.THICK, p.H, -0.9, 0.5), # x y norm
(p.RAD_HIGH , p.H, 0,1), (p.RAD_HIGH, p.H, 0, 1),
(p.RAD_HIGH+p.THICK, p.H, +1,0), (p.RAD_HIGH + p.THICK, p.H, +1, 0),
(p.RAD_LOW+p.THICK, p.THICK, +1,0), (p.RAD_LOW + p.THICK, p.THICK, +1, 0),
(p.RAD_LOW , 0, 0,-1), (p.RAD_LOW, 0, 0, -1),
( 0, 0, 0,-1), (0, 0, 0, -1),
( 0, p.THICK, 0,1), (0, p.THICK, 0, 1),
(p.RAD_LOW-3*p.THICK, p.THICK, 0,1), (p.RAD_LOW - 3 * p.THICK, p.THICK, 0, 1),
(p.RAD_LOW-p.THICK, 3*p.THICK,-0.5,1.0), (p.RAD_LOW - p.THICK, 3 * p.THICK, -0.5, 1.0),
] ]
p.belt.reverse() p.belt.reverse()
p.belt_simple = [ p.belt_simple = [(p.RAD_HIGH - p.THICK, p.H), (p.RAD_HIGH + p.THICK, p.H), (p.RAD_LOW, 0),
(p.RAD_HIGH-p.THICK, p.H), (p.RAD_LOW - p.THICK, 0)]
(p.RAD_HIGH+p.THICK, p.H), obj = Obj("plate.obj")
(p.RAD_LOW , 0), obj.out.write("usemtl solid_color\n")
(p.RAD_LOW-p.THICK , 0) generate_plate(p, obj, "plate-collision%02i.obj")
]
obj = Obj("plate.obj")
obj.out.write("usemtl solid_color\n")
generate_plate(p, obj, "plate-collision%02i.obj")
plate() plate()

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

@@ -3,51 +3,44 @@ import time
import math import math
pi = 3.14159264 pi = 3.14159264
limitVal = 2*pi limitVal = 2 * pi
legpos = 3./4.*pi legpos = 3. / 4. * pi
legposS = 0 legposS = 0
legposSright = 0#-0.3 legposSright = 0 #-0.3
legposSleft = 0#0.3 legposSleft = 0 #0.3
defaultERP=0.4 defaultERP = 0.4
maxMotorForce = 5000 maxMotorForce = 5000
maxGearForce = 10000 maxGearForce = 10000
jointFriction = 0.1 jointFriction = 0.1
p.connect(p.GUI) p.connect(p.GUI)
amplitudeId = p.addUserDebugParameter("amplitude", 0, 3.14, 0.5)
timeScaleId = p.addUserDebugParameter("timeScale", 0, 10, 1)
amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5) jointTypeNames = {}
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1) jointTypeNames[p.JOINT_REVOLUTE] = "JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED] = "JOINT_FIXED"
jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100) p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True) p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
jointNamesToIndex = {}
jointNamesToIndex={} p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
vision = p.loadURDF("vision60.urdf", [0, 0, 0.4], useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
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)): for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision,j) jointInfo = p.getJointInfo(vision, j)
jointInfoName = jointInfo[1].decode("utf-8") jointInfoName = jointInfo[1].decode("utf-8")
print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]]) print("joint ", j, " = ", jointInfoName, "type=", jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName ]=j jointNamesToIndex[jointInfoName] = j
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName]) #print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction) p.setJointMotorControl2(vision, j, p.VELOCITY_CONTROL, targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center'] chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint'] motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
@@ -72,149 +65,277 @@ knee_back_leftL_joint = jointNamesToIndex['knee_back_leftL_joint']
motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint'] motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint']
motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint'] motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint']
motA_rf_Id= p.addUserDebugParameter("motA_rf",-limitVal,limitVal,legpos) motA_rf_Id = p.addUserDebugParameter("motA_rf", -limitVal, limitVal, legpos)
motB_rf_Id= p.addUserDebugParameter("motB_rf",-limitVal,limitVal,legpos) motB_rf_Id = p.addUserDebugParameter("motB_rf", -limitVal, limitVal, legpos)
motC_rf_Id= p.addUserDebugParameter("motC_rf",-limitVal,limitVal,legposSright) motC_rf_Id = p.addUserDebugParameter("motC_rf", -limitVal, limitVal, legposSright)
erp_rf_Id= p.addUserDebugParameter("erp_rf",0,1,defaultERP) erp_rf_Id = p.addUserDebugParameter("erp_rf", 0, 1, defaultERP)
relPosTarget_rf_Id= p.addUserDebugParameter("relPosTarget_rf",-limitVal,limitVal,0) relPosTarget_rf_Id = p.addUserDebugParameter("relPosTarget_rf", -limitVal, limitVal, 0)
motA_lf_Id = p.addUserDebugParameter("motA_lf", -limitVal, limitVal, -legpos)
motB_lf_Id = p.addUserDebugParameter("motB_lf", -limitVal, limitVal, -legpos)
motC_lf_Id = p.addUserDebugParameter("motC_lf", -limitVal, limitVal, legposSleft)
motA_lf_Id= p.addUserDebugParameter("motA_lf",-limitVal,limitVal,-legpos) erp_lf_Id = p.addUserDebugParameter("erp_lf", 0, 1, defaultERP)
motB_lf_Id= p.addUserDebugParameter("motB_lf",-limitVal,limitVal,-legpos) relPosTarget_lf_Id = p.addUserDebugParameter("relPosTarget_lf", -limitVal, limitVal, 0)
motC_lf_Id= p.addUserDebugParameter("motC_lf",-limitVal,limitVal,legposSleft)
erp_lf_Id= p.addUserDebugParameter("erp_lf",0,1,defaultERP) motA_rb_Id = p.addUserDebugParameter("motA_rb", -limitVal, limitVal, legpos)
relPosTarget_lf_Id= p.addUserDebugParameter("relPosTarget_lf",-limitVal,limitVal,0) motB_rb_Id = p.addUserDebugParameter("motB_rb", -limitVal, limitVal, legpos)
motC_rb_Id = p.addUserDebugParameter("motC_rb", -limitVal, limitVal, legposSright)
motA_rb_Id= p.addUserDebugParameter("motA_rb",-limitVal,limitVal,legpos) erp_rb_Id = p.addUserDebugParameter("erp_rb", 0, 1, defaultERP)
motB_rb_Id= p.addUserDebugParameter("motB_rb",-limitVal,limitVal,legpos) relPosTarget_rb_Id = p.addUserDebugParameter("relPosTarget_rb", -limitVal, limitVal, 0)
motC_rb_Id= p.addUserDebugParameter("motC_rb",-limitVal,limitVal,legposSright)
erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP) motA_lb_Id = p.addUserDebugParameter("motA_lb", -limitVal, limitVal, -legpos)
relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0) motB_lb_Id = p.addUserDebugParameter("motB_lb", -limitVal, limitVal, -legpos)
motC_lb_Id = p.addUserDebugParameter("motC_lb", -limitVal, limitVal, legposSleft)
erp_lb_Id = p.addUserDebugParameter("erp_lb", 0, 1, defaultERP)
relPosTarget_lb_Id = p.addUserDebugParameter("relPosTarget_lb", -limitVal, limitVal, 0)
motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos) camTargetPos = [0.25, 0.62, -0.15]
motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos)
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)
erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
camTargetPos=[0.25,0.62,-0.15]
camDist = 2 camDist = 2
camYaw = -2 camYaw = -2
camPitch=-16 camPitch = -16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos) p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,
knee_front_rightR_joint,
vision,
motor_front_rightL_joint,
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])
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])
p.changeConstraint(c_rb, gearRatio=-1, gearAuxLink=motor_back_rightR_joint, maxForce=maxGearForce)
c_lb = p.createConstraint(vision,
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]) knee_back_leftL_joint,
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce) vision,
motor_back_leftR_joint,
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]) jointType=p.JOINT_GEAR,
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce) jointAxis=[0, 1, 0],
parentFramePosition=[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]) childFramePosition=[0, 0, 0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce) p.changeConstraint(c_lb, gearRatio=-1, gearAuxLink=motor_back_leftL_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])
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
for i in range (1): for i in range(1):
#while (1): #while (1):
motA_rf = p.readUserDebugParameter(motA_rf_Id) motA_rf = p.readUserDebugParameter(motA_rf_Id)
motB_rf = p.readUserDebugParameter(motB_rf_Id) motB_rf = p.readUserDebugParameter(motB_rf_Id)
motC_rf = p.readUserDebugParameter(motC_rf_Id) motC_rf = p.readUserDebugParameter(motC_rf_Id)
erp_rf = p.readUserDebugParameter(erp_rf_Id) erp_rf = p.readUserDebugParameter(erp_rf_Id)
relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id) relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
#motC_rf #motC_rf
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce) motor_front_rightR_joint,
p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce) targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.changeConstraint(c_rf,
gearRatio=-1,
gearAuxLink=motor_front_rightR_joint,
erp=erp_rf,
relativePositionTarget=relPosTarget_rf,
maxForce=maxGearForce)
motA_lf = p.readUserDebugParameter(motA_lf_Id) motA_lf = p.readUserDebugParameter(motA_lf_Id)
motB_lf = p.readUserDebugParameter(motB_lf_Id) motB_lf = p.readUserDebugParameter(motB_lf_Id)
motC_lf = p.readUserDebugParameter(motC_lf_Id) motC_lf = p.readUserDebugParameter(motC_lf_Id)
erp_lf = p.readUserDebugParameter(erp_lf_Id) erp_lf = p.readUserDebugParameter(erp_lf_Id)
relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id) relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce) motor_front_leftL_joint,
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,erp=erp_lf, relativePositionTarget=relPosTarget_lf,maxForce=maxGearForce) targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.changeConstraint(c_lf,
gearRatio=-1,
gearAuxLink=motor_front_leftL_joint,
erp=erp_lf,
relativePositionTarget=relPosTarget_lf,
maxForce=maxGearForce)
motA_rb = p.readUserDebugParameter(motA_rb_Id)
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)
motA_rb = p.readUserDebugParameter(motA_rb_Id) motA_lb = p.readUserDebugParameter(motA_lb_Id)
motB_rb = p.readUserDebugParameter(motB_rb_Id) motB_lb = p.readUserDebugParameter(motB_lb_Id)
motC_rb = p.readUserDebugParameter(motC_rb_Id) motC_lb = p.readUserDebugParameter(motC_lb_Id)
erp_rb = p.readUserDebugParameter(erp_rb_Id) erp_lb = p.readUserDebugParameter(erp_lb_Id)
relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id) relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce) motor_back_leftL_joint,
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce) p.POSITION_CONTROL,
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,erp=erp_rb, relativePositionTarget=relPosTarget_rb,maxForce=maxGearForce) targetPosition=motA_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)
motA_lb = p.readUserDebugParameter(motA_lb_Id) p.setGravity(0, 0, -10)
motB_lb = p.readUserDebugParameter(motB_lb_Id) time.sleep(1. / 240.)
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.setGravity(0,0,-10)
time.sleep(1./240.)
t = 0 t = 0
prevTime = time.time() prevTime = time.time()
while (1): while (1):
timeScale = p.readUserDebugParameter(timeScaleId) timeScale = p.readUserDebugParameter(timeScaleId)
amplitude = p.readUserDebugParameter(amplitudeId) amplitude = p.readUserDebugParameter(amplitudeId)
newTime = time.time() newTime = time.time()
dt = (newTime-prevTime)*timeScale dt = (newTime - prevTime) * timeScale
t = t+dt t = t + dt
prevTime = newTime prevTime = newTime
amp=amplitude amp = amplitude
motA_rf = math.sin(t)*amp+legpos motA_rf = math.sin(t) * amp + legpos
motA_rb = math.sin(t)*amp+legpos motA_rb = math.sin(t) * amp + legpos
motA_lf = -(math.sin(t)*amp+legpos) motA_lf = -(math.sin(t) * amp + legpos)
motA_lb = -(math.sin(t)*amp+legpos) motA_lb = -(math.sin(t) * amp + legpos)
motB_rf = math.sin(t)*amp+legpos motB_rf = math.sin(t) * amp + legpos
motB_rb = math.sin(t)*amp+legpos motB_rb = math.sin(t) * amp + legpos
motB_lf = -(math.sin(t)*amp+legpos) motB_lf = -(math.sin(t) * amp + legpos)
motB_lb = -(math.sin(t)*amp+legpos) motB_lb = -(math.sin(t) * amp + legpos)
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,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,
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce) motor_front_rightR_joint,
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce) p.POSITION_CONTROL,
targetPosition=motA_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightL_joint,
p.POSITION_CONTROL,
targetPosition=motB_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_rightS_joint,
p.POSITION_CONTROL,
targetPosition=motC_rf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftL_joint,
p.POSITION_CONTROL,
targetPosition=motA_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftR_joint,
p.POSITION_CONTROL,
targetPosition=motB_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,
motor_front_leftS_joint,
p.POSITION_CONTROL,
targetPosition=motC_lf,
force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce) p.setJointMotorControl2(vision,
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce) motor_back_rightR_joint,
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce) p.POSITION_CONTROL,
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.setGravity(0, 0, -10)
p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce) time.sleep(1. / 240.)
p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
p.setGravity(0,0,-10)
time.sleep(1./240.)

87
data/terrain.py
View File

@@ -2,58 +2,57 @@ import math
NUM_VERTS_X = 30 NUM_VERTS_X = 30
NUM_VERTS_Y = 30 NUM_VERTS_Y = 30
totalVerts = NUM_VERTS_X*NUM_VERTS_Y totalVerts = NUM_VERTS_X * NUM_VERTS_Y
totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1) totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1)
offset = -50.0 offset = -50.0
TRIANGLE_SIZE = 1. TRIANGLE_SIZE = 1.
waveheight=0.1 waveheight = 0.1
gGroundVertices = [None] * totalVerts*3 gGroundVertices = [None] * totalVerts * 3
gGroundIndices = [None] * totalTriangles*3 gGroundIndices = [None] * totalTriangles * 3
i=0 i = 0
for i in range (NUM_VERTS_X): for i in range(NUM_VERTS_X):
for j in range (NUM_VERTS_Y): for j in range(NUM_VERTS_Y):
gGroundVertices[(i+j*NUM_VERTS_X)*3+0] = (i-NUM_VERTS_X*0.5)*TRIANGLE_SIZE gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 0] = (i - NUM_VERTS_X * 0.5) * TRIANGLE_SIZE
gGroundVertices[(i+j*NUM_VERTS_X)*3+1] = (j-NUM_VERTS_Y*0.5)*TRIANGLE_SIZE gGroundVertices[(i + j * NUM_VERTS_X) * 3 + 1] = (j - NUM_VERTS_Y * 0.5) * TRIANGLE_SIZE
gGroundVertices[(i+j*NUM_VERTS_X)*3+2] = waveheight*math.sin(float(i))*math.cos(float(j)+offset) gGroundVertices[(i + j * NUM_VERTS_X) * 3 +
2] = waveheight * math.sin(float(i)) * math.cos(float(j) + offset)
index=0
for i in range (NUM_VERTS_X-1): index = 0
for j in range (NUM_VERTS_Y-1): for i in range(NUM_VERTS_X - 1):
gGroundIndices[index] = 1+j*NUM_VERTS_X+i for j in range(NUM_VERTS_Y - 1):
index+=1 gGroundIndices[index] = 1 + j * NUM_VERTS_X + i
gGroundIndices[index] = 1+j*NUM_VERTS_X+i+1 index += 1
index+=1 gGroundIndices[index] = 1 + j * NUM_VERTS_X + i + 1
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1 index += 1
index+=1 gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i + 1
gGroundIndices[index] = 1+j*NUM_VERTS_X+i index += 1
index+=1 gGroundIndices[index] = 1 + j * NUM_VERTS_X + i
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i+1 index += 1
index+=1 gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i + 1
gGroundIndices[index] = 1+(j+1)*NUM_VERTS_X+i index += 1
index+=1 gGroundIndices[index] = 1 + (j + 1) * NUM_VERTS_X + i
index += 1
#print(gGroundVertices) #print(gGroundVertices)
#print(gGroundIndices) #print(gGroundIndices)
print("o Terrain") print("o Terrain")
for i in range (totalVerts): for i in range(totalVerts):
print("v "), print("v "),
print(gGroundVertices[i*3+0]), print(gGroundVertices[i * 3 + 0]),
print(" "), print(" "),
print(gGroundVertices[i*3+1]), print(gGroundVertices[i * 3 + 1]),
print(" "), print(" "),
print(gGroundVertices[i*3+2]) print(gGroundVertices[i * 3 + 2])
for i in range (totalTriangles):
print("f "),
print(gGroundIndices[i*3+0]),
print(" "),
print(gGroundIndices[i*3+1]),
print(" "),
print(gGroundIndices[i*3+2]),
print(" ")
for i in range(totalTriangles):
print("f "),
print(gGroundIndices[i * 3 + 0]),
print(" "),
print(gGroundIndices[i * 3 + 1]),
print(" "),
print(gGroundIndices[i * 3 + 2]),
print(" ")

5
data/threecubes/threecubes.py
View File

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

34
data/threecubes/threecubes_org.py
View File

@@ -3,20 +3,30 @@ useEGL = False
useEGLGUI = False useEGLGUI = False
if useEGL: if useEGL:
if useEGLGUI: if useEGLGUI:
p.connect(p.GUI, "window_backend=2") p.connect(p.GUI, "window_backend=2")
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin") p.loadPlugin("eglRendererPlugin")
else: else:
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL) p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1): while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
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 ) viewmat = [
p.stepSimulation() 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.stepSimulation()

1031
data/xacro_standalone.py
View File

File diff suppressed because it is too large Load Diff

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

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

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

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

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

@@ -10,69 +10,86 @@ import pybullet_pb2_grpc
#todo: how to add this? #todo: how to add this?
MJCF_COLORS_FROM_FILE = 512 MJCF_COLORS_FROM_FILE = 512
def run(): def run():
print("grpc.insecure_channel") print("grpc.insecure_channel")
channel = grpc.insecure_channel('localhost:6667') channel = grpc.insecure_channel('localhost:6667')
print("pybullet_pb2_grpc.PyBulletAPIStub") print("pybullet_pb2_grpc.PyBulletAPIStub")
stub = pybullet_pb2_grpc.PyBulletAPIStub(channel) stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
response=0 response = 0
print("submit CheckVersionCommand") print("submit CheckVersionCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(clientVersion=123))) response = stub.SubmitCommand(
print("PyBullet client received: " , response) 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()))
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)))
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)))
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)))
print("PyBullet client received: ", response)
print("submit ChangeDynamicsCommand ")
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)))
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])))
print("PyBullet client received: ", response)
print("submit RequestActualStateCommand")
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()))
print("PyBullet client received: ", response.statusType)
print("submit_ResetSimulationCommand") #print("TerminateServerCommand")
response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(resetSimulationCommand=pybullet_pb2.ResetSimulationCommand())) #response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
print("PyBullet client received: ", response) #print("PyBullet client received: " , response.statusType)
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)))
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)))
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)))
print("PyBullet client received: " , response)
print("submit ChangeDynamicsCommand ")
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)))
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])))
print("PyBullet client received: " , response)
print("submit RequestActualStateCommand")
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()))
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__': if __name__ == '__main__':
run() run()

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

@@ -2,74 +2,100 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() def getRayFromTo(mouseX, mouseY):
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(
farPlane = 10000 )
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] camPos = [
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] camTarget[2] - dist * camForward[2]
rayFrom = camPos ]
oneOverWidth = float(1)/float(width) farPlane = 10000
oneOverHeight = float(1)/float(height) rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] rayForward[1] + rayForward[2] * rayForward[2]))
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom = camPos
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], oneOverWidth = float(1) / float(width)
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] oneOverHeight = float(1) / float(height)
return rayFrom,rayTo 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]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True) p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1) p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here #disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0,-0.02,0] shift = [0, -0.02, 0]
meshScale=[0.1,0.1,0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) fileName="duck.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="duck_vhacd.obj",
collisionFramePosition=shift,
meshScale=meshScale)
rangex = 3 rangex = 3
rangey = 3 rangey = 3
for i in range (rangex): for i in range(rangex):
for j in range (rangey ): for j in range(rangey):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) p.createMultiBody(baseMass=1,
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,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.stopStateLogging(logId)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
while (1): while (1):
mouseEvents = p.getMouseEvents() mouseEvents = p.getMouseEvents()
for e in mouseEvents: for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1] mouseX = e[1]
mouseY = e[2] mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY) rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom,rayTo) rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)): for l in range(len(rayInfo)):
hit = rayInfo[l] hit = rayInfo[l]
objectUid = hit[0] objectUid = hit[0]
if (objectUid>=1): if (objectUid >= 1):
#p.removeBody(objectUid) #p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor+=1 currentColor += 1
if (currentColor>=len(colors)): if (currentColor >= len(colors)):
currentColor=0 currentColor = 0

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

@@ -5,73 +5,70 @@ import math
useGui = True useGui = True
if (useGui): if (useGui):
p.connect(p.GUI) p.connect(p.GUI)
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.loadURDF("samurai.urdf") #p.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf",[3,3,1]) p.loadURDF("r2d2.urdf", [3, 3, 1])
rayFrom = []
rayFrom=[] rayTo = []
rayTo=[] rayIds = []
rayIds=[]
numRays = 1024 numRays = 1024
rayLen = 13 rayLen = 13
rayHitColor = [1, 0, 0]
rayHitColor = [1,0,0] rayMissColor = [0, 1, 0]
rayMissColor = [0,1,0]
replaceLines = True replaceLines = True
for i in range (numRays): for i in range(numRays):
rayFrom.append([0,0,1]) rayFrom.append([0, 0, 1])
rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) rayTo.append([
if (replaceLines): rayLen * math.sin(2. * math.pi * float(i) / numRays),
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
else: ])
rayIds.append(-1) if (replaceLines):
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
else:
rayIds.append(-1)
if (not useGui): if (not useGui):
timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "rayCastBench.json")
numSteps = 10 numSteps = 10
if (useGui): if (useGui):
numSteps = 327680 numSteps = 327680
for i in range(numSteps):
p.stepSimulation()
for j in range(8):
results = p.rayTestBatch(rayFrom, rayTo, j + 1)
#for i in range (10):
# p.removeAllUserDebugItems()
if (useGui):
if (not replaceLines):
p.removeAllUserDebugItems()
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])
else:
hitPosition = results[i][3]
p.addUserDebugLine(rayFrom[i], hitPosition, rayHitColor, replaceItemUniqueId=rayIds[i])
#time.sleep(1./240.)
for i in range (numSteps):
p.stepSimulation()
for j in range (8):
results = p.rayTestBatch(rayFrom,rayTo,j+1)
#for i in range (10):
# p.removeAllUserDebugItems()
if (useGui):
if (not replaceLines):
p.removeAllUserDebugItems()
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])
else:
hitPosition = results[i][3]
p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor,replaceItemUniqueId=rayIds[i])
#time.sleep(1./240.)
if (not useGui): if (not useGui):
p.stopStateLogging(timingLog) p.stopStateLogging(timingLog)

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

@@ -4,28 +4,26 @@ import os
import time import time
GRAVITY = -9.8 GRAVITY = -9.8
dt = 1e-3 dt = 1e-3
iters=2000 iters = 2000
physicsClient = p.connect(p.GUI) physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation() p.resetSimulation()
#p.setRealTimeSimulation(True) #p.setRealTimeSimulation(True)
p.setGravity(0,0,GRAVITY) p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt) p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf") planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1.13] cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0]) cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
cubeStartPos,
cubeStartOrientation)
#disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce=1
for joint in range (p.getNumJoints(botId)):
p.setJointMotorControl2(botId,joint,p.POSITION_CONTROL,force=jointFrictionForce)
#for i in range(10000): #disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce = 1
for joint in range(p.getNumJoints(botId)):
p.setJointMotorControl2(botId, joint, p.POSITION_CONTROL, force=jointFrictionForce)
#for i in range(10000):
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) # p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
# p.stepSimulation() # p.stepSimulation()
#import ipdb #import ipdb
@@ -33,8 +31,8 @@ for joint in range (p.getNumJoints(botId)):
import time import time
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while (1): while (1):
#p.stepSimulation() #p.stepSimulation()
#p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) #p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
p.setGravity(0,0,GRAVITY) p.setGravity(0, 0, GRAVITY)
time.sleep(1/240.) time.sleep(1 / 240.)
time.sleep(1000) time.sleep(1000)

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

@@ -2,14 +2,14 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
cube2 = p.loadURDF("cube.urdf",[0,0,3], useFixedBase=True) cube2 = p.loadURDF("cube.urdf", [0, 0, 3], useFixedBase=True)
cube = p.loadURDF("cube.urdf", useFixedBase=True) cube = p.loadURDF("cube.urdf", useFixedBase=True)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
timeStep = 1./240. timeStep = 1. / 240.
p.setTimeStep(timeStep) p.setTimeStep(timeStep)
p.changeDynamics(cube2,-1,mass=1) p.changeDynamics(cube2, -1, mass=1)
#now cube2 will have a floating base and move #now cube2 will have a floating base and move
while (p.isConnected()): while (p.isConnected()):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

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

@@ -1,43 +1,41 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0]) planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0])
planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1]) planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1])
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
p.changeVisualShape(planeUidA,-1,rgbaColor=[1,1,1,0.5]) p.changeVisualShape(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid,-1,rgbaColor=[1,1,1,0.5]) p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid,-1, textureUniqueId = texUid) p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
width = 256 width = 256
height = 256 height = 256
pixels = [255]*width*height*3 pixels = [255] * width * height * 3
colorR = 0 colorR = 0
colorG = 0 colorG = 0
colorB = 0 colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) #p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
blue=0 blue = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
for i in range (100000): for i in range(100000):
p.stepSimulation() p.stepSimulation()
for i in range (width): for i in range(width):
for j in range(height): for j in range(height):
pixels[(i+j*width)*3+0]=i pixels[(i + j * width) * 3 + 0] = i
pixels[(i+j*width)*3+1]=(j+blue)%256 pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
pixels[(i+j*width)*3+2]=blue pixels[(i + j * width) * 3 + 2] = blue
blue=blue+1 blue = blue + 1
p.changeTexture(texUid, pixels,width,height) p.changeTexture(texUid, pixels, width, height)
start = time.time() start = time.time()
p.getCameraImage(300,300,renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL)
end = time.time() end = time.time()
print("rendering duraction") print("rendering duraction")
print(end-start) print(end - start)
p.stopStateLogging(logId) p.stopStateLogging(logId)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) #p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1) #p.configureDebugVisualizer(p.COV_ENABLE_GUI,1)

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

@@ -2,17 +2,17 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False) planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
cubeId = p.loadURDF("cube_collisionfilter.urdf", [0,0,3], useMaximalCoordinates=False) cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False)
collisionFilterGroup = 0 collisionFilterGroup = 0
collisionFilterMask = 0 collisionFilterMask = 0
p.setCollisionFilterGroupMask(cubeId,-1,collisionFilterGroup,collisionFilterMask) p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask)
enableCollision = 1 enableCollision = 1
p.setCollisionFilterPair(planeId, cubeId,-1,-1,enableCollision ) p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
while (p.isConnected()): while (p.isConnected()):
time.sleep(1./240.) time.sleep(1. / 240.)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)

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

@@ -2,14 +2,13 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS,"commandLog.bin") logId = p.startStateLogging(p.STATE_LOGGING_ALL_COMMANDS, "commandLog.bin")
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
p.loadURDF("r2d2.urdf",[0,0,1]) p.loadURDF("r2d2.urdf", [0, 0, 1])
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.resetSimulation(); p.resetSimulation()
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS,"commandLog.bin")
while(p.isConnected()):
time.sleep(1./240.)
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS, "commandLog.bin")
while (p.isConnected()):
time.sleep(1. / 240.)

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

@@ -5,22 +5,22 @@ import math
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube_small.urdf",0,0,1) cubeId = p.loadURDF("cube_small.urdf", 0, 0, 1)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1]) cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
print (cid) print(cid)
print (p.getConstraintUniqueId(0)) print(p.getConstraintUniqueId(0))
prev=[0,0,1] prev = [0, 0, 1]
a=-math.pi a = -math.pi
while 1: while 1:
a=a+0.01 a = a + 0.01
if (a>math.pi): if (a > math.pi):
a=-math.pi a = -math.pi
time.sleep(.01) time.sleep(.01)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
pivot=[a,0,1] pivot = [a, 0, 1]
orn = p.getQuaternionFromEuler([a,0,0]) orn = p.getQuaternionFromEuler([a, 0, 0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50) p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid) p.removeConstraint(cid)

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

@@ -1,34 +1,29 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
useMaximalCoordinates = False useMaximalCoordinates = False
p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates ) p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("sphere2.urdf",[0,0,1]) #p.loadURDF("sphere2.urdf",[0,0,1])
p.loadURDF("cube.urdf",[0,0,1], useMaximalCoordinates=useMaximalCoordinates ) p.loadURDF("cube.urdf", [0, 0, 1], useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0,3,-10) p.setGravity(0, 3, -10)
while(1): while (1):
p.stepSimulation() p.stepSimulation()
pts = p.getContactPoints() pts = p.getContactPoints()
print("num pts=",len(pts))
totalNormalForce = 0
totalFrictionForce = [0,0,0]
totalLateralFrictionForce=[0,0,0]
for pt in pts:
#print("pt.normal=",pt[7])
#print("pt.normalForce=",pt[9])
totalNormalForce += pt[9]
#print("pt.lateralFrictionA=",pt[10])
#print("pt.lateralFrictionADir=",pt[11])
#print("pt.lateralFrictionB=",pt[12])
#print("pt.lateralFrictionBDir=",pt[13])
totalLateralFrictionForce[0]+=pt[11][0]*pt[10]+pt[13][0]*pt[12]
totalLateralFrictionForce[1]+=pt[11][1]*pt[10]+pt[13][1]*pt[12]
totalLateralFrictionForce[2]+=pt[11][2]*pt[10]+pt[13][2]*pt[12]
print("num pts=", len(pts))
print("totalNormalForce=",totalNormalForce) totalNormalForce = 0
print("totalLateralFrictionForce=",totalLateralFrictionForce) totalFrictionForce = [0, 0, 0]
totalLateralFrictionForce = [0, 0, 0]
for pt in pts:
#print("pt.normal=",pt[7])
#print("pt.normalForce=",pt[9])
totalNormalForce += pt[9]
#print("pt.lateralFrictionA=",pt[10])
#print("pt.lateralFrictionADir=",pt[11])
#print("pt.lateralFrictionB=",pt[12])
#print("pt.lateralFrictionBDir=",pt[13])
totalLateralFrictionForce[0] += pt[11][0] * pt[10] + pt[13][0] * pt[12]
totalLateralFrictionForce[1] += pt[11][1] * pt[10] + pt[13][1] * pt[12]
totalLateralFrictionForce[2] += pt[11][2] * pt[10] + pt[13][2] * pt[12]
print("totalNormalForce=", totalNormalForce)
print("totalLateralFrictionForce=", totalLateralFrictionForce)

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

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

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

@@ -3,145 +3,140 @@ import time
import math import math
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1./120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True) p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here #disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0,-0.02,0] shift = [0, -0.02, 0]
meshScale=[0.1,0.1,0.1] meshScale = [0.1, 0.1, 0.1]
vertices=[ vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000,-1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000,1.000000,1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000,-1.000000,-1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000,1.000000,-1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,-1.000000], [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000,1.000000,1.000000], [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000], normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[1.000000,1.000000,-1.000000], [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[1.000000,1.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[1.000000,-1.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[-1.000000,-1.000000,1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[1.000000,-1.000000,1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[1.000000,-1.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[-1.000000,1.000000,-1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,1.000000,1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[1.000000,1.000000,1.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[1.000000,1.000000,-1.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]]
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
] ]
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]
]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) #p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
batchPositions = []
batchPositions=[] for x in range(32):
for 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])
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
for x in range (32): baseCollisionShapeIndex=collisionShapeId,
for y in range (32): baseVisualShapeIndex=visualShapeId,
for z in range (10): basePosition=[0, 0, 2],
batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5]) batchPositions=batchPositions,
useMaximalCoordinates=True)
bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
p.syncBodyInfo() p.syncBodyInfo()
print("numBodies=",p.getNumBodies()) print("numBodies=", p.getNumBodies())
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
while (1): while (1):
p.stepSimulation() p.stepSimulation()
#time.sleep(1./120.) #time.sleep(1./120.)
#p.getCameraImage(320,200) #p.getCameraImage(320,200)

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

@@ -1,56 +1,73 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE) p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,0) p.createMultiBody(0, 0)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
link_Masses=[1] linkVisualShapeIndices = [-1]
linkCollisionShapeIndices=[colBoxId] linkPositions = [[0, 0, 0.11]]
linkVisualShapeIndices=[-1] linkOrientations = [[0, 0, 0, 1]]
linkPositions=[[0,0,0.11]] linkInertialFramePositions = [[0, 0, 0]]
linkOrientations=[[0,0,0,1]] linkInertialFrameOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions=[[0,0,0]] indices = [0]
linkInertialFrameOrientations=[[0,0,0,1]] jointTypes = [p.JOINT_REVOLUTE]
indices=[0] axis = [[0, 0, 1]]
jointTypes=[p.JOINT_REVOLUTE]
axis=[[0,0,1]]
for i in range (3): for i in range(3):
for j in range (3): for j in range(3):
for k in range (3): for k in range(3):
basePosition = [1+i*5*sphereRadius,1+j*5*sphereRadius,1+k*5*sphereRadius+1] basePosition = [
baseOrientation = [0,0,0,1] 1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
if (k&2): ]
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation) baseOrientation = [0, 0, 0, 1]
else: if (k & 2):
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, colSphereId, visualShapeId, basePosition,
baseOrientation)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) else:
for joint in range (p.getNumJoints(sphereUid)): sphereUid = p.createMultiBody(mass,
p.setJointMotorControl2(sphereUid,joint,p.VELOCITY_CONTROL,targetVelocity=1,force=10) 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)
for joint in range(p.getNumJoints(sphereUid)):
p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.getNumJoints(sphereUid) p.getNumJoints(sphereUid)
for i in range (p.getNumJoints(sphereUid)): for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid,i) p.getJointInfo(sphereUid, i)
while (1):
keys = p.getKeyboardEvents()
print(keys)
time.sleep(0.01) while (1):
keys = p.getKeyboardEvents()
print(keys)
time.sleep(0.01)

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

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

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

@@ -5,34 +5,45 @@ useMaximalCoordinates = 0
p.connect(p.GUI) p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates) #p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH) monastryId = concaveEnv = p.createCollisionShape(p.GEOM_MESH,
orn = p.getQuaternionFromEuler([1.5707963,0,0]) fileName="samurai_monastry.obj",
p.createMultiBody (0,monastryId, baseOrientation=orn) flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963, 0, 0])
p.createMultiBody(0, monastryId, baseOrientation=orn)
sphereRadius = 0.05 sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
for i in range(5):
for 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)
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)
for i in range (5): p.setGravity(0, 0, -10)
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)
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.setGravity(0,0,-10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while (1): while (1):
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
#print(keys) #print(keys)
time.sleep(0.01) time.sleep(0.01)

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

@@ -2,167 +2,180 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() def getRayFromTo(mouseX, mouseY):
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(
farPlane = 10000 )
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] camPos = [
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] camTarget[2] - dist * camForward[2]
rayFrom = camPos ]
oneOverWidth = float(1)/float(width) farPlane = 10000
oneOverHeight = float(1)/float(height) rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] rayForward[1] + rayForward[2] * rayForward[2]))
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom = camPos
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], oneOverWidth = float(1) / float(width)
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] oneOverHeight = float(1) / float(height)
return rayFrom,rayTo 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]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True) p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here #disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0,-0.02,0] shift = [0, -0.02, 0]
meshScale=[0.1,0.1,0.1] meshScale = [0.1, 0.1, 0.1]
vertices=[ vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,1.000000], [1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000,-1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000,1.000000,1.000000], [1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,1.000000,1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000,-1.000000,-1.000000], [1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000,1.000000,-1.000000], [1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[-1.000000,-1.000000,-1.000000], [1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000,1.000000,-1.000000], [-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000,1.000000,1.000000], [1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000], normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[1.000000,1.000000,-1.000000], [0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[1.000000,1.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[1.000000,-1.000000,1.000000], [0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[-1.000000,-1.000000,-1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[-1.000000,-1.000000,1.000000], [-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[1.000000,-1.000000,1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[1.000000,-1.000000,-1.000000], [1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[-1.000000,1.000000,-1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[-1.000000,1.000000,1.000000], [0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[1.000000,1.000000,1.000000], [0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[1.000000,1.000000,-1.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]]
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
] ]
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]
]
indices=[ 0, 1, 2, 0, 2, 3, #//ground face
6, 5, 4, 7, 6, 4, #//top face
10, 9, 8, 11, 10, 8,
12, 13, 14, 12, 14, 15,
18, 17, 16, 19, 18, 16,
20, 21, 22, 20, 22, 23]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) #visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") #visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
vertices=vertices,
collisionFramePosition=shift,
meshScale=meshScale)
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
rangex = 1 rangex = 1
rangey = 1 rangey = 1
for i in range (rangex): for i in range(rangex):
for j in range (rangey ): for j in range(rangey):
bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) bodyUid = p.createMultiBody(baseMass=1,
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) baseInertialFramePosition=[0, 0, 0],
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) 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) p.stopStateLogging(logId)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
while (1): while (1):
p.getCameraImage(320,200) p.getCameraImage(320, 200)
mouseEvents = p.getMouseEvents() mouseEvents = p.getMouseEvents()
for e in mouseEvents: for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1] mouseX = e[1]
mouseY = e[2] mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY) rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom,rayTo) rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)): for l in range(len(rayInfo)):
hit = rayInfo[l] hit = rayInfo[l]
objectUid = hit[0] objectUid = hit[0]
if (objectUid>=1): if (objectUid >= 1):
#p.removeBody(objectUid) #p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor+=1 currentColor += 1
if (currentColor>=len(colors)): if (currentColor >= len(colors)):
currentColor=0 currentColor = 0

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

@@ -2,72 +2,98 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() def getRayFromTo(mouseX, mouseY):
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
farPlane = 10000 width, height, viewMat, projMat, cameraUp, camForward, horizon, vertical, _, _, dist, camTarget = p.getDebugVisualizerCamera(
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])) camPos = [
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
rayFrom = camPos camTarget[2] - dist * camForward[2]
oneOverWidth = float(1)/float(width) ]
oneOverHeight = float(1)/float(height) farPlane = 10000
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayForward[1] + rayForward[2] * rayForward[2]))
rayTo = [rayToCenter[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayToCenter[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], rayFrom = camPos
rayToCenter[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] oneOverWidth = float(1) / float(width)
return rayFrom,rayTo 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]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True) p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here #disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0,-0.02,0] shift = [0, -0.02, 0]
meshScale=[0.1,0.1,0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="duck.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) fileName="duck.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="duck_vhacd.obj",
collisionFramePosition=shift,
meshScale=meshScale)
rangex = 5 rangex = 5
rangey = 5 rangey = 5
for i in range (rangex): for i in range(rangex):
for j in range (rangey ): for j in range(rangey):
p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True) p.createMultiBody(baseMass=1,
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,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.stopStateLogging(logId)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
while (1): while (1):
mouseEvents = p.getMouseEvents() mouseEvents = p.getMouseEvents()
for e in mouseEvents: for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1] mouseX = e[1]
mouseY = e[2] mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY) rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom,rayTo) rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)): for l in range(len(rayInfo)):
hit = rayInfo[l] hit = rayInfo[l]
objectUid = hit[0] objectUid = hit[0]
if (objectUid>=1): if (objectUid >= 1):
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor+=1 currentColor += 1
if (currentColor>=len(colors)): if (currentColor >= len(colors)):
currentColor=0 currentColor = 0

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

@@ -2,80 +2,111 @@ import pybullet as p
import time import time
import math import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera() def getRayFromTo(mouseX, mouseY):
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(
farPlane = 10000 )
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] camPos = [
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2])) camTarget[0] - dist * camForward[0], camTarget[1] - dist * camForward[1],
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]] camTarget[2] - dist * camForward[2]
rayFrom = camPos ]
oneOverWidth = float(1)/float(width) farPlane = 10000
oneOverHeight = float(1)/float(height) rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] rayForward[1] + rayForward[2] * rayForward[2]))
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]] rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0], rayFrom = camPos
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1], oneOverWidth = float(1) / float(width)
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]] oneOverHeight = float(1) / float(height)
return rayFrom,rayTo 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]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.) p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True) p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation. #disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#disable tinyrenderer, software (CPU) renderer, we don't use it here #disable tinyrenderer, software (CPU) renderer, we don't use it here
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
shift = [0,-0.02,0] shift = [0, -0.02, 0]
shift1 = [0,0.1,0] shift1 = [0, 0.1, 0]
shift2 = [0,0,0] shift2 = [0, 0, 0]
meshScale=[0.1,0.1,0.1] meshScale = [0.1, 0.1, 0.1]
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck.obj",""], visualFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck_vhacd.obj",""], collisionFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale]) halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
fileNames=["duck.obj", ""],
visualFramePositions=[
shift1,
shift2,
],
meshScales=[meshScale, meshScale])
collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX],
halfExtents=[[0, 0, 0], [0.1, 0.1, 0.1]],
fileNames=["duck_vhacd.obj", ""],
collisionFramePositions=[
shift1,
shift2,
],
meshScales=[meshScale, meshScale])
rangex = 2
rangex =2
rangey = 2 rangey = 2
for i in range (rangex): for i in range(rangex):
for j in range (rangey ): for j in range(rangey):
mb = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i*2)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*4,1], useMaximalCoordinates=False) mb = p.createMultiBody(baseMass=1,
p.changeVisualShape(mb,-1,rgbaColor=[1,1,1,1]) baseInertialFramePosition=[0, 0, 0],
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) 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) p.stopStateLogging(logId)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]]
currentColor = 0 currentColor = 0
p.getCameraImage(64,64, renderer=p.ER_BULLET_HARDWARE_OPENGL) p.getCameraImage(64, 64, renderer=p.ER_BULLET_HARDWARE_OPENGL)
while (1): while (1):
mouseEvents = p.getMouseEvents() mouseEvents = p.getMouseEvents()
for e in mouseEvents: for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)): if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1] mouseX = e[1]
mouseY = e[2] mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY) rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom,rayTo) rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)): for l in range(len(rayInfo)):
hit = rayInfo[l] hit = rayInfo[l]
objectUid = hit[0] objectUid = hit[0]
if (objectUid>=0): if (objectUid >= 0):
#p.removeBody(objectUid) #p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor]) p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor+=1 currentColor += 1
if (currentColor>=len(colors)): if (currentColor >= len(colors)):
currentColor=0 currentColor = 0

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

@@ -2,18 +2,22 @@ import pybullet as p
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf") kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6) p.addUserDebugText("tip", [0, 0, 0.1],
p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6) textColorRGB=[1, 0, 0],
p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6) textSize=1.5,
p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6) 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)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
angle=0 angle = 0
while (True): while (True):
time.sleep(0.01) time.sleep(0.01)
p.resetJointState(kuka,2,angle) p.resetJointState(kuka, 2, angle)
p.resetJointState(kuka,3,angle) p.resetJointState(kuka, 3, angle)
angle+=0.01 angle += 0.01

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

@@ -7,7 +7,8 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
print('Opened'), print('Opened'),
@@ -44,19 +45,19 @@ def readLogFile(filename, verbose = True):
if verbose: if verbose:
print("num chunks:") print("num chunks:")
print(len(chunks)) print(len(chunks))
for chunk in chunks: for chunk in chunks:
print("len(chunk)=",len(chunk)," sz = ", sz) print("len(chunk)=", len(chunk), " sz = ", sz)
if len(chunk) == sz: if len(chunk) == sz:
print("chunk #",chunkIndex) print("chunk #", chunkIndex)
chunkIndex=chunkIndex+1 chunkIndex = chunkIndex + 1
values = struct.unpack(fmt, chunk) values = struct.unpack(fmt, chunk)
record = list() record = list()
for i in range(ncols): for i in range(ncols):
record.append(values[i]) record.append(values[i])
if verbose: if verbose:
print(" ",keys[i],"=",values[i]) print(" ", keys[i], "=", values[i])
log.append(record) log.append(record)
else: else:
print("Error, expected aabb terminal") print("Error, expected aabb terminal")
@@ -65,16 +66,16 @@ def readLogFile(filename, verbose = True):
numArgs = len(sys.argv) numArgs = len(sys.argv)
print ('Number of arguments:', numArgs, 'arguments.') print('Number of arguments:', numArgs, 'arguments.')
print ('Argument List:', str(sys.argv)) print('Argument List:', str(sys.argv))
fileName = "log.bin" fileName = "log.bin"
if (numArgs>1): if (numArgs > 1):
fileName = sys.argv[1] fileName = sys.argv[1]
print("filename=") print("filename=")
print(fileName) print(fileName)
verbose = True verbose = True
readLogFile(fileName,verbose) readLogFile(fileName, verbose)

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

@@ -7,7 +7,8 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
print('Opened'), print('Opened'),
@@ -37,20 +38,20 @@ def readLogFile(filename, verbose = True):
chunks = wholeFile.split(b'\xaa\xbb') chunks = wholeFile.split(b'\xaa\xbb')
log = list() log = list()
if verbose: if verbose:
print("num chunks:") print("num chunks:")
print(len(chunks)) print(len(chunks))
chunkIndex = 0 chunkIndex = 0
for chunk in chunks: for chunk in chunks:
print("len(chunk)=",len(chunk)," sz = ", sz) print("len(chunk)=", len(chunk), " sz = ", sz)
if len(chunk) == sz: if len(chunk) == sz:
print("chunk #",chunkIndex) print("chunk #", chunkIndex)
chunkIndex=chunkIndex+1 chunkIndex = chunkIndex + 1
values = struct.unpack(fmt, chunk) values = struct.unpack(fmt, chunk)
record = list() record = list()
for i in range(ncols): for i in range(ncols):
record.append(values[i]) record.append(values[i])
if verbose: if verbose:
print(" ",keys[i],"=",values[i]) print(" ", keys[i], "=", values[i])
log.append(record) log.append(record)
@@ -59,19 +60,19 @@ def readLogFile(filename, verbose = True):
numArgs = len(sys.argv) numArgs = len(sys.argv)
print ('Number of arguments:', numArgs, 'arguments.') print('Number of arguments:', numArgs, 'arguments.')
print ('Argument List:', str(sys.argv)) print('Argument List:', str(sys.argv))
fileName = "data/example_log_vr.bin" fileName = "data/example_log_vr.bin"
if (numArgs>1): if (numArgs > 1):
fileName = sys.argv[1] fileName = sys.argv[1]
print("filename=") print("filename=")
print(fileName) print(fileName)
verbose = True verbose = True
log = readLogFile(fileName,verbose) log = readLogFile(fileName, verbose)
# the index of the first integer in the vr log file for packed buttons # the index of the first integer in the vr log file for packed buttons
firstPackedButtonIndex = 13 firstPackedButtonIndex = 13
@@ -83,26 +84,27 @@ numPackedButtons = 7
buttonMask = 7 buttonMask = 7
for record in log: for record in log:
# indices of buttons that are down # indices of buttons that are down
buttonDownIndices = [] buttonDownIndices = []
# indices of buttons that are triggered # indices of buttons that are triggered
buttonTriggeredIndices = [] buttonTriggeredIndices = []
# indices of buttons that are released # indices of buttons that are released
buttonReleasedIndices = [] buttonReleasedIndices = []
buttonIndex = 0 buttonIndex = 0
for packedButtonIndex in range(firstPackedButtonIndex, firstPackedButtonIndex+numPackedButtons): for packedButtonIndex in range(firstPackedButtonIndex,
for packButtonShift in range(numGroupedButtons): firstPackedButtonIndex + numPackedButtons):
buttonEvent = buttonMask & record[packedButtonIndex] for packButtonShift in range(numGroupedButtons):
if buttonEvent & 1: buttonEvent = buttonMask & record[packedButtonIndex]
buttonDownIndices.append(buttonIndex) if buttonEvent & 1:
elif buttonEvent & 2: buttonDownIndices.append(buttonIndex)
buttonTriggeredIndices.append(buttonIndex) elif buttonEvent & 2:
elif buttonEvent & 4: buttonTriggeredIndices.append(buttonIndex)
buttonReleasedIndices.append(buttonIndex) elif buttonEvent & 4:
record[packedButtonIndex] = record[packedButtonIndex] >> 3 buttonReleasedIndices.append(buttonIndex)
buttonIndex += 1 record[packedButtonIndex] = record[packedButtonIndex] >> 3
if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices): buttonIndex += 1
print ('timestamp: ', record[1]) if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices):
print ('button is down: ', buttonDownIndices) print('timestamp: ', record[1])
print ('button is triggered: ', buttonTriggeredIndices) print('button is down: ', buttonDownIndices)
print ('button is released: ', buttonReleasedIndices) print('button is triggered: ', buttonTriggeredIndices)
print('button is released: ', buttonReleasedIndices)

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

@@ -1,4 +1,3 @@
import pybullet as p import pybullet as p
import time import time
import pkgutil import pkgutil
@@ -7,41 +6,48 @@ egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT) p.connect(p.DIRECT)
plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin") plugin = p.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
print("plugin=",plugin) print("plugin=", plugin)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.loadURDF("plane.urdf",[0,0,-1]) p.loadURDF("plane.urdf", [0, 0, -1])
p.loadURDF("r2d2.urdf") p.loadURDF("r2d2.urdf")
pixelWidth = 320 pixelWidth = 320
pixelHeight = 220 pixelHeight = 220
camTargetPos = [0,0,0] camTargetPos = [0, 0, 0]
camDistance = 4 camDistance = 4
pitch = -10.0 pitch = -10.0
roll=0 roll = 0
upAxisIndex = 2 upAxisIndex = 2
while (p.isConnected()): while (p.isConnected()):
for yaw in range (0,360,10) : for yaw in range(0, 360, 10):
start = time.time() start = time.time()
p.stepSimulation() p.stepSimulation()
stop = time.time() stop = time.time()
print ("stepSimulation %f" % (stop - start)) print("stepSimulation %f" % (stop - start))
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0] #viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
projectionMatrix = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0] upAxisIndex)
projectionMatrix = [
start = time.time() 1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0,
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, shadow=1,lightDirection=[1,1,1]) -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0
stop = time.time() ]
print ("renderImage %f" % (stop - start))
#time.sleep(.1)
#print("img_arr=",img_arr)
start = time.time()
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) p.unloadPlugin(plugin)

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

@@ -4,48 +4,50 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0) 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,
fileName="terrain.obj",
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,
terrain_position, terrain_orientation)
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,
fileName="terrain.obj",
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,
terrain_position, terrain_orientation)
useMaximalCoordinates = True useMaximalCoordinates = True
sphereRadius = 0.005 sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
for i in range(5):
for 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)
#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, ccdSweptSphereRadius=0.002)
for i in range (5): p.setGravity(0, 0, -10)
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)
#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,ccdSweptSphereRadius=0.002)
p.setGravity(0,0,-10)
pts = p.getContactPoints() pts = p.getContactPoints()
print("num points=",len(pts)) print("num points=", len(pts))
print(pts) print(pts)
while (p.isConnected()): while (p.isConnected()):
time.sleep(1./240.) time.sleep(1. / 240.)
p.stepSimulation() p.stepSimulation()

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

@@ -5,57 +5,55 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf") p.loadSDF("stadium.sdf")
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml") objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0] sphere = objects[0]
p.resetBasePositionAndOrientation(sphere,[0,0,1],[0,0,0,1]) p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere,-1,linearDamping=0.9) p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere,-1,rgbaColor=[1,0,0,1]) p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0 forward = 0
turn = 0 turn = 0
forwardVec = [2, 0, 0]
forwardVec = [2,0,0]
cameraDistance = 1 cameraDistance = 1
cameraYaw = 35 cameraYaw = 35
cameraPitch = -35 cameraPitch = -35
while (1): while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere) spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance,cameraYaw,cameraPitch,cameraTargetPosition) p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera() camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5] camForward = camInfo[5]
keys = p.getKeyboardEvents()
keys = p.getKeyboardEvents() for k, v in keys.items():
for k,v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)): turn = -0.5
turn = -0.5 if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)): turn = 0
turn = 0 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)): turn = 0.5
turn = 0.5 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)): turn = 0
turn = 0
if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)):
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_TRIGGERED)): forward = 1
forward=1 if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)):
if (k == p.B3G_UP_ARROW and (v&p.KEY_WAS_RELEASED)): forward = 0
forward=0 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)):
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_TRIGGERED)): forward = -1
forward=-1 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)):
if (k == p.B3G_DOWN_ARROW and (v&p.KEY_WAS_RELEASED)): forward = 0
forward=0
force = [forward * camForward[0], forward * camForward[1], 0]
force = [forward*camForward[0],forward*camForward[1],0] cameraYaw = cameraYaw + turn
cameraYaw = cameraYaw+turn
if (forward):
if (forward): p.applyExternalForce(sphere, -1, force, spherePos, flags=p.WORLD_FRAME)
p.applyExternalForce(sphere,-1, force , spherePos, flags = p.WORLD_FRAME )
p.stepSimulation()
p.stepSimulation() time.sleep(1. / 240.)
time.sleep(1./240.)

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

@@ -3,18 +3,17 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
fileIO = p.loadPlugin("fileIOPlugin") fileIO = p.loadPlugin("fileIOPlugin")
if (fileIO>=0): if (fileIO >= 0):
p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO]) p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
objs= p.loadSDF("pickup/model.sdf") objs = p.loadSDF("pickup/model.sdf")
dobot =objs[0] dobot = objs[0]
p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1]) p.changeVisualShape(dobot, -1, rgbaColor=[1, 1, 1, 1])
else:
print("fileIOPlugin is disabled.")
else:
print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False) p.setPhysicsEngineParameter(enableFileCaching=False)
while (1): while (1):
p.stepSimulation() p.stepSimulation()
time.sleep(1./240.) time.sleep(1. / 240.)

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

@@ -6,21 +6,20 @@ print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
hingeJointIndex = 0 hingeJointIndex = 0
#by default, joint motors are enabled, maintaining zero velocity #by default, joint motors are enabled, maintaining zero velocity
p.setJointMotorControl2(hinge,hingeJointIndex,p.VELOCITY_CONTROL,0,0,0) p.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0, 0, 0)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.stepSimulation() p.stepSimulation()
print("joint state without sensor") print("joint state without sensor")
print(p.getJointState(0,0)) print(p.getJointState(0, 0))
p.enableJointForceTorqueSensor(hinge,hingeJointIndex) p.enableJointForceTorqueSensor(hinge, hingeJointIndex)
p.stepSimulation() p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]") print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0,0)) print(p.getJointState(0, 0))
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
p.stepSimulation() p.stepSimulation()
print("joint state with force/torque sensor, no gravity") print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0,0)) print(p.getJointState(0, 0))
p.disconnect() p.disconnect()

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

@@ -5,42 +5,42 @@ import math
p.connect(p.GUI) p.connect(p.GUI)
useMaximalCoordinates = False useMaximalCoordinates = False
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates) plane = p.loadURDF("plane.urdf", [0, 0, -1], useMaximalCoordinates=useMaximalCoordinates)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
velocity = 1 velocity = 1
num = 40 num = 40
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) #disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
p.setPhysicsEngineParameter(enableConeFriction=1) p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num): for i in range(num):
print("progress:",i,num) print("progress:", i, 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)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])
prevPos = [0,0,0] x = velocity * math.sin(2. * 3.1415 * float(i) / num)
for i in range (2048): y = velocity * math.cos(2. * 3.1415 * float(i) / num)
p.stepSimulation() print("velocity=", x, y)
pos = p.getBasePositionAndOrientation(sphere)[0] sphere = p.loadURDF("sphere_small_zeroinertia.urdf",
if (i&64): flags=p.URDF_USE_INERTIA_FROM_FILE,
p.addUserDebugLine(prevPos,pos,[1,0,0],1) useMaximalCoordinates=useMaximalCoordinates)
prevPos = pos p.changeDynamics(sphere, -1, lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere, -1, linearDamping=0)
p.changeDynamics(sphere, -1, angularDamping=0)
p.resetBaseVelocity(sphere, linearVelocity=[x, y, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) prevPos = [0, 0, 0]
for i in range(2048):
p.stepSimulation()
pos = p.getBasePositionAndOrientation(sphere)[0]
if (i & 64):
p.addUserDebugLine(prevPos, pos, [1, 0, 0], 1)
prevPos = pos
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
while (1): while (1):
time.sleep(0.01) time.sleep(0.01)

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

@@ -1,78 +1,79 @@
import pybullet as p import pybullet as p
draw=1 draw = 1
printtext = 0 printtext = 0
if (draw): if (draw):
p.connect(p.GUI) p.connect(p.GUI)
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
r2d2 = p.loadURDF("r2d2.urdf") r2d2 = p.loadURDF("r2d2.urdf")
def drawAABB(aabb): def drawAABB(aabb):
f = [aabbMin[0],aabbMin[1],aabbMin[2]] f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0],aabbMin[1],aabbMin[2]] t = [aabbMax[0], aabbMin[1], aabbMin[2]]
p.addUserDebugLine(f,t,[1,0,0]) p.addUserDebugLine(f, t, [1, 0, 0])
f = [aabbMin[0],aabbMin[1],aabbMin[2]] f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0],aabbMax[1],aabbMin[2]] t = [aabbMin[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f,t,[0,1,0]) p.addUserDebugLine(f, t, [0, 1, 0])
f = [aabbMin[0],aabbMin[1],aabbMin[2]] f = [aabbMin[0], aabbMin[1], aabbMin[2]]
t = [aabbMin[0],aabbMin[1],aabbMax[2]] t = [aabbMin[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f,t,[0,0,1]) p.addUserDebugLine(f, t, [0, 0, 1])
f = [aabbMin[0],aabbMin[1],aabbMax[2]] f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMin[0],aabbMax[1],aabbMax[2]] t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f,t,[1,1,1]) p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMin[0],aabbMin[1],aabbMax[2]] f = [aabbMin[0], aabbMin[1], aabbMax[2]]
t = [aabbMax[0],aabbMin[1],aabbMax[2]] t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f,t,[1,1,1]) p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0],aabbMin[1],aabbMin[2]] f = [aabbMax[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0],aabbMin[1],aabbMax[2]] t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f,t,[1,1,1]) p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMin[1], aabbMin[2]]
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMin[2]]
t = [aabbMin[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMin[0], aabbMax[1], aabbMin[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMin[0], aabbMax[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1.0, 0.5, 0.5])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMin[1], aabbMax[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0], aabbMax[1], aabbMax[2]]
t = [aabbMax[0], aabbMax[1], aabbMin[2]]
p.addUserDebugLine(f, t, [1, 1, 1])
f = [aabbMax[0],aabbMin[1],aabbMin[2]]
t = [aabbMax[0],aabbMax[1],aabbMin[2]]
p.addUserDebugLine(f,t,[1,1,1])
f = [aabbMax[0],aabbMax[1],aabbMin[2]]
t = [aabbMin[0],aabbMax[1],aabbMin[2]]
p.addUserDebugLine(f,t,[1,1,1])
f = [aabbMin[0],aabbMax[1],aabbMin[2]]
t = [aabbMin[0],aabbMax[1],aabbMax[2]]
p.addUserDebugLine(f,t,[1,1,1])
f = [aabbMax[0],aabbMax[1],aabbMax[2]]
t = [aabbMin[0],aabbMax[1],aabbMax[2]]
p.addUserDebugLine(f,t,[1.0,0.5,0.5])
f = [aabbMax[0],aabbMax[1],aabbMax[2]]
t = [aabbMax[0],aabbMin[1],aabbMax[2]]
p.addUserDebugLine(f,t,[1,1,1])
f = [aabbMax[0],aabbMax[1],aabbMax[2]]
t = [aabbMax[0],aabbMax[1],aabbMin[2]]
p.addUserDebugLine(f,t,[1,1,1])
aabb = p.getAABB(r2d2) aabb = p.getAABB(r2d2)
aabbMin = aabb[0] aabbMin = aabb[0]
aabbMax = aabb[1] aabbMax = aabb[1]
if (printtext): if (printtext):
print(aabbMin) print(aabbMin)
print(aabbMax) print(aabbMax)
if (draw==1): if (draw == 1):
drawAABB(aabb) drawAABB(aabb)
for i in range (p.getNumJoints(r2d2)): for i in range(p.getNumJoints(r2d2)):
aabb = p.getAABB(r2d2,i) aabb = p.getAABB(r2d2, i)
aabbMin = aabb[0] aabbMin = aabb[0]
aabbMax = aabb[1] aabbMax = aabb[1]
if (printtext): if (printtext):
print(aabbMin) print(aabbMin)
print(aabbMax) print(aabbMax)
if (draw==1): if (draw == 1):
drawAABB(aabb) drawAABB(aabb)
while (1):
while(1): a = 0
a=0 p.stepSimulation()
p.stepSimulation()

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

@@ -3,16 +3,14 @@ import numpy as np
import pybullet as p import pybullet as p
import time import time
direct = p.connect(p.GUI) #, options="--window_backend=2 --render_device=0")
direct = p.connect(p.GUI)#, options="--window_backend=2 --render_device=0")
#egl = p.loadPlugin("eglRendererPlugin") #egl = p.loadPlugin("eglRendererPlugin")
p.loadURDF('plane.urdf') p.loadURDF('plane.urdf')
p.loadURDF("r2d2.urdf",[0,0,1]) p.loadURDF("r2d2.urdf", [0, 0, 1])
p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025]) p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.0, 0.025])
cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025]) cube_trans = p.loadURDF('cube_small.urdf', basePosition=[0.0, 0.1, 0.025])
p.changeVisualShape(cube_trans,-1,rgbaColor=[1,1,1,0.1]) p.changeVisualShape(cube_trans, -1, rgbaColor=[1, 1, 1, 0.1])
width = 128 width = 128
height = 128 height = 128
@@ -25,42 +23,71 @@ view_matrix = p.computeViewMatrix([0, 0, 0.5], [0, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far) projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)
# Get depth values using the OpenGL renderer # Get depth values using the OpenGL renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True,renderer=p.ER_BULLET_HARDWARE_OPENGL) images = p.getCameraImage(width,
rgb_opengl= np.reshape(images[2], (height, width, 4))*1./255. 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_buffer_opengl = np.reshape(images[3], [width, height])
depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl) depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl)
seg_opengl = np.reshape(images[4], [width, height])*1./255. seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255.
time.sleep(1) time.sleep(1)
# Get depth values using Tiny renderer # Get depth values using Tiny renderer
images = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_TINY_RENDERER) images = p.getCameraImage(width,
height,
view_matrix,
projection_matrix,
shadow=True,
renderer=p.ER_TINY_RENDERER)
depth_buffer_tiny = np.reshape(images[3], [width, height]) depth_buffer_tiny = np.reshape(images[3], [width, height])
depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny) depth_tiny = far * near / (far - (far - near) * depth_buffer_tiny)
rgb_tiny= np.reshape(images[2], (height, width, 4))*1./255. rgb_tiny = np.reshape(images[2], (height, width, 4)) * 1. / 255.
seg_tiny = np.reshape(images[4],[width,height])*1./255. seg_tiny = np.reshape(images[4], [width, height]) * 1. / 255.
bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0] bearStartPos2 = [0, 0, 0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg") textureId = p.loadTexture("checker_grid.jpg")
for b in range (p.getNumBodies()): for b in range(p.getNumBodies()):
p.changeVisualShape(b,linkIndex=-1,textureUniqueId=textureId) p.changeVisualShape(b, linkIndex=-1, textureUniqueId=textureId)
for j in range(p.getNumJoints(b)): for j in range(p.getNumJoints(b)):
p.changeVisualShape(b,linkIndex=j,textureUniqueId=textureId) p.changeVisualShape(b, linkIndex=j, textureUniqueId=textureId)
viewMat = [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0] viewMat = [
projMat = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0] 0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
images = p.getCameraImage(width, height, viewMatrix = viewMat, projectionMatrix = projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat) 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
proj_opengl= np.reshape(images[2], (height, width, 4))*1./255. 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) 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,
proj_tiny= np.reshape(images[2], (height, width, 4))*1./255. 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 # Plot both images - should show depth values of 0.45 over the cube and 0.5 over the plane
plt.subplot(4, 2, 1) plt.subplot(4, 2, 1)
@@ -71,30 +98,29 @@ plt.subplot(4, 2, 2)
plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1) plt.imshow(depth_tiny, cmap='gray', vmin=0, vmax=1)
plt.title('Depth TinyRenderer') plt.title('Depth TinyRenderer')
plt.subplot(4,2,3) plt.subplot(4, 2, 3)
plt.imshow(rgb_opengl) plt.imshow(rgb_opengl)
plt.title('RGB OpenGL3') plt.title('RGB OpenGL3')
plt.subplot(4,2,4) plt.subplot(4, 2, 4)
plt.imshow(rgb_tiny) plt.imshow(rgb_tiny)
plt.title('RGB Tiny') plt.title('RGB Tiny')
plt.subplot(4,2,5) plt.subplot(4, 2, 5)
plt.imshow(seg_opengl) plt.imshow(seg_opengl)
plt.title('Seg OpenGL3') plt.title('Seg OpenGL3')
plt.subplot(4,2,6) plt.subplot(4, 2, 6)
plt.imshow(seg_tiny) plt.imshow(seg_tiny)
plt.title('Seg Tiny') plt.title('Seg Tiny')
plt.subplot(4,2,7) plt.subplot(4, 2, 7)
plt.imshow(proj_opengl) plt.imshow(proj_opengl)
plt.title('Proj OpenGL') plt.title('Proj OpenGL')
plt.subplot(4,2,8) plt.subplot(4, 2, 8)
plt.imshow(proj_tiny) plt.imshow(proj_tiny)
plt.title('Proj Tiny') plt.title('Proj Tiny')
plt.subplots_adjust(hspace=0.7) plt.subplots_adjust(hspace=0.7)
plt.show() plt.show()

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

@@ -2,51 +2,68 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
useCollisionShapeQuery = True useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1) geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2,0.2,0.2]) geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0,0.3,0])#[0,0.5,0.5,0] baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5,0,1] basePositionB = [1.5, 0, 1]
obA=-1 obA = -1
obB=-1 obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1]) obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB ) obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
lineWidth=3 colorRGB = [1, 0, 0]
colorRGB=[1,0,0] lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0) lineToXYZ=[0, 0, 0],
pitch=0 lineColorRGB=colorRGB,
yaw=0 lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()): while (p.isConnected()):
pitch += 0.01 pitch += 0.01
if (pitch>=3.1415*2.): if (pitch >= 3.1415 * 2.):
pitch=0 pitch = 0
yaw+= 0.01 yaw += 0.01
if (yaw>=3.1415*2.): if (yaw >= 3.1415 * 2.):
yaw=0 yaw = 0
baseOrientationB = p.getQuaternionFromEuler([yaw,pitch,0])
if (obB>=0):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery): baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) if (obB >= 0):
#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB) p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
if len(pts)>0: if (useCollisionShapeQuery):
#print(pts) pts = p.getClosestPoints(bodyA=-1,
distance = pts[0][8] bodyB=-1,
#print("distance=",distance) distance=100,
ptA = pts[0][5] collisionShapeA=geom,
ptB = pts[0][6] collisionShapeB=geomBox,
p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId); collisionShapePositionA=[0.5, 0, 1],
#time.sleep(1./240.) 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)
if len(pts) > 0:
#print(pts)
distance = pts[0][8]
#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)
#time.sleep(1./240.)
#removeCollisionShape is optional: #removeCollisionShape is optional:
#only use removeCollisionShape if the collision shape is not used to create a body #only use removeCollisionShape if the collision shape is not used to create a body

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

@@ -6,14 +6,13 @@ print(visualData)
curTexUid = visualData[0][8] curTexUid = visualData[0][8]
print(curTexUid) print(curTexUid)
texUid = p.loadTexture("tex256.png") texUid = p.loadTexture("tex256.png")
print("texUid=",texUid) print("texUid=", texUid)
p.changeVisualShape(plane,-1,textureUniqueId=texUid) p.changeVisualShape(plane, -1, textureUniqueId=texUid)
for i in range (100): for i in range(100):
p.getCameraImage(320,200) p.getCameraImage(320, 200)
p.changeVisualShape(plane,-1,textureUniqueId=curTexUid) p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
for i in range (100):
p.getCameraImage(320,200)
for i in range(100):
p.getCameraImage(320, 200)

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

@@ -1,19 +1,17 @@
import pybullet as p import pybullet as p
usePort = True usePort = True
if (usePort): if (usePort):
id = p.connect(p.GRPC,"localhost:12345") id = p.connect(p.GRPC, "localhost:12345")
else: else:
id = p.connect(p.GRPC,"localhost") id = p.connect(p.GRPC, "localhost")
print("id=",id) print("id=", id)
if (id<0): if (id < 0):
print("Cannot connect to GRPC server") print("Cannot connect to GRPC server")
exit(0) exit(0)
print ("Connected to GRPC") print("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf") r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2)) print("numJoints = ", p.getNumJoints(r2d2))

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

@@ -10,20 +10,20 @@ id = p.loadPlugin("grpcPlugin")
#id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin") #id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin")
#start the GRPC server at hostname, port #start the GRPC server at hostname, port
if (id<0): if (id < 0):
print("Cannot load grpcPlugin") print("Cannot load grpcPlugin")
exit(0) exit(0)
if usePort: if usePort:
p.executePluginCommand(id, "localhost:12345") p.executePluginCommand(id, "localhost:12345")
else: else:
p.executePluginCommand(id, "localhost") p.executePluginCommand(id, "localhost")
while p.isConnected(): while p.isConnected():
if (useDirect): if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs: #Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10 numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC]) p.executePluginCommand(id, intArgs=[numRPC])
else: else:
dt = 1./240. dt = 1. / 240.
time.sleep(dt) time.sleep(dt)

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

@@ -12,18 +12,18 @@ import pybullet as p
#first try to connect to shared memory (VR), if it fails use local GUI #first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY) c = p.connect(p.SHARED_MEMORY)
print(c) print(c)
if (c<0): if (c < 0):
p.connect(p.GUI) p.connect(p.GUI)
#load the MuJoCo MJCF hand #load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/MPL.xml") objects = p.loadMJCF("MPL/MPL.xml")
hand=objects[0] hand = objects[0]
#clamp in range 400-600 #clamp in range 400-600
#minV = 400 #minV = 400
#maxV = 600 #maxV = 600
minVarray = [275,280,350,290] minVarray = [275, 280, 350, 290]
maxVarray = [450,550,500,400] maxVarray = [450, 550, 500, 400]
pinkId = 0 pinkId = 0
middleId = 1 middleId = 1
@@ -32,82 +32,93 @@ thumbId = 3
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
def getSerialOrNone(portname): def getSerialOrNone(portname):
try: try:
return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) return serial.Serial(port=portname,
except: baudrate=115200,
return None parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
except:
return None
def convertSensor(x, fingerIndex): def convertSensor(x, fingerIndex):
minV = minVarray[fingerIndex] minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex] maxV = maxVarray[fingerIndex]
v = minV v = minV
try: try:
v = float(x) v = float(x)
except ValueError: except ValueError:
v = minV v = minV
if (v<minV): if (v < minV):
v=minV v = minV
if (v>maxV): if (v > maxV):
v=maxV v = maxV
b = (v-minV)/float(maxV-minV) b = (v - minV) / float(maxV - minV)
return (b) return (b)
ser = None ser = None
portindex = 0 portindex = 0
while (ser is None and portindex < 30): while (ser is None and portindex < 30):
portname = 'COM'+str(portindex) portname = 'COM' + str(portindex)
print(portname) print(portname)
ser = getSerialOrNone(portname) ser = getSerialOrNone(portname)
if (ser is None): if (ser is None):
portname = "/dev/cu.usbmodem14"+str(portindex) portname = "/dev/cu.usbmodem14" + str(portindex)
print(portname) print(portname)
ser = getSerialOrNone(portname) ser = getSerialOrNone(portname)
if (ser is not None): if (ser is not None):
print("COnnected!") print("COnnected!")
portindex = portindex+1 portindex = portindex + 1
if (ser is None): if (ser is None):
ser = serial.Serial(port = "/dev/cu.usbmodem1421",baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS) ser = serial.Serial(port="/dev/cu.usbmodem1421",
pi=3.141592 baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
pi = 3.141592
if (ser is not None and ser.isOpen()): if (ser is not None and ser.isOpen()):
while True: while True:
while ser.inWaiting() > 0: while ser.inWaiting() > 0:
line = str(ser.readline()) line = str(ser.readline())
words = line.split(",") words = line.split(",")
if (len(words)==6): if (len(words) == 6):
pink = convertSensor(words[1],pinkId) pink = convertSensor(words[1], pinkId)
middle = convertSensor(words[2],middleId) middle = convertSensor(words[2], middleId)
index = convertSensor(words[3],indexId) index = convertSensor(words[3], indexId)
thumb = convertSensor(words[4],thumbId) thumb = convertSensor(words[4], thumbId)
p.setJointMotorControl2(hand,7,p.POSITION_CONTROL,pi/4.)
p.setJointMotorControl2(hand,9,p.POSITION_CONTROL,thumb+pi/10)
p.setJointMotorControl2(hand,11,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,13,p.POSITION_CONTROL,thumb)
p.setJointMotorControl2(hand,17,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,19,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,21,p.POSITION_CONTROL,index)
p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle) p.setJointMotorControl2(hand, 7, p.POSITION_CONTROL, pi / 4.)
p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle) p.setJointMotorControl2(hand, 9, p.POSITION_CONTROL, thumb + pi / 10)
p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle) p.setJointMotorControl2(hand, 11, p.POSITION_CONTROL, thumb)
p.setJointMotorControl2(hand, 13, p.POSITION_CONTROL, thumb)
p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
ringpos = 0.5*(pink+middle) p.setJointMotorControl2(hand, 17, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand,32,p.POSITION_CONTROL,ringpos) p.setJointMotorControl2(hand, 19, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand,34,p.POSITION_CONTROL,ringpos) p.setJointMotorControl2(hand, 21, p.POSITION_CONTROL, index)
p.setJointMotorControl2(hand,36,p.POSITION_CONTROL,ringpos)
p.setJointMotorControl2(hand, 24, p.POSITION_CONTROL, middle)
#print(middle) p.setJointMotorControl2(hand, 26, p.POSITION_CONTROL, middle)
#print(pink) p.setJointMotorControl2(hand, 28, p.POSITION_CONTROL, middle)
#print(index)
#print(thumb) p.setJointMotorControl2(hand, 40, p.POSITION_CONTROL, pink)
p.setJointMotorControl2(hand, 42, p.POSITION_CONTROL, pink)
p.setJointMotorControl2(hand, 44, p.POSITION_CONTROL, pink)
ringpos = 0.5 * (pink + middle)
p.setJointMotorControl2(hand, 32, p.POSITION_CONTROL, ringpos)
p.setJointMotorControl2(hand, 34, p.POSITION_CONTROL, ringpos)
p.setJointMotorControl2(hand, 36, p.POSITION_CONTROL, ringpos)
#print(middle)
#print(pink)
#print(index)
#print(thumb)
else: else:
print("Cannot find port") print("Cannot find port")

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

@@ -3,21 +3,21 @@ from time import sleep
physicsClient = p.connect(p.GUI) physicsClient = p.connect(p.GUI)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf") planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1] cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
useRealTimeSimulation = 0 useRealTimeSimulation = 0
if (useRealTimeSimulation): if (useRealTimeSimulation):
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while 1: while 1:
if (useRealTimeSimulation): if (useRealTimeSimulation):
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
sleep(0.01) # Time in seconds. sleep(0.01) # Time in seconds.
else: else:
p.stepSimulation() p.stepSimulation()

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

@@ -1,182 +1,233 @@
import pybullet as p import pybullet as p
import json import json
import time import time
useGUI = True useGUI = True
if useGUI: if useGUI:
p.connect(p.GUI) p.connect(p.GUI)
else: else:
p.connect(p.DIRECT) p.connect(p.DIRECT)
useZUp = False useZUp = False
useYUp = not useZUp useYUp = not useZUp
showJointMotorTorques = False showJointMotorTorques = False
if useYUp: if useYUp:
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1) p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1)
from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerStable import PDControllerStableMultiDof from pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p) explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p) stablePD = PDControllerStableMultiDof(p)
p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16]) p.resetDebugVisualizerCamera(cameraDistance=7,
cameraYaw=-94,
cameraPitch=-14,
cameraTargetPosition=[0.31, 0.03, -1.16])
import pybullet_data import pybullet_data
p.setTimeOut(10000) p.setTimeOut(10000)
useMotionCapture=False useMotionCapture = False
useMotionCaptureReset=False#not useMotionCapture useMotionCaptureReset = False #not useMotionCapture
useExplicitPD = True useExplicitPD = True
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30) p.setPhysicsEngineParameter(numSolverIterations=30)
#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30) #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
#explicit PD control requires small timestep #explicit PD control requires small timestep
timeStep = 1./600. timeStep = 1. / 600.
#timeStep = 1./240. #timeStep = 1./240.
p.setPhysicsEngineParameter(fixedTimeStep=timeStep) p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt" path = pybullet_data.getDataPath() + "/data/motions/humanoid3d_backflip.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" #path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03]) #p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path) print("path = ", path)
with open(path, 'r') as f: with open(path, 'r') as f:
motion_dict = json.load(f) motion_dict = json.load(f)
#print("motion_dict = ", motion_dict) #print("motion_dict = ", motion_dict)
print("len motion=", len(motion_dict)) print("len motion=", len(motion_dict))
print(motion_dict['Loop']) print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames']) numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames) print("#frames = ", numFrames)
frameId = p.addUserDebugParameter("frame", 0, numFrames - 1, 0)
frameId= p.addUserDebugParameter("frame",0,numFrames-1,0) erpId = p.addUserDebugParameter("erp", 0, 1, 0.2)
kpMotorId = p.addUserDebugParameter("kpMotor", 0, 1, .2)
forceMotorId = p.addUserDebugParameter("forceMotor", 0, 2000, 1000)
erpId = p.addUserDebugParameter("erp",0,1,0.2) jointTypes = [
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
]
kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2) startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]]
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
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_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])
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC", startPose = [
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"] 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,
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]] -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
]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0]) startVel = [
p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0]) 1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0,
p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0]) 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952,
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0]) -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
]
humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) p.resetBasePositionAndOrientation(humanoid4, 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]) index0 = 7
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) for j in range(p.getNumJoints(humanoid)):
humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) ji = p.getJointInfo(humanoid, j)
humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
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)
print("spherical velocity: ", targetVel)
p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
targetPosition = [startPose[index0]]
targetVel = [startVel[index0]]
index0 += 1
print("revolute:", targetPosition)
print("revolute velocity:", targetVel)
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])
startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, p.setJointMotorControl2(humanoid3, j, p.VELOCITY_CONTROL, targetVelocity=0, force=31)
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] p.setJointMotorControl2(humanoid4, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10)
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, #print(ji)
-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, print("joint[", j, "].type=", jointTypes[ji[2]])
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] print("joint[", j, "].name=", ji[1])
p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1]) jointIds = []
p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1]) paramIds = []
p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1]) for j in range(p.getNumJoints(humanoid)):
p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid, j, rgbaColor=[1, 1, 1, 1])
info = p.getJointInfo(humanoid, j)
#print(info)
if (not useMotionCapture):
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
#print("jointName=",jointName, "at ", j)
p.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1])
chest = 1
index0=7 neck = 2
for j in range (p.getNumJoints(humanoid)): rightHip = 3
ji = p.getJointInfo(humanoid,j) rightKnee = 4
targetPosition=[0] rightAnkle = 5
jointType = ji[2] rightShoulder = 6
if (jointType == p.JOINT_SPHERICAL): rightElbow = 7
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] leftHip = 9
targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] leftKnee = 10
index0+=4 leftAnkle = 11
print("spherical position: ",targetPosition) leftShoulder = 12
print("spherical velocity: ",targetVel) leftElbow = 13
p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
targetPosition=[startPose[index0]]
targetVel = [startVel[index0]]
index0+=1
print("revolute:", targetPosition)
print("revolute velocity:", targetVel)
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])
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
#print(ji)
print("joint[",j,"].type=",jointTypes[ji[2]])
print("joint[",j,"].name=",ji[1])
jointIds=[]
paramIds=[]
for j in range (p.getNumJoints(humanoid)):
#p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
info = p.getJointInfo(humanoid,j)
#print(info)
if (not useMotionCapture):
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
#paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
#print("jointName=",jointName, "at ", j)
p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
chest=1
neck=2
rightHip=3
rightKnee=4
rightAnkle=5
rightShoulder=6
rightElbow=7
leftHip=9
leftKnee=10
leftAnkle=11
leftShoulder=12
leftElbow=13
#rightShoulder=3 #rightShoulder=3
#rightElbow=4 #rightElbow=4
@@ -191,249 +242,320 @@ leftElbow=13
import time 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] once = True
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] p.getCameraImage(320, 200)
once=True while (p.isConnected()):
p.getCameraImage(320,200)
if useGUI:
erp = p.readUserDebugParameter(erpId)
kpMotor = p.readUserDebugParameter(kpMotorId)
maxForce = p.readUserDebugParameter(forceMotorId)
frameReal = p.readUserDebugParameter(frameId)
else:
erp = 0.2
kpMotor = 0.2
maxForce = 1000
frameReal = 0
kp = kpMotor
frame = int(frameReal)
frameNext = frame + 1
if (frameNext >= numFrames):
frameNext = frame
while (p.isConnected()): frameFraction = frameReal - frame
#print("frameFraction=",frameFraction)
#print("frame=",frame)
#print("frameNext=", frameNext)
if useGUI: #getQuaternionSlerp
erp = p.readUserDebugParameter(erpId)
kpMotor = p.readUserDebugParameter(kpMotorId)
maxForce=p.readUserDebugParameter(forceMotorId)
frameReal = p.readUserDebugParameter(frameId)
else:
erp = 0.2
kpMotor = 0.2
maxForce=1000
frameReal = 0
kp=kpMotor frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
frame = int(frameReal)
frameNext = frame+1
if (frameNext >= numFrames):
frameNext = frame
frameFraction = frameReal - frame
#print("frameFraction=",frameFraction)
#print("frame=",frame)
#print("frameNext=", frameNext)
#getQuaternionSlerp
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
#print("duration=",frameData[0])
#print(pos=[frameData])
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
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)
#pre-rotate to make z-up
if (useZUp):
y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zPos=[0,2,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
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)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
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)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
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]]
leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
if (0):#if (once):
p.resetJointStateMultiDof(humanoid,chest,chestRot)
p.resetJointStateMultiDof(humanoid,neck,neckRot)
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot)
p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot)
p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot)
p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
once=False
#print("chestRot=",chestRot)
p.setGravity(0,0,-10)
kp=kpMotor
if (useExplicitPD):
jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
#[x,y,z] base position and [x,y,z,w] base orientation!
totalDofs =7
for dof in jointDofCounts:
totalDofs += dof
jointIndicesAll = [
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],
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],
leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
leftElbowRot[0] ]
#print("pose=")
#for po in pose:
# print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices = jointIndicesAll,
desiredPositions = pose,
desiredVelocities = [0]*totalDofs,
kps = kpOrg,
kds = kdOrg,
maxForces = [maxForce]*totalDofs,
timeStep=timeStep)
taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
jointIndices = jointIndicesAll,
desiredPositions = pose,
desiredVelocities = [0]*totalDofs,
kps = kpOrg,
kds = kdOrg,
maxForces = [maxForce*0.05]*totalDofs,
timeStep=timeStep)
#taus=[0]*43 #print("duration=",frameData[0])
dofIndex=7 #print(pos=[frameData])
for index in range (len(jointIndicesAll)):
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]])
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]])
dofIndex+=jointDofCounts[index] basePos1Start = [frameData[1], frameData[2], frameData[3]]
basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]]
#print("len(taus)=",len(taus)) basePos1 = [
#print("taus=",taus) basePos1Start[0] + frameFraction * (basePos1End[0] - basePos1Start[0]),
basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]),
basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2])
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]) baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]]
p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce]) baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]]
p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce]) baseOrn1 = p.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction)
p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce]) #pre-rotate to make z-up
p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce]) if (useZUp):
p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce]) y2zPos = [0, 0, 0.0]
p.setJointMotorControlMultiDof(humanoid2,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce]) y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce]) basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
p.setJointMotorControlMultiDof(humanoid2,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce]) p.resetBasePositionAndOrientation(humanoid, basePos, baseOrn)
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]) y2zPos = [0, 2, 0.0]
y2zOrn = p.getQuaternionFromEuler([1.57, 0, 0])
kinematicHumanoid4 = True basePos, baseOrn = p.multiplyTransforms(y2zPos, y2zOrn, basePos1, baseOrn1)
if (kinematicHumanoid4): p.resetBasePositionAndOrientation(humanoid2, basePos, baseOrn)
p.resetJointStateMultiDof(humanoid4,chest,chestRot)
p.resetJointStateMultiDof(humanoid4,neck,neckRot) chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]]
p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot) chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]]
p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot) chestRot = p.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction)
p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot) neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]]
p.resetJointStateMultiDof(humanoid4,rightElbow, rightElbowRot) neckRotEnd = [frameDataNext[13], frameDataNext[14], frameDataNext[15], frameDataNext[12]]
p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot) neckRot = p.getQuaternionSlerp(neckRotStart, neckRotEnd, frameFraction)
p.resetJointStateMultiDof(humanoid4,leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot) rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]]
p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot) rightHipRotEnd = [frameDataNext[17], frameDataNext[18], frameDataNext[19], frameDataNext[16]]
p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot) rightHipRot = p.getQuaternionSlerp(rightHipRotStart, rightHipRotEnd, frameFraction)
p.stepSimulation()
rightKneeRotStart = [frameData[20]]
if showJointMotorTorques: rightKneeRotEnd = [frameDataNext[20]]
for j in range(p.getNumJoints(humanoid2)): rightKneeRot = [
jointState = p.getJointStateMultiDof(humanoid2,j) rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0])
print("jointStateMultiDof[",j,"].pos=",jointState[0]) ]
print("jointStateMultiDof[",j,"].vel=",jointState[1])
print("jointStateMultiDof[",j,"].jointForces=",jointState[3]) rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]]
time.sleep(timeStep) 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)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
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]]
leftHipRot = p.getQuaternionSlerp(leftHipRotStart, leftHipRotEnd, frameFraction)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
leftKneeRot = [leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0])]
leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]]
leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]]
leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd, frameFraction)
leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]]
leftShoulderRotEnd = [frameDataNext[40], frameDataNext[41], frameDataNext[42], frameDataNext[39]]
leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart, leftShoulderRotEnd, frameFraction)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
leftElbowRot = [
leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0])
]
if (0): #if (once):
p.resetJointStateMultiDof(humanoid, chest, chestRot)
p.resetJointStateMultiDof(humanoid, neck, neckRot)
p.resetJointStateMultiDof(humanoid, rightHip, rightHipRot)
p.resetJointStateMultiDof(humanoid, rightKnee, rightKneeRot)
p.resetJointStateMultiDof(humanoid, rightAnkle, rightAnkleRot)
p.resetJointStateMultiDof(humanoid, rightShoulder, rightShoulderRot)
p.resetJointStateMultiDof(humanoid, rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid, leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid, leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid, leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid, leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid, leftElbow, leftElbowRot)
once = False
#print("chestRot=",chestRot)
p.setGravity(0, 0, -10)
kp = kpMotor
if (useExplicitPD):
jointDofCounts = [4, 4, 4, 1, 4, 4, 1, 4, 1, 4, 4, 1]
#[x,y,z] base position and [x,y,z,w] base orientation!
totalDofs = 7
for dof in jointDofCounts:
totalDofs += dof
jointIndicesAll = [
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],
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],
leftShoulderRot[0], leftShoulderRot[1], leftShoulderRot[2], leftShoulderRot[3],
leftElbowRot[0]
]
#print("pose=")
#for po in pose:
# print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid,
jointIndices=jointIndicesAll,
desiredPositions=pose,
desiredVelocities=[0] * totalDofs,
kps=kpOrg,
kds=kdOrg,
maxForces=[maxForce] * totalDofs,
timeStep=timeStep)
taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
jointIndices=jointIndicesAll,
desiredPositions=pose,
desiredVelocities=[0] * totalDofs,
kps=kpOrg,
kds=kdOrg,
maxForces=[maxForce * 0.05] * totalDofs,
timeStep=timeStep)
#taus=[0]*43
dofIndex = 7
for index in range(len(jointIndicesAll)):
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]])
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]])
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])
kinematicHumanoid4 = True
if (kinematicHumanoid4):
p.resetJointStateMultiDof(humanoid4, chest, chestRot)
p.resetJointStateMultiDof(humanoid4, neck, neckRot)
p.resetJointStateMultiDof(humanoid4, rightHip, rightHipRot)
p.resetJointStateMultiDof(humanoid4, rightKnee, rightKneeRot)
p.resetJointStateMultiDof(humanoid4, rightAnkle, rightAnkleRot)
p.resetJointStateMultiDof(humanoid4, rightShoulder, rightShoulderRot)
p.resetJointStateMultiDof(humanoid4, rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid4, leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid4, leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid4, leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid4, leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid4, leftElbow, leftElbowRot)
p.stepSimulation()
if showJointMotorTorques:
for j in range(p.getNumJoints(humanoid2)):
jointState = p.getJointStateMultiDof(humanoid2, j)
print("jointStateMultiDof[", j, "].pos=", jointState[0])
print("jointStateMultiDof[", j, "].vel=", jointState[1])
print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
time.sleep(timeStep)

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

@@ -1,20 +1,25 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.DIRECT) p.connect(p.DIRECT)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=5) p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
p.setPhysicsEngineParameter(numSubSteps=1) p.setPhysicsEngineParameter(numSubSteps=1)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml") objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0] ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923]) p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ] [0.710965, 0.218117, 0.519402, -0.420923])
for jointIndex in range (p.getNumJoints(ob)): jointPositions = [
p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) -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 #first let the humanoid fall
#p.setRealTimeSimulation(1) #p.setRealTimeSimulation(1)
@@ -26,11 +31,10 @@ p.setRealTimeSimulation(0)
print("Starting benchmark") print("Starting benchmark")
fileName = "pybullet_humanoid_timings.json" fileName = "pybullet_humanoid_timings.json"
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,fileName) logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, fileName)
for i in range(1000): for i in range(1000):
p.stepSimulation() p.stepSimulation()
p.stopStateLogging(logId) p.stopStateLogging(logId)
print("ended benchmark") print("ended benchmark")
print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName) print("Use Chrome browser, visit about://tracing, and load the %s file" % fileName)

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

@@ -2,8 +2,8 @@ import pybullet as p
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
cid = p.connect(p.GUI) cid = p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
@@ -11,36 +11,41 @@ useRealTime = 0
p.setRealTimeSimulation(useRealTime) p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.loadSDF("stadium.sdf") p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml") obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0] human = obUids[0]
for i in range(p.getNumJoints(human)):
p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle", -4, 4, -1)
maxForceId = p.addUserDebugParameter("maxForce", 0, 500, 10)
for i in range (p.getNumJoints(human)): kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500) maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1) kneeJointIndex = 11
maxForceId = p.addUserDebugParameter("maxForce",0,500,10) kneeJointIndexLeft = 18
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL",-4,4,-1)
maxForceLeftId = p.addUserDebugParameter("maxForceL",0,500,10)
kneeJointIndex=11
kneeJointIndexLeft=18
while (1): while (1):
time.sleep(0.01) time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId) kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId) maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce) p.setJointMotorControl2(human,
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId) kneeJointIndex,
maxForceLeft = p.readUserDebugParameter(maxForceLeftId) p.POSITION_CONTROL,
p.setJointMotorControl2(human,kneeJointIndexLeft,p.POSITION_CONTROL,targetPosition=kneeAngleTargetLeft,force=maxForceLeft) targetPosition=kneeAngleTarget,
force=maxForce)
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
p.setJointMotorControl2(human,
kneeJointIndexLeft,
p.POSITION_CONTROL,
targetPosition=kneeAngleTargetLeft,
force=maxForceLeft)
if (useRealTime==0): if (useRealTime == 0):
p.stepSimulation() p.stepSimulation()

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

@@ -5,28 +5,28 @@ p.connect(p.GUI)
obUids = p.loadMJCF("mjcf/humanoid.xml") obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1] humanoid = obUids[1]
gravId = p.addUserDebugParameter("gravity",-10,10,-10) gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds=[] jointIds = []
paramIds=[] paramIds = []
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0) p.changeDynamics(humanoid, -1, linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(humanoid)): for j in range(p.getNumJoints(humanoid)):
p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0) p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid,j) info = p.getJointInfo(humanoid, j)
#print(info) #print(info)
jointName = info[1] jointName = info[1]
jointType = info[2] jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE): if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j) jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while(1): while (1):
p.setGravity(0,0,p.readUserDebugParameter(gravId)) p.setGravity(0, 0, p.readUserDebugParameter(gravId))
for i in range(len(paramIds)): for i in range(len(paramIds)):
c = paramIds[i] c = paramIds[i]
targetPos = p.readUserDebugParameter(c) targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.) p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
time.sleep(0.01) time.sleep(0.01)

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

@@ -5,26 +5,37 @@ from datetime import datetime
from time import sleep from time import sleep
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6 kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId) numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move. #Joint damping coefficents. Using large values for the joints that we don't want to move.
jd=[100.0,100.0,100.0,100.0,100.0,100.0,0.5] jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5] #jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
while 1: while 1:
p.stepSimulation() p.stepSimulation()
for i in range (1): for i in range(1):
pos = [0,0,1.26] pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0,0,3.14]) orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
for i in range (numJoints): pos,
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) orn,
sleep(0.05) 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)
sleep(0.05)

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

@@ -1,13 +1,12 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
cube = p.loadURDF("cube.urdf") cube = p.loadURDF("cube.urdf")
frequency = 240 frequency = 240
timeStep = 1./frequency timeStep = 1. / frequency
p.setGravity(0,0,-9.8) p.setGravity(0, 0, -9.8)
p.changeDynamics(cube,-1,linearDamping=0,angularDamping=0) p.changeDynamics(cube, -1, linearDamping=0, angularDamping=0)
p.setPhysicsEngineParameter(fixedTimeStep = timeStep) p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
for i in range (frequency): for i in range(frequency):
p.stepSimulation() p.stepSimulation()
pos,orn = p.getBasePositionAndOrientation(cube) pos, orn = p.getBasePositionAndOrientation(cube)
print(pos) print(pos)

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

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

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

@@ -3,7 +3,7 @@ plot = True
import time import time
if (plot): if (plot):
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import math import math
verbose = False verbose = False
@@ -19,137 +19,146 @@ bullet.setTimeStep(delta_t)
# Switch between URDF with/without FIXED joints # Switch between URDF with/without FIXED joints
with_fixed_joints = True with_fixed_joints = True
if with_fixed_joints: if with_fixed_joints:
id_revolute_joints = [0, 3] id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf", id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True) robot_base,
robot_orientation,
useFixedBase=True)
else: else:
id_revolute_joints = [0, 1] id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf", id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True) robot_base,
robot_orientation,
useFixedBase=True)
bullet.changeDynamics(id_robot, -1, linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot, 0, linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot, 1, linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,-1,linearDamping=0, angularDamping=0) jointTypeNames = [
bullet.changeDynamics(id_robot,0,linearDamping=0, angularDamping=0) "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED",
bullet.changeDynamics(id_robot,1,linearDamping=0, angularDamping=0) "JOINT_POINT2POINT", "JOINT_GEAR"
]
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"]
# Disable the motors for torque control: # Disable the motors for torque control:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0]) bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.VELOCITY_CONTROL,
forces=[0.0, 0.0])
# Target Positions: # Target Positions:
start = 0.0 start = 0.0
end = 1.0 end = 1.0
steps = int((end-start)/delta_t) steps = int((end - start) / delta_t)
t = [0]*steps t = [0] * steps
q_pos_desired = [[0.]* steps,[0.]* steps] q_pos_desired = [[0.] * steps, [0.] * steps]
q_vel_desired = [[0.]* steps,[0.]* steps] q_vel_desired = [[0.] * steps, [0.] * steps]
q_acc_desired = [[0.]* steps,[0.]* steps] q_acc_desired = [[0.] * steps, [0.] * steps]
for s in range(steps): for s in range(steps):
t[s] = start+s*delta_t t[s] = start + s * delta_t
q_pos_desired[0][s] = 1./(2.*math.pi) * math.sin(2. * math.pi * t[s]) - t[s] q_pos_desired[0][s] = 1. / (2. * math.pi) * math.sin(2. * math.pi * t[s]) - t[s]
q_pos_desired[1][s] = -1./(2.*math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0) q_pos_desired[1][s] = -1. / (2. * math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1.
q_vel_desired[1][s] = math.sin(2. * math.pi * t[s])
q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s])
q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
q_pos = [[0.]* steps,[0.]* steps] q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1.
q_vel = [[0.]* steps,[0.]* steps] q_vel_desired[1][s] = math.sin(2. * math.pi * t[s])
q_tor = [[0.]* steps,[0.]* 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]
# Do Torque Control: # Do Torque Control:
for i in range(len(t)): for i in range(len(t)):
# Read Sensor States: # Read Sensor States:
joint_states = bullet.getJointStates(id_robot, id_revolute_joints) joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
q_pos[0][i] = joint_states[0][0]
a = joint_states[1][0]
if (verbose):
print("joint_states[1][0]")
print(joint_states[1][0])
q_pos[1][i] = a
q_vel[0][i] = joint_states[0][1]
q_vel[1][i] = joint_states[1][1]
# Computing the torque from inverse dynamics: q_pos[0][i] = joint_states[0][0]
obj_pos = [q_pos[0][i], q_pos[1][i]] a = joint_states[1][0]
obj_vel = [q_vel[0][i], q_vel[1][i]] if (verbose):
obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]] print("joint_states[1][0]")
print(joint_states[1][0])
q_pos[1][i] = a
if (verbose): q_vel[0][i] = joint_states[0][1]
print("calculateInverseDynamics") q_vel[1][i] = joint_states[1][1]
print("id_robot")
print(id_robot)
print("obj_pos")
print(obj_pos)
print("obj_vel")
print(obj_vel)
print("obj_acc")
print(obj_acc)
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
q_tor[0][i] = torque[0]
q_tor[1][i] = torque[1]
if (verbose):
print("torque=")
print(torque)
# Set the Joint Torques: # Computing the torque from inverse dynamics:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]]) obj_pos = [q_pos[0][i], q_pos[1][i]]
obj_vel = [q_vel[0][i], q_vel[1][i]]
obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
# Step Simulation if (verbose):
bullet.stepSimulation() print("calculateInverseDynamics")
print("id_robot")
print(id_robot)
print("obj_pos")
print(obj_pos)
print("obj_vel")
print(obj_vel)
print("obj_acc")
print(obj_acc)
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
q_tor[0][i] = torque[0]
q_tor[1][i] = torque[1]
if (verbose):
print("torque=")
print(torque)
# Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.TORQUE_CONTROL,
forces=[torque[0], torque[1]])
# Step Simulation
bullet.stepSimulation()
# Plot the Position, Velocity and Acceleration: # Plot the Position, Velocity and Acceleration:
if plot: if plot:
figure = plt.figure(figsize=[15, 4.5]) figure = plt.figure(figsize=[15, 4.5])
figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55) figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55)
ax_pos = figure.add_subplot(141)
ax_pos.set_title("Joint Position")
ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
ax_pos.set_ylim(-1., 1.)
ax_pos.legend()
ax_vel = figure.add_subplot(142)
ax_vel.set_title("Joint Velocity")
ax_vel.plot(t, q_vel_desired[0], '--r', lw=4, label='Desired q0')
ax_vel.plot(t, q_vel_desired[1], '--b', lw=4, label='Desired q1')
ax_vel.plot(t, q_vel[0], '-r', lw=1, label='Measured q0')
ax_vel.plot(t, q_vel[1], '-b', lw=1, label='Measured q1')
ax_vel.set_ylim(-2., 2.)
ax_vel.legend()
ax_acc = figure.add_subplot(143)
ax_acc.set_title("Joint Acceleration")
ax_acc.plot(t, q_acc_desired[0], '--r', lw=4, label='Desired q0')
ax_acc.plot(t, q_acc_desired[1], '--b', lw=4, label='Desired q1')
ax_acc.set_ylim(-10., 10.)
ax_acc.legend()
ax_tor = figure.add_subplot(144)
ax_tor.set_title("Executed Torque")
ax_tor.plot(t, q_tor[0], '-r', lw=2, label='Torque q0')
ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1')
ax_tor.set_ylim(-20., 20.)
ax_tor.legend()
plt.pause(0.01) ax_pos = figure.add_subplot(141)
ax_pos.set_title("Joint Position")
ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
ax_pos.set_ylim(-1., 1.)
ax_pos.legend()
ax_vel = figure.add_subplot(142)
ax_vel.set_title("Joint Velocity")
ax_vel.plot(t, q_vel_desired[0], '--r', lw=4, label='Desired q0')
ax_vel.plot(t, q_vel_desired[1], '--b', lw=4, label='Desired q1')
ax_vel.plot(t, q_vel[0], '-r', lw=1, label='Measured q0')
ax_vel.plot(t, q_vel[1], '-b', lw=1, label='Measured q1')
ax_vel.set_ylim(-2., 2.)
ax_vel.legend()
ax_acc = figure.add_subplot(143)
ax_acc.set_title("Joint Acceleration")
ax_acc.plot(t, q_acc_desired[0], '--r', lw=4, label='Desired q0')
ax_acc.plot(t, q_acc_desired[1], '--b', lw=4, label='Desired q1')
ax_acc.set_ylim(-10., 10.)
ax_acc.legend()
ax_tor = figure.add_subplot(144)
ax_tor.set_title("Executed Torque")
ax_tor.plot(t, q_tor[0], '-r', lw=2, label='Torque q0')
ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1')
ax_tor.set_ylim(-20., 20.)
ax_tor.legend()
plt.pause(0.01)
while (1): while (1):
bullet.stepSimulation() bullet.stepSimulation()
time.sleep(0.01) time.sleep(0.01)

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

@@ -4,35 +4,34 @@ import math
from datetime import datetime from datetime import datetime
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
if (clid<0): if (clid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6 kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId) numJoints = p.getNumJoints(kukaId)
if (numJoints!=7): if (numJoints != 7):
exit() exit()
#lower limits for null space #lower limits for null space
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space #upper limits for null space
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space #joint ranges for null space
jr=[5.8,4,5.8,4,5.8,4,6] jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space #restposes for null space
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents #joint damping coefficents
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
for i in range (numJoints): for i in range(numJoints):
p.resetJointState(kukaId,i,rp[i]) p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
t=0. t = 0.
prevPose=[0,0,0] prevPose = [0, 0, 0]
prevPose1=[0,0,0] prevPose1 = [0, 0, 0]
hasPrevPose = 0 hasPrevPose = 0
useNullSpace = 1 useNullSpace = 1
@@ -46,46 +45,73 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically #trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal #use 0 for no-removal
trailDuration = 15 trailDuration = 15
while 1:
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
else:
t=t+0.001
if (useSimulation and useRealTimeSimulation==0):
p.stepSimulation()
for i in range (1):
pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0,-math.pi,0])
if (useNullSpace==1):
if (useOrientation==1):
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)
else:
if (useOrientation==1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01)
else:
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex) while 1:
if (hasPrevPose): p.getCameraImage(320,
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) 200,
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX,
prevPose=pos renderer=p.ER_BULLET_HARDWARE_OPENGL)
prevPose1=ls[4] if (useRealTimeSimulation):
hasPrevPose = 1 dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.001
if (useSimulation and useRealTimeSimulation == 0):
p.stepSimulation()
for i in range(1):
pos = [-0.4, 0.2 * math.cos(t), 0. + 0.2 * math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])
if (useNullSpace == 1):
if (useOrientation == 1):
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)
else:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100,
residualThreshold=.01)
else:
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1

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

@@ -5,57 +5,57 @@ from datetime import datetime
from datetime import datetime from datetime import datetime
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
if (clid<0): if (clid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659]) husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
for i in range (p.getNumJoints(husky)): [0.002328, -0.000984, 0.996491, 0.083659])
print(p.getJointInfo(husky,i)) for i in range(p.getNumJoints(husky)):
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659) 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)
ob = kukaId ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ] jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
for jointIndex in range (p.getNumJoints(ob)): for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#put kuka on top of husky #put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1]) cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5],
[0, 0, 0, 1])
baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3]) baseorn = [0, 0, 0, 1]
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707] #[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1]) #p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6 kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId) numJoints = p.getNumJoints(kukaId)
if (numJoints!=7): if (numJoints != 7):
exit() exit()
#lower limits for null space #lower limits for null space
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space #upper limits for null space
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space #joint ranges for null space
jr=[5.8,4,5.8,4,5.8,4,6] jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space #restposes for null space
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents #joint damping coefficents
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
for i in range (numJoints): for i in range(numJoints):
p.resetJointState(kukaId,i,rp[i]) p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
t=0. t = 0.
prevPose=[0,0,0] prevPose = [0, 0, 0]
prevPose1=[0,0,0] prevPose1 = [0, 0, 0]
hasPrevPose = 0 hasPrevPose = 0
useNullSpace = 0 useNullSpace = 0
useOrientation =0 useOrientation = 0
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
#This can be used to test the IK result accuracy. #This can be used to test the IK result accuracy.
useSimulation = 0 useSimulation = 0
@@ -64,106 +64,130 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically #trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal #use 0 for no-removal
trailDuration = 15 trailDuration = 15
basepos =[0,0,0] basepos = [0, 0, 0]
ang = 0
ang = 0 ang = 0
ang=0
def accurateCalculateInverseKinematics( kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
dist2 = 1e30
while (not closeEnough and iter<maxIter):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,targetPos)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0]-newPos[0],targetPos[1]-newPos[1],targetPos[2]-newPos[2]]
dist2 = (diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2])
closeEnough = (dist2 < threshold)
iter=iter+1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
wheels=[2,3,4,5] def accurateCalculateInverseKinematics(kukaId, endEffectorId, targetPos, threshold, maxIter):
closeEnough = False
iter = 0
dist2 = 1e30
while (not closeEnough and iter < maxIter):
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, targetPos)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
newPos = ls[4]
diff = [targetPos[0] - newPos[0], targetPos[1] - newPos[1], targetPos[2] - newPos[2]]
dist2 = (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2])
closeEnough = (dist2 < threshold)
iter = iter + 1
#print ("Num iter: "+str(iter) + "threshold: "+str(dist2))
return jointPoses
wheels = [2, 3, 4, 5]
#(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link') #(2, b'front_left_wheel', 0, 7, 6, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_left_wheel_link')
#(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link') #(3, b'front_right_wheel', 0, 8, 7, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'front_right_wheel_link')
#(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link') #(4, b'rear_left_wheel', 0, 9, 8, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_left_wheel_link')
#(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link') #(5, b'rear_right_wheel', 0, 10, 9, 1, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, b'rear_right_wheel_link')
wheelVelocities=[0,0,0,0] wheelVelocities = [0, 0, 0, 0]
wheelDeltasTurn=[1,-1,1,-1] wheelDeltasTurn = [1, -1, 1, -1]
wheelDeltasFwd=[1,1,1,1] wheelDeltasFwd = [1, 1, 1, 1]
while 1: while 1:
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
shift = 0.01 shift = 0.01
wheelVelocities=[0,0,0,0] wheelVelocities = [0, 0, 0, 0]
speed = 1.0 speed = 1.0
for k in keys: for k in keys:
if ord('s') in keys: if ord('s') in keys:
p.saveWorld("state.py") p.saveWorld("state.py")
if ord('a') in keys: if ord('a') in keys:
basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]] basepos = basepos = [basepos[0], basepos[1] - shift, basepos[2]]
if ord('d') in keys: if ord('d') in keys:
basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]] basepos = basepos = [basepos[0], basepos[1] + shift, basepos[2]]
if p.B3G_LEFT_ARROW in keys: if p.B3G_LEFT_ARROW in keys:
for i in range(len(wheels)): for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i] wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasTurn[i]
if p.B3G_RIGHT_ARROW in keys: if p.B3G_RIGHT_ARROW in keys:
for i in range(len(wheels)): for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i] wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasTurn[i]
if p.B3G_UP_ARROW in keys: if p.B3G_UP_ARROW in keys:
for i in range(len(wheels)): for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i] wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasFwd[i]
if p.B3G_DOWN_ARROW in keys: if p.B3G_DOWN_ARROW in keys:
for i in range(len(wheels)): for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] -speed*wheelDeltasFwd[i] wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasFwd[i]
baseorn = p.getQuaternionFromEuler([0,0,ang]) baseorn = p.getQuaternionFromEuler([0, 0, ang])
for i in range(len(wheels)): for i in range(len(wheels)):
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000) p.setJointMotorControl2(husky,
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1]) wheels[i],
if (useRealTimeSimulation): p.VELOCITY_CONTROL,
t = time.time()#(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.') targetVelocity=wheelVelocities[i],
#t = (dt.second/60.)*2.*math.pi force=1000)
else: #p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
t=t+0.001 if (useRealTimeSimulation):
t = time.time() #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
if (useSimulation and useRealTimeSimulation==0): #t = (dt.second/60.)*2.*math.pi
p.stepSimulation() else:
t = t + 0.001
for i in range (1):
#pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
pos = [0.2*math.cos(t),0,0.+0.2*math.sin(t)+0.7]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0,-math.pi,0])
if (useNullSpace==1):
if (useOrientation==1):
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)
else:
if (useOrientation==1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
else:
threshold =0.001
maxIter = 100
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex) if (useSimulation and useRealTimeSimulation == 0):
if (hasPrevPose): p.stepSimulation()
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration)
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) for i in range(1):
prevPose=pos #pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
prevPose1=ls[4] pos = [0.2 * math.cos(t), 0, 0. + 0.2 * math.sin(t) + 0.7]
hasPrevPose = 1 #end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])
if (useNullSpace == 1):
if (useOrientation == 1):
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)
else:
if (useOrientation == 1):
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
else:
threshold = 0.001
maxIter = 100
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1

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

@@ -4,24 +4,23 @@ import math
from datetime import datetime from datetime import datetime
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
if (clid<0): if (clid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-1.3]) p.loadURDF("plane.urdf", [0, 0, -1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sawyerId = p.loadURDF("pole.urdf",[0,0,0]) sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1]) p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3 sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId) numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents #joint damping coefficents
jd=[0.1,0.1,0.1,0.1] jd = [0.1, 0.1, 0.1, 0.1]
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
t=0. t = 0.
prevPose=[0,0,0] prevPose = [0, 0, 0]
prevPose1=[0,0,0] prevPose1 = [0, 0, 0]
hasPrevPose = 0 hasPrevPose = 0
ikSolver = 0 ikSolver = 0
@@ -30,30 +29,35 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically #trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal #use 0 for no-removal
trailDuration = 1 trailDuration = 1
while 1: while 1:
if (useRealTimeSimulation): if (useRealTimeSimulation):
dt = datetime.now() dt = datetime.now()
t = (dt.second/60.)*2.*math.pi t = (dt.second / 60.) * 2. * math.pi
else: else:
t=t+0.01 t = t + 0.01
time.sleep(0.01) time.sleep(0.01)
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)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range(1):
for i in range (numJoints): pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointInfo = p.getJointInfo(sawyerId, i) jointPoses = p.calculateInverseKinematics(sawyerId,
qIndex = jointInfo[3] sawyerEndEffectorIndex,
if qIndex > -1: pos,
p.resetJointState(sawyerId,i,jointPoses[qIndex-7]) jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex) #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
if (hasPrevPose): for i in range(numJoints):
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) jointInfo = p.getJointInfo(sawyerId, i)
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) qIndex = jointInfo[3]
prevPose=pos if qIndex > -1:
prevPose1=ls[4] p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
hasPrevPose = 1
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1

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

@@ -2,47 +2,54 @@ import pybullet as p
def getJointStates(robot): def getJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_positions = [state[0] for state in joint_states] joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states] joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states] joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot): def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot))) joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))] joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1] joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
joint_positions = [state[0] for state in joint_states] joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states] joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states] joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3): def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot) num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints zero_vec = [0.0] * num_joints
if len(position) == num_joints: if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL, p.setJointMotorControlArray(robot,
targetPositions=position, targetVelocities=zero_vec, range(num_joints),
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints) p.POSITION_CONTROL,
else: targetPositions=position,
print("Not setting torque. " targetVelocities=zero_vec,
"Expected torque vector of " positionGains=[kp] * num_joints,
"length {}, got {}".format(num_joints, len(torque))) 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): def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0] result = [0.0, 0.0, 0.0]
i = 0 i = 0
for c in range(len(vector)): for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1: if p.getJointInfo(robot, c)[3] > -1:
for r in range(3): for r in range(3):
result[r] += jacobian[r][i] * vector[c] result[r] += jacobian[r][i] * vector[c]
i += 1 i += 1
return result return result
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
if (clid<0): if (clid < 0):
p.connect(p.DIRECT) p.connect(p.DIRECT)
time_step = 0.001 time_step = 0.001
gravity_constant = -9.81 gravity_constant = -9.81
@@ -50,14 +57,14 @@ p.resetSimulation()
p.setTimeStep(time_step) p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant) p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True) kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0]) #kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) #kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0]) #kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0]) #kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(kukaId) numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1 kukaEndEffectorIndex = numJoints - 1
@@ -69,7 +76,10 @@ p.stepSimulation()
pos, vel, torq = getJointStates(kukaId) pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId) mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1) result = p.getLinkState(kukaId,
kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link. # Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn. # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
@@ -78,11 +88,11 @@ link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = r
zero_vec = [0.0] * len(mpos) zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec) jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
print ("Link linear velocity of CoM from getLinkState:") print("Link linear velocity of CoM from getLinkState:")
print (link_vt) print(link_vt)
print ("Link linear velocity of CoM from linearJacobian * q_dot:") print("Link linear velocity of CoM from linearJacobian * q_dot:")
print (multiplyJacobian(kukaId, jac_t, vel)) print(multiplyJacobian(kukaId, jac_t, vel))
print ("Link angular velocity of CoM from getLinkState:") print("Link angular velocity of CoM from getLinkState:")
print (link_vr) print(link_vr)
print ("Link angular velocity of CoM from angularJacobian * q_dot:") print("Link angular velocity of CoM from angularJacobian * q_dot:")
print (multiplyJacobian(kukaId, jac_r, vel)) print(multiplyJacobian(kukaId, jac_r, vel))

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

@@ -2,25 +2,23 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
door=p.loadURDF("door.urdf") door = p.loadURDF("door.urdf")
#linear/angular damping for base and all children=0 #linear/angular damping for base and all children=0
p.changeDynamics(door,-1,linearDamping=0, angularDamping=0) p.changeDynamics(door, -1, linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(door)): for j in range(p.getNumJoints(door)):
p.changeDynamics(door,j,linearDamping=0, angularDamping=0) p.changeDynamics(door, j, linearDamping=0, angularDamping=0)
print(p.getJointInfo(door,j)) print(p.getJointInfo(door, j))
frictionId = p.addUserDebugParameter("jointFriction", 0, 20, 10)
frictionId = p.addUserDebugParameter("jointFriction",0,20,10) torqueId = p.addUserDebugParameter("joint torque", 0, 20, 5)
torqueId = p.addUserDebugParameter("joint torque",0,20,5)
while (1): while (1):
frictionForce = p.readUserDebugParameter(frictionId) frictionForce = p.readUserDebugParameter(frictionId)
jointTorque = p.readUserDebugParameter(torqueId) jointTorque = p.readUserDebugParameter(torqueId)
#set the joint friction #set the joint friction
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce) p.setJointMotorControl2(door, 1, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce)
#apply a joint torque #apply a joint torque
p.setJointMotorControl2(door,1,p.TORQUE_CONTROL,force=jointTorque) p.setJointMotorControl2(door, 1, p.TORQUE_CONTROL, force=jointTorque)
p.stepSimulation() p.stepSimulation()
time.sleep(0.01) time.sleep(0.01)

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

@@ -2,27 +2,34 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.25]) p.loadURDF("plane.urdf", [0, 0, -0.25])
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf",useFixedBase=True) minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True)
print(p.getNumJoints(minitaur)) print(p.getNumJoints(minitaur))
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6,cameraTargetPosition=[-0.064,.621,-0.2]) p.resetDebugVisualizerCamera(cameraDistance=1,
cameraYaw=23.2,
cameraPitch=-6.6,
cameraTargetPosition=[-0.064, .621, -0.2])
motorJointId = 1 motorJointId = 1
p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=100000,force=0) p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0)
p.resetJointState(minitaur,motorJointId,targetValue=0, targetVelocity=1) p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1)
angularDampingSlider = p.addUserDebugParameter("angularDamping", 0,1,0) angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0)
jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0,0.1,0) jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0)
textId = p.addUserDebugText("jointVelocity=0",[0,0,-0.2]) textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2])
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while (1): while (1):
frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) frictionForce = p.readUserDebugParameter(jointFrictionForceSlider)
angularDamping = p.readUserDebugParameter(angularDampingSlider) angularDamping = p.readUserDebugParameter(angularDampingSlider)
p.setJointMotorControl2(minitaur,motorJointId,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce) p.setJointMotorControl2(minitaur,
p.changeDynamics(minitaur,motorJointId,linearDamping=0, angularDamping=angularDamping) motorJointId,
p.VELOCITY_CONTROL,
targetVelocity=0,
force=frictionForce)
p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping)
time.sleep(0.01) time.sleep(0.01)
txt = "jointVelocity="+str(p.getJointState(minitaur,motorJointId)[1]) txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1])
prevTextId = textId prevTextId = textId
textId = p.addUserDebugText(txt,[0,0,-0.2]) textId = p.addUserDebugText(txt, [0, 0, -0.2])
p.removeUserDebugItem(prevTextId) p.removeUserDebugItem(prevTextId)

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

@@ -2,15 +2,15 @@ import pybullet as p
import struct import struct
def readLogFile(filename, verbose = True): def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
print('Opened'), print('Opened'),
print(filename) print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',') keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n') fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record # The byte number of one record
sz = struct.calcsize(fmt) sz = struct.calcsize(fmt)
# The type number of one record # The type number of one record
@@ -41,11 +41,12 @@ def readLogFile(filename, verbose = True):
return log return log
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf") p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf",[0,0,0]) p.loadURDF("tray/tray.urdf", [0, 0, 0])
p.loadURDF("block.urdf",[0,0,2]) p.loadURDF("block.urdf", [0, 0, 2])
log = readLogFile("data/block_grasp_log.bin") log = readLogFile("data/block_grasp_log.bin")
@@ -58,26 +59,26 @@ print(recordNum)
print('item num:'), print('item num:'),
print(itemNum) print(itemNum)
def Step(stepIndex): def Step(stepIndex):
for objectId in range(objectNum): for objectId in range(objectNum):
record = log[stepIndex*objectNum+objectId] record = log[stepIndex * objectNum + objectId]
Id = record[2] Id = record[2]
pos = [record[3],record[4],record[5]] pos = [record[3], record[4], record[5]]
orn = [record[6],record[7],record[8],record[9]] orn = [record[6], record[7], record[8], record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn) p.resetBasePositionAndOrientation(Id, pos, orn)
numJoints = p.getNumJoints(Id) numJoints = p.getNumJoints(Id)
for i in range (numJoints): for i in range(numJoints):
jointInfo = p.getJointInfo(Id,i) jointInfo = p.getJointInfo(Id, i)
qIndex = jointInfo[3] qIndex = jointInfo[3]
if qIndex > -1: if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17]) p.resetJointState(Id, i, record[qIndex - 7 + 17])
stepIndexId = p.addUserDebugParameter("stepIndex",0,recordNum/objectNum-1,0) stepIndexId = p.addUserDebugParameter("stepIndex", 0, recordNum / objectNum - 1, 0)
while True: while True:
stepIndex = int(p.readUserDebugParameter(stepIndexId)) stepIndex = int(p.readUserDebugParameter(stepIndexId))
Step(stepIndex) Step(stepIndex)
p.stepSimulation() p.stepSimulation()
Step(stepIndex) Step(stepIndex)

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

@@ -5,36 +5,36 @@ from datetime import datetime
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True) p.loadURDF("plane.urdf", [0, 0, -0.3], useFixedBase=True)
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True) kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6 kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId) numJoints = p.getNumJoints(kukaId)
if (numJoints!=7): if (numJoints != 7):
exit() exit()
p.loadURDF("cube.urdf",[2,2,5]) p.loadURDF("cube.urdf", [2, 2, 5])
p.loadURDF("cube.urdf",[-2,-2,5]) p.loadURDF("cube.urdf", [-2, -2, 5])
p.loadURDF("cube.urdf",[2,-2,5]) p.loadURDF("cube.urdf", [2, -2, 5])
#lower limits for null space #lower limits for null space
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space #upper limits for null space
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space #joint ranges for null space
jr=[5.8,4,5.8,4,5.8,4,6] jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space #restposes for null space
rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents #joint damping coefficents
jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
for i in range (numJoints): for i in range(numJoints):
p.resetJointState(kukaId,i,rp[i]) p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
t=0. t = 0.
prevPose=[0,0,0] prevPose = [0, 0, 0]
prevPose1=[0,0,0] prevPose1 = [0, 0, 0]
hasPrevPose = 0 hasPrevPose = 0
useNullSpace = 0 useNullSpace = 0
@@ -47,50 +47,68 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#use 0 for no-removal #use 0 for no-removal
trailDuration = 15 trailDuration = 15
logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2]) logId1 = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "LOG0001.txt", [0, 1, 2])
logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS,"LOG0002.txt",bodyUniqueIdA=2) logId2 = p.startStateLogging(p.STATE_LOGGING_CONTACT_POINTS, "LOG0002.txt", bodyUniqueIdA=2)
for i in range(5): for i in range(5):
print ("Body %d's name is %s." % (i, p.getBodyInfo(i)[1])) print("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+0.1
if (useSimulation and useRealTimeSimulation==0):
p.stepSimulation()
for i in range (1):
pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0,-math.pi,0])
if (useNullSpace==1):
if (useOrientation==1):
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)
else:
if (useOrientation==1):
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
p.resetJointState(kukaId,i,jointPoses[i])
ls = p.getLinkState(kukaId,kukaEndEffectorIndex) while 1:
if (hasPrevPose): if (useRealTimeSimulation):
p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) dt = datetime.now()
p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) t = (dt.second / 60.) * 2. * math.pi
prevPose=pos else:
prevPose1=ls[4] t = t + 0.1
hasPrevPose = 1
if (useSimulation and useRealTimeSimulation == 0):
p.stepSimulation()
for i in range(1):
pos = [-0.4, 0.2 * math.cos(t), 0. + 0.2 * math.sin(t)]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])
if (useNullSpace == 1):
if (useOrientation == 1):
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)
else:
if (useOrientation == 1):
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)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
p.resetJointState(kukaId, i, jointPoses[i])
ls = p.getLinkState(kukaId, kukaEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1

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

@@ -10,15 +10,16 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
print('Opened'), print('Opened'),
print(filename) print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',') keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n') fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record # The byte number of one record
sz = struct.calcsize(fmt) sz = struct.calcsize(fmt)
# The type number of one record # The type number of one record
@@ -49,13 +50,14 @@ def readLogFile(filename, verbose = True):
return log return log
#clid = p.connect(p.SHARED_MEMORY) #clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3]) p.loadURDF("plane.urdf", [0, 0, -0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,1]) p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 1])
p.loadURDF("cube.urdf",[2,2,5]) p.loadURDF("cube.urdf", [2, 2, 5])
p.loadURDF("cube.urdf",[-2,-2,5]) p.loadURDF("cube.urdf", [-2, -2, 5])
p.loadURDF("cube.urdf",[2,-2,5]) p.loadURDF("cube.urdf", [2, -2, 5])
log = readLogFile("LOG0001.txt") log = readLogFile("LOG0001.txt")
@@ -67,14 +69,14 @@ print('item num:'),
print(itemNum) print(itemNum)
for record in log: for record in log:
Id = record[2] Id = record[2]
pos = [record[3],record[4],record[5]] pos = [record[3], record[4], record[5]]
orn = [record[6],record[7],record[8],record[9]] orn = [record[6], record[7], record[8], record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn) p.resetBasePositionAndOrientation(Id, pos, orn)
numJoints = p.getNumJoints(Id) numJoints = p.getNumJoints(Id)
for i in range (numJoints): for i in range(numJoints):
jointInfo = p.getJointInfo(Id,i) jointInfo = p.getJointInfo(Id, i)
qIndex = jointInfo[3] qIndex = jointInfo[3]
if qIndex > -1: if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17]) p.resetJointState(Id, i, record[qIndex - 7 + 17])
sleep(0.0005) sleep(0.0005)

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

@@ -3,20 +3,20 @@ from time import sleep
physicsClient = p.connect(p.GUI) physicsClient = p.connect(p.GUI)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf") planeId = p.loadURDF("plane.urdf")
bunnyId = p.loadSoftBody("bunny.obj") bunnyId = p.loadSoftBody("bunny.obj")
p.loadURDF("cube_small.urdf",[1,0,1]) p.loadURDF("cube_small.urdf", [1, 0, 1])
useRealTimeSimulation = 1 useRealTimeSimulation = 1
if (useRealTimeSimulation): if (useRealTimeSimulation):
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while p.isConnected(): while p.isConnected():
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
if (useRealTimeSimulation): if (useRealTimeSimulation):
sleep(0.01) # Time in seconds. sleep(0.01) # Time in seconds.
#p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL ) #p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL )
else: else:
p.stepSimulation() p.stepSimulation()

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

@@ -4,25 +4,23 @@ p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json") timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
print("load plane.urdf") print("load plane.urdf")
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
print("load r2d2.urdf") print("load r2d2.urdf")
p.loadURDF("r2d2.urdf",0,0,1) p.loadURDF("r2d2.urdf", 0, 0, 1)
print("load kitchen/1.sdf") print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf") p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf") print("load 100 times plate.urdf")
for i in range (100): for i in range(100):
p.loadURDF("dinnerware/plate.urdf",0,i,1) p.loadURDF("dinnerware/plate.urdf", 0, i, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(timinglog) p.stopStateLogging(timinglog)
print("stopped state logging") print("stopped state logging")
p.getCameraImage(320,200) p.getCameraImage(320, 200)
while (1): while (1):
p.stepSimulation() p.stepSimulation()

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

@@ -1,11 +1,11 @@
import pybullet as p import pybullet as p
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid < 0) : if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf") quadruped = p.loadURDF("quadruped/quadruped.urdf")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped]) logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "LOG00048.TXT", [quadruped])
p.stepSimulation() p.stepSimulation()
p.stepSimulation() p.stepSimulation()
p.stepSimulation() p.stepSimulation()

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

@@ -2,34 +2,34 @@ import pybullet as p
import time import time
conid = p.connect(p.SHARED_MEMORY) conid = p.connect(p.SHARED_MEMORY)
if (conid<0): if (conid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.setInternalSimFlags(0) p.setInternalSimFlags(0)
p.resetSimulation() p.resetSimulation()
p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)
gravXid = p.addUserDebugParameter("gravityX",-10,10,0) p.loadURDF("plane.urdf", useMaximalCoordinates=True)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0) p.loadURDF("tray/traybox.urdf", useMaximalCoordinates=True)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
gravXid = p.addUserDebugParameter("gravityX", -10, 10, 0)
gravYid = p.addUserDebugParameter("gravityY", -10, 10, 0)
gravZid = p.addUserDebugParameter("gravityZ", -10, 10, -10)
p.setPhysicsEngineParameter(numSolverIterations=10) p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
for i in range (10): for i in range(10):
for j in range (10): for j in range(10):
for k in range (10): for k in range(10):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True) ob = p.loadURDF("sphere_1cm.urdf", [0.02 * i, 0.02 * j, 0.2 + 0.02 * k],
useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while True: while True:
gravX = p.readUserDebugParameter(gravXid) gravX = p.readUserDebugParameter(gravXid)
gravY = p.readUserDebugParameter(gravYid) gravY = p.readUserDebugParameter(gravYid)
gravZ = p.readUserDebugParameter(gravZid) gravZ = p.readUserDebugParameter(gravZid)
p.setGravity(gravX,gravY,gravZ) p.setGravity(gravX, gravY, gravZ)
time.sleep(0.01) time.sleep(0.01)

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

@@ -4,26 +4,44 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("plane.urdf",0,0,-2) p.loadURDF("plane.urdf", 0, 0, -2)
wheelA = p.loadURDF("differential/diff_ring.urdf",[0,0,0]) wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
for i in range(p.getNumJoints(wheelA)): for i in range(p.getNumJoints(wheelA)):
print(p.getJointInfo(wheelA,i)) print(p.getJointInfo(wheelA, i))
p.setJointMotorControl2(wheelA,i,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
c = p.createConstraint(wheelA,
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,1,wheelA,3,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(wheelA,
p.changeConstraint(c,gearRatio=1, maxForce=10000) 2,
wheelA,
c = p.createConstraint(wheelA,2,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) 4,
p.changeConstraint(c,gearRatio=-1, maxForce=10000) jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
c = p.createConstraint(wheelA,1,wheelA,4,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) parentFramePosition=[0, 0, 0],
p.changeConstraint(c,gearRatio=-1, maxForce=10000) 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])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while(1): while (1):
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
time.sleep(0.01) time.sleep(0.01)
#p.removeConstraint(c) #p.removeConstraint(c)

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

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

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

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

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

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

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

@@ -2,11 +2,19 @@ import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
cartpole=p.loadURDF("cartpole.urdf") cartpole = p.loadURDF("cartpole.urdf")
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5) p.setJointMotorControl2(cartpole,
1,
p.POSITION_CONTROL,
targetPosition=1000,
targetVelocity=0,
force=1000,
positionGain=1,
velocityGain=0,
maxVelocity=0.5)
while (1): while (1):
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
js = p.getJointState(cartpole,1) js = p.getJointState(cartpole, 1)
print("position=",js[0],"velocity=",js[1]) print("position=", js[0], "velocity=", js[1])
time.sleep(0.01) time.sleep(0.01)

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

@@ -6,43 +6,47 @@ import math
usePhysX = True usePhysX = True
useMaximalCoordinates = True useMaximalCoordinates = True
if usePhysX: if usePhysX:
p.connect(p.PhysX,options="--numCores=8 --solver=pgs") p.connect(p.PhysX, options="--numCores=8 --solver=pgs")
p.loadPlugin("eglRendererPlugin") p.loadPlugin("eglRendererPlugin")
else: else:
p.connect(p.GUI) p.connect(p.GUI)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024) p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.,
numSolverIterations=4,
minimumSolverIslandSize=1024)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.01) p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
p.setAdditionalSearchPath(pd.getDataPath()) p.setAdditionalSearchPath(pd.getDataPath())
#Always make ground plane maximal coordinates, to avoid performance drop in PhysX #Always make ground plane maximal coordinates, to avoid performance drop in PhysX
#See https://github.com/NVIDIAGameWorks/PhysX/issues/71 #See https://github.com/NVIDIAGameWorks/PhysX/issues/71
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates) p.loadURDF("plane.urdf", useMaximalCoordinates=True) #useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "physx_create_dominoes.json")
jran = 50 jran = 50
iran = 100 iran = 100
num=64 num = 64
radius=0.1 radius = 0.1
numDominoes=0 numDominoes = 0
for i in range (int(num*50)): for i in range(int(num * 50)):
num=(radius*2*math.pi)/0.08 num = (radius * 2 * math.pi) / 0.08
radius += 0.05/float(num) radius += 0.05 / float(num)
orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)]) orn = p.getQuaternionFromEuler([0, 0, 0.5 * math.pi + math.pi * 2 * i / float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03] pos = [
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates) radius * math.cos(2 * math.pi * (i / float(num))),
numDominoes+=1 radius * math.sin(2 * math.pi * (i / float(num))), 0.03
]
pos=[pos[0],pos[1],pos[2]+0.3] sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
orn = p.getQuaternionFromEuler([0,0,-math.pi/4.]) numDominoes += 1
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
print("numDominoes=",numDominoes) pos = [pos[0], pos[1], pos[2] + 0.3]
orn = p.getQuaternionFromEuler([0, 0, -math.pi / 4.])
sphere = p.loadURDF("domino/domino.urdf", pos, orn, useMaximalCoordinates=useMaximalCoordinates)
print("numDominoes=", numDominoes)
#for j in range (20): #for j in range (20):
# for i in range (100): # for i in range (100):
@@ -51,48 +55,51 @@ print("numDominoes=",numDominoes)
# else: # else:
# orn = p.getQuaternionFromEuler([0,-3.14*0.24,0]) # orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
# sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates) # sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
print("loaded!")
print("loaded!")
#p.changeDynamics(sphere ,-1, mass=1000) #p.changeDynamics(sphere ,-1, mass=1000)
door = p.loadURDF("door.urdf",[0,0,-11]) door = p.loadURDF("door.urdf", [0, 0, -11])
p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) p.changeDynamics(door, 1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
print("numJoints = ", p.getNumJoints(door)) print("numJoints = ", p.getNumJoints(door))
p.setGravity(0, 0, -10)
p.setGravity(0,0,-10)
position_control = True position_control = True
angle = math.pi*0.25 angle = math.pi * 0.25
p.resetJointState(door,1,angle) p.resetJointState(door, 1, angle)
angleread = p.getJointState(door,1) angleread = p.getJointState(door, 1)
print("angleread = ",angleread) print("angleread = ", angleread)
prevTime = time.time() prevTime = time.time()
angle = math.pi*0.5 angle = math.pi * 0.5
count=0 count = 0
while (1): while (1):
count+=1 count += 1
if (count==12): if (count == 12):
p.stopStateLogging(logId) p.stopStateLogging(logId)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
curTime = time.time() curTime = time.time()
diff = curTime - prevTime diff = curTime - prevTime
#every second, swap target angle #every second, swap target angle
if (diff>1): if (diff > 1):
angle = - angle angle = -angle
prevTime = curTime prevTime = curTime
if position_control: if position_control:
p.setJointMotorControl2(door,1,p.POSITION_CONTROL, targetPosition = angle, positionGain=10.1, velocityGain=1, force=11.001) p.setJointMotorControl2(door,
else: 1,
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL, targetVelocity=1, force=1011) 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() #contacts = p.getContactPoints()
#print("contacts=",contacts) #print("contacts=",contacts)
p.stepSimulation() p.stepSimulation()

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

@@ -1,4 +1,3 @@
import pybullet as p import pybullet as p
from pdControllerExplicit import PDControllerExplicitMultiDof from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerExplicit import PDControllerExplicit from pdControllerExplicit import PDControllerExplicit
@@ -6,107 +5,145 @@ from pdControllerStable import PDControllerStable
import time import time
useMaximalCoordinates = False
useMaximalCoordinates=False
p.connect(p.GUI) p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0,0,0], useMaximalCoordinates=useMaximalCoordinates) pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0,1,0], useMaximalCoordinates=useMaximalCoordinates) pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0,2,0], useMaximalCoordinates=useMaximalCoordinates) pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0,3,0], useMaximalCoordinates=useMaximalCoordinates) pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p) exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(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)
p.setJointMotorControl2(pole2, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole3, i, p.POSITION_CONTROL, targetPosition=0, force=0)
p.setJointMotorControl2(pole4, i, p.POSITION_CONTROL, targetPosition=0, force=0)
for i in range (p.getNumJoints(pole2)): #print("joint",i,"=",p.getJointInfo(pole2,i))
#disable default constraint-based motors
p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
p.setJointMotorControl2(pole2,i,p.POSITION_CONTROL,targetPosition=0,force=0)
p.setJointMotorControl2(pole3,i,p.POSITION_CONTROL,targetPosition=0,force=0)
p.setJointMotorControl2(pole4,i,p.POSITION_CONTROL,targetPosition=0,force=0)
#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)
kpCartId = p.addUserDebugParameter("kpCart", 0, 500, 1300)
kdCartId = p.addUserDebugParameter("kdCart", 0, 300, 150)
maxForceCartId = p.addUserDebugParameter("maxForceCart", 0, 5000, 1000)
timeStepId = p.addUserDebugParameter("timeStep",0.001,0.1,0.01) textColor = [1, 1, 1]
desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
kpCartId = p.addUserDebugParameter("kpCart",0,500,1300)
kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
textColor = [1,1,1]
shift = 0.05 shift = 0.05
p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,parentLinkIndex=1) p.addUserDebugText("explicit PD", [shift, 0, .1],
p.addUserDebugText("explicit PD plugin", [shift,0,-.1],textColor,parentObjectUniqueId=pole2,parentLinkIndex=1) textColor,
p.addUserDebugText("stablePD", [shift,0,.1],textColor,parentObjectUniqueId=pole4,parentLinkIndex=1) parentObjectUniqueId=pole,
p.addUserDebugText("position constraint", [shift,0,-.1],textColor,parentObjectUniqueId=pole3,parentLinkIndex=1) parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0) desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0) desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole",0,500,1200) kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
kdPoleId = p.addUserDebugParameter("kdPole",0,300,100) kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000) maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin") pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0, 0, -10)
p.setGravity(0,0,-10)
useRealTimeSim = False useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim) p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001 timeStep = 0.001
while p.isConnected(): while p.isConnected():
#p.getCameraImage(320,200) #p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId) timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep) p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId) desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId) desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId) kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId) kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId) maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId) desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId) desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId) kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId) kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId) maxForcePole = p.readUserDebugParameter(maxForcePoleId)
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)
for j in [0,1]:
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)
link = 1
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)
#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]])
basePos, baseOrn = p.getBasePositionAndOrientation(pole)
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) baseDof = 7
taus = exPD.computePD(pole, [0, 1], [
if (not useRealTimeSim): basePos[0], basePos[1], basePos[2], baseOrn[0], baseOrn[1], baseOrn[2], baseOrn[3],
p.stepSimulation() desiredPosCart, desiredPosPole
time.sleep(timeStep) ], [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.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)
link = 1
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)
#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)
if (not useRealTimeSim):
p.stepSimulation()
time.sleep(timeStep)

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

@@ -2,88 +2,97 @@ import numpy as np
class PDControllerExplicitMultiDof(object): class PDControllerExplicitMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def __init__(self, pb):
self._pb = pb
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId) def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]] maxForces, timeStep):
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0] numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
qError = [0,0,0, 0,0,0,0] curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
qIndex=7 q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
qdotIndex=7 baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
zeroAccelerations=[0,0,0, 0,0,0,0] qdot1 = [
for i in range (numJoints): baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i]) ]
qError = [0, 0, 0, 0, 0, 0, 0]
jointPos=js[0] qIndex = 7
jointVel = js[1] qdotIndex = 7
q1+=jointPos zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
for i in range(numJoints):
if len(js[0])==1: js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
desiredPos=desiredPositions[qIndex]
jointPos = js[0]
qdiff=desiredPos - jointPos[0] jointVel = js[1]
qError.append(qdiff) q1 += jointPos
zeroAccelerations.append(0.)
qdot1+=jointVel if len(js[0]) == 1:
qIndex+=1 desiredPos = desiredPositions[qIndex]
qdotIndex+=1
if len(js[0])==4: qdiff = desiredPos - jointPos[0]
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] qError.append(qdiff)
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos) zeroAccelerations.append(0.)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0] qdot1 += jointVel
qdot1+=jointVelNew qIndex += 1
qError.append(axis[0]) qdotIndex += 1
qError.append(axis[1]) if len(js[0]) == 4:
qError.append(axis[2]) desiredPos = [
qError.append(0) desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]] desiredPositions[qIndex + 3]
zeroAccelerations+=[0.,0.,0.,0.] ]
qIndex+=4 axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
qdotIndex+=4 jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew
q = np.array(q1) qError.append(axis[0])
qdot=np.array(qdot1) qError.append(axis[1])
qdotdesired = np.array(desiredVelocities) qError.append(axis[2])
qdoterr = qdotdesired-qdot qError.append(0)
Kp = np.diagflat(kps) desiredVel = [
Kd = np.diagflat(kds) desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
p = Kp.dot(qError) desiredVelocities[qdotIndex + 2]
d = Kd.dot(qdoterr) ]
forces = p + d zeroAccelerations += [0., 0., 0., 0.]
maxF = np.array(maxForces) qIndex += 4
forces = np.clip(forces, -maxF , maxF ) qdotIndex += 4
return forces
q = np.array(q1)
qdot = np.array(qdot1)
qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdoterr)
forces = p + d
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF)
return forces
class PDControllerExplicit(object): class PDControllerExplicit(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def __init__(self, pb):
numJoints = self._pb.getNumJoints(bodyUniqueId) self._pb = pb
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = [] def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
qdot1 = [] maxForces, timeStep):
for i in range (numJoints): numJoints = self._pb.getNumJoints(bodyUniqueId)
q1.append(jointStates[i][0]) jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
qdot1.append(jointStates[i][1]) q1 = []
q = np.array(q1) qdot1 = []
qdot=np.array(qdot1) for i in range(numJoints):
qdes = np.array(desiredPositions) q1.append(jointStates[i][0])
qdotdes = np.array(desiredVelocities) qdot1.append(jointStates[i][1])
qError = qdes - q q = np.array(q1)
qdotError = qdotdes - qdot qdot = np.array(qdot1)
Kp = np.diagflat(kps) qdes = np.array(desiredPositions)
Kd = np.diagflat(kds) qdotdes = np.array(desiredVelocities)
forces = Kp.dot(qError) + Kd.dot(qdotError) qError = qdes - q
maxF = np.array(maxForces) qdotError = qdotdes - qdot
forces = np.clip(forces, -maxF , maxF ) Kp = np.diagflat(kps)
return forces Kd = np.diagflat(kds)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF, maxF)
return forces

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

@@ -1,144 +1,148 @@
import numpy as np import numpy as np
class PDControllerStableMultiDof(object): class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def __init__(self, pb):
self._pb = pb
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId) def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
#q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]] maxForces, timeStep):
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
#qdot1 = [0,0,0, 0,0,0,0] curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId) #q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
qError = [0,0,0, 0,0,0,0] #qdot1 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qIndex=7
qdotIndex=7 qdot1 = [
zeroAccelerations=[0,0,0, 0,0,0,0] baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
for i in range (numJoints): ]
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i]) qError = [0, 0, 0, 0, 0, 0, 0]
jointPos=js[0] qIndex = 7
jointVel = js[1] qdotIndex = 7
q1+=jointPos zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
for i in range(numJoints):
if len(js[0])==1: js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
desiredPos=desiredPositions[qIndex]
jointPos = js[0]
qdiff=desiredPos - jointPos[0] jointVel = js[1]
qError.append(qdiff) q1 += jointPos
zeroAccelerations.append(0.)
qdot1+=jointVel if len(js[0]) == 1:
qIndex+=1 desiredPos = desiredPositions[qIndex]
qdotIndex+=1
if len(js[0])==4: qdiff = desiredPos - jointPos[0]
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] qError.append(qdiff)
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos) zeroAccelerations.append(0.)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0] qdot1 += jointVel
qdot1+=jointVelNew qIndex += 1
qError.append(axis[0]) qdotIndex += 1
qError.append(axis[1]) if len(js[0]) == 4:
qError.append(axis[2]) desiredPos = [
qError.append(0) desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]] desiredPositions[qIndex + 3]
zeroAccelerations+=[0.,0.,0.,0.] ]
qIndex+=4 axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
qdotIndex+=4 jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew
q = np.array(q1) qError.append(axis[0])
qdot=np.array(qdot1) qError.append(axis[1])
qError.append(axis[2])
qdotdesired = np.array(desiredVelocities) qError.append(0)
qdoterr = qdotdesired-qdot desiredVel = [
desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
desiredVelocities[qdotIndex + 2]
Kp = np.diagflat(kps) ]
Kd = np.diagflat(kds) zeroAccelerations += [0., 0., 0., 0.]
qIndex += 4
p = Kp.dot(qError) qdotIndex += 4
#np.savetxt("pb_qError.csv", qError, delimiter=",") q = np.array(q1)
#np.savetxt("pb_p.csv", p, delimiter=",") qdot = np.array(qdot1)
d = Kd.dot(qdoterr) qdotdesired = np.array(desiredVelocities)
qdoterr = qdotdesired - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
#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=",")
M = (M2 + Kd * timeStep)
#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
#p = [0]*43
b = p + d - c
#np.savetxt("pb_acc.csv",b, delimiter=",")
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF, maxF)
return forces
#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=",")
M = (M2 + Kd * timeStep)
#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
#p = [0]*43
b = p + d -c
#np.savetxt("pb_acc.csv",b, delimiter=",")
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
#print("len(tau)=",len(tau))
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
return forces
class PDControllerStable(object): class PDControllerStable(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): def __init__(self, pb):
numJoints = self._pb.getNumJoints(bodyUniqueId) self._pb = pb
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = [] def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
qdot1 = [] maxForces, timeStep):
zeroAccelerations = [] numJoints = self._pb.getNumJoints(bodyUniqueId)
for i in range (numJoints): jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1.append(jointStates[i][0]) q1 = []
qdot1.append(jointStates[i][1]) qdot1 = []
zeroAccelerations.append(0) zeroAccelerations = []
q = np.array(q1) for i in range(numJoints):
qdot=np.array(qdot1) q1.append(jointStates[i][0])
qdes = np.array(desiredPositions) qdot1.append(jointStates[i][1])
qdotdes = np.array(desiredVelocities) zeroAccelerations.append(0)
qError = qdes - q q = np.array(q1)
qdotError = qdotdes - qdot qdot = np.array(qdot1)
Kp = np.diagflat(kps) qdes = np.array(desiredPositions)
Kd = np.diagflat(kds) qdotdes = np.array(desiredVelocities)
p = Kp.dot(qError) qError = qdes - q
d = Kd.dot(qdotError) qdotError = qdotdes - qdot
forces = p + d Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1) p = Kp.dot(qError)
M2 = np.array(M1) d = Kd.dot(qdotError)
M = (M2 + Kd * timeStep) forces = p + d
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1) M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1)
A = M M2 = np.array(M1)
b = -c + p + d M = (M2 + Kd * timeStep)
qddot = np.linalg.solve(A, b) c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
tau = p + d - Kd.dot(qddot) * timeStep c = np.array(c1)
maxF = np.array(maxForces) A = M
forces = np.clip(tau, -maxF , maxF ) b = -c + p + d
#print("c=",c) qddot = np.linalg.solve(A, b)
return tau tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF, maxF)
#print("c=",c)
return tau

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

@@ -1,123 +1,136 @@
import pybullet as p import pybullet as p
import math import math
import numpy as np import numpy as np
p.connect(p.GUI) p.connect(p.GUI)
plane = p.loadURDF("plane100.urdf") plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf",[0,0,1]) cube = p.loadURDF("cube.urdf", [0, 0, 1])
def getRayFromTo(mouseX,mouseY):
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])
invLen = farPlane*1./lenFwd
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]]
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]]
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() def getRayFromTo(mouseX, mouseY):
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])
invLen = farPlane * 1. / lenFwd
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]
]
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]
]
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]
]
farPlane = 10000 farPlane = 10000
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])] rayForward = [(camTarget[0] - camPos[0]), (camTarget[1] - camPos[1]), (camTarget[2] - camPos[2])]
lenFwd = math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]) lenFwd = math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] * rayForward[1] +
oneOverWidth = float(1)/float(width) rayForward[2] * rayForward[2])
oneOverHeight = float(1)/float(height) oneOverWidth = float(1) / float(width)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth] oneOverHeight = float(1) / float(height)
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight] dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
lendHor = math.sqrt(dHor[0]*dHor[0]+dHor[1]*dHor[1]+dHor[2]*dHor[2])
lendVer = math.sqrt(dVer[0]*dVer[0]+dVer[1]*dVer[1]+dVer[2]*dVer[2])
cornersX = [0,width,width,0] lendHor = math.sqrt(dHor[0] * dHor[0] + dHor[1] * dHor[1] + dHor[2] * dHor[2])
cornersY = [0,0,height,height] lendVer = math.sqrt(dVer[0] * dVer[0] + dVer[1] * dVer[1] + dVer[2] * dVer[2])
corners3D=[]
imgW = int(width/10) cornersX = [0, width, width, 0]
imgH = int(height/10) cornersY = [0, 0, height, height]
corners3D = []
img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL) imgW = int(width / 10)
rgbBuffer=img[2] imgH = int(height / 10)
img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer = img[2]
depthBuffer = img[3] depthBuffer = img[3]
print("rgbBuffer.shape=",rgbBuffer.shape) print("rgbBuffer.shape=", rgbBuffer.shape)
print("depthBuffer.shape=",depthBuffer.shape) print("depthBuffer.shape=", depthBuffer.shape)
#disable rendering temporary makes adding objects faster #disable rendering temporary makes adding objects faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1,1,1,1], radius=0.03 ) visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1, 1, 1, 1], radius=0.03)
collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale) collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
for i in range (4): for i in range(4):
w = cornersX[i] w = cornersX[i]
h = cornersY[i] h = cornersY[i]
rayFrom,rayTo, _= getRayFromTo(w,h) rayFrom, rayTo, _ = getRayFromTo(w, h)
rf = np.array(rayFrom) rf = np.array(rayFrom)
rt = np.array(rayTo) rt = np.array(rayTo)
vec = rt-rf vec = rt - rf
l = np.sqrt(np.dot(vec,vec)) l = np.sqrt(np.dot(vec, vec))
newTo = (0.01/l)*vec+rf newTo = (0.01 / l) * vec + rf
#print("len vec=",np.sqrt(np.dot(vec,vec))) #print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom,newTo,[1,0,0]) p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
corners3D.append(newTo) corners3D.append(newTo)
count = 0 count = 0
stepX=5 stepX = 5
stepY=5 stepY = 5
for w in range(0,imgW,stepX): for w in range(0, imgW, stepX):
for h in range (0,imgH,stepY): for h in range(0, imgH, stepY):
count+=1 count += 1
if ((count % 100)==0): if ((count % 100) == 0):
print(count,"out of ", imgW*imgH/(stepX*stepY)) print(count, "out of ", imgW * imgH / (stepX * stepY))
rayFrom,rayTo, alpha = getRayFromTo(w*(width/imgW),h*(height/imgH)) rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW), h * (height / imgH))
rf = np.array(rayFrom) rf = np.array(rayFrom)
rt = np.array(rayTo) rt = np.array(rayTo)
vec = rt-rf vec = rt - rf
l = np.sqrt(np.dot(vec,vec)) l = np.sqrt(np.dot(vec, vec))
depthImg = float(depthBuffer[h,w]) depthImg = float(depthBuffer[h, w])
far=1000. far = 1000.
near=0.01 near = 0.01
depth = far * near / (far - (far - near) * depthImg) depth = far * near / (far - (far - near) * depthImg)
depth/=math.cos(alpha) depth /= math.cos(alpha)
newTo = (depth/l)*vec+rf newTo = (depth / l) * vec + rf
p.addUserDebugLine(rayFrom,newTo,[1,0,0]) p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
mb = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = newTo, useMaximalCoordinates=True) mb = p.createMultiBody(baseMass=0,
color = rgbBuffer[h,w] baseCollisionShapeIndex=collisionShapeId,
color=[color[0]/255.,color[1]/255.,color[2]/255.,1] baseVisualShapeIndex=visualShapeId,
p.changeVisualShape(mb,-1,rgbaColor=color) basePosition=newTo,
p.addUserDebugLine(corners3D[0],corners3D[1],[1,0,0]) useMaximalCoordinates=True)
p.addUserDebugLine(corners3D[1],corners3D[2],[1,0,0]) color = rgbBuffer[h, w]
p.addUserDebugLine(corners3D[2],corners3D[3],[1,0,0]) color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.addUserDebugLine(corners3D[3],corners3D[0],[1,0,0]) p.changeVisualShape(mb, -1, rgbaColor=color)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) p.addUserDebugLine(corners3D[0], corners3D[1], [1, 0, 0])
p.addUserDebugLine(corners3D[1], corners3D[2], [1, 0, 0])
p.addUserDebugLine(corners3D[2], corners3D[3], [1, 0, 0])
p.addUserDebugLine(corners3D[3], corners3D[0], [1, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
print("ready\n") print("ready\n")
#p.removeBody(plane) #p.removeBody(plane)
#p.removeBody(cube) #p.removeBody(cube)
while (1): while (1):
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)

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

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

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

@@ -5,31 +5,35 @@ import numpy as np
physicsClient = p.connect(p.GUI) physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
bearStartPos1 = [-3.3,0,0] bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0]) bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1) bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0] bearStartPos2 = [0, 0, 0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg") textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) #p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1 useRealTimeSimulation = 1
if (useRealTimeSimulation): if (useRealTimeSimulation):
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
while 1: while 1:
if (useRealTimeSimulation): if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera() camera = p.getDebugVisualizerCamera()
viewMat = camera[2] viewMat = camera[2]
projMat = camera[3] projMat = camera[3]
#An example of setting the view matrix for the projective texture. #An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1]) #viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat) p.getCameraImage(300,
p.setGravity(0,0,0) 300,
else: renderer=p.ER_BULLET_HARDWARE_OPENGL,
p.stepSimulation() flags=p.ER_USE_PROJECTIVE_TEXTURE,
projectiveTextureView=viewMat,
projectiveTextureProj=projMat)
p.setGravity(0, 0, 0)
else:
p.stepSimulation()

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

@@ -4,43 +4,102 @@ import math
def drawInertiaBox(parentUid, parentLinkIndex, color): def drawInertiaBox(parentUid, parentLinkIndex, color):
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex) dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
mass=dyn[0] mass = dyn[0]
frictionCoeff=dyn[1] frictionCoeff = dyn[1]
inertia = dyn[2] inertia = dyn[2]
if (mass>0): if (mass > 0):
Ixx = inertia[0] Ixx = inertia[0]
Iyy = inertia[1] Iyy = inertia[1]
Izz = inertia[2] Izz = inertia[2]
boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass); boxScaleX = 0.5 * math.sqrt(6 * (Izz + Iyy - Ixx) / mass)
boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass); boxScaleY = 0.5 * math.sqrt(6 * (Izz + Ixx - Iyy) / mass)
boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass); boxScaleZ = 0.5 * math.sqrt(6 * (Ixx + Iyy - Izz) / mass)
halfExtents = [boxScaleX,boxScaleY,boxScaleZ] halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0],halfExtents[1],halfExtents[2]], pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
[-halfExtents[0],halfExtents[1],halfExtents[2]], [-halfExtents[0], halfExtents[1], halfExtents[2]],
[halfExtents[0],-halfExtents[1],halfExtents[2]], [halfExtents[0], -halfExtents[1], halfExtents[2]],
[-halfExtents[0],-halfExtents[1],halfExtents[2]], [-halfExtents[0], -halfExtents[1], halfExtents[2]],
[halfExtents[0],halfExtents[1],-halfExtents[2]], [halfExtents[0], halfExtents[1], -halfExtents[2]],
[-halfExtents[0],halfExtents[1],-halfExtents[2]], [-halfExtents[0], halfExtents[1], -halfExtents[2]],
[halfExtents[0],-halfExtents[1],-halfExtents[2]], [halfExtents[0], -halfExtents[1], -halfExtents[2]],
[-halfExtents[0],-halfExtents[1],-halfExtents[2]]] [-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
p.addUserDebugLine(pts[0],
p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[1],
p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 1,
p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],
p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[3],
p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 1,
parentObjectUniqueId=parentUid,
p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],
p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) pts[2],
p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[0],
pts[4],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[1],
pts[5],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[2],
pts[6],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[3],
pts[7],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 0],
pts[4 + 1],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 1],
pts[4 + 3],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 3],
pts[4 + 2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 2],
pts[4 + 0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
toeConstraint = True toeConstraint = True
@@ -48,12 +107,12 @@ useMaximalCoordinates = False
useRealTime = 1 useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance #the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1./100 fixedTimeStep = 1. / 100
numSolverIterations = 50 numSolverIterations = 50
if (useMaximalCoordinates): if (useMaximalCoordinates):
fixedTimeStep = 1./500 fixedTimeStep = 1. / 500
numSolverIterations = 200 numSolverIterations = 200
speed = 10 speed = 10
amplitude = 0.8 amplitude = 0.8
@@ -64,32 +123,37 @@ kp = 1
kd = .5 kd = .5
maxKneeForce = 1000 maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId<0): if (physId < 0):
p.connect(p.GUI) p.connect(p.GUI)
#p.resetSimulation() #p.resetSimulation()
angle = 0 # pick in range 0..0.2 radians angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0,angle,0]) orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf",[0,0,0],orn) p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations) p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000) p.setTimeOut(4000000)
p.setGravity(0,0,0) p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep) p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4]) orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_USE_IMPLICIT_CYLINDER) quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
for i in range(nJoints): for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i) jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0] jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint'] motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
@@ -116,183 +180,286 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0]) #fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir=[-1,-1,-1,-1,1,1,1,1] motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679 halfpi = 1.57079632679
twopi = 4*halfpi twopi = 4 * halfpi
kneeangle = -2.1834 kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped,-1) dyn = p.getDynamicsInfo(quadruped, -1)
mass=dyn[0] mass = dyn[0]
friction=dyn[1] friction = dyn[1]
localInertiaDiagonal = dyn[2] localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal",localInertiaDiagonal) print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API #this is a no-op, just to show the API
p.changeDynamics(quadruped,-1,localInertiaDiagonal=localInertiaDiagonal) p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints): #for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001]) # p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
drawInertiaBox(quadruped,-1, [1,0,0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0]) #drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range (nJoints): for i in range(nJoints):
drawInertiaBox(quadruped,i, [0,1,0]) drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates): if (useMaximalCoordinates):
steps = 400 steps = 400
for aa in range (steps): for aa in range(steps):
p.setJointMotorControl2(quadruped,motor_front_leftL_joint,p.POSITION_CONTROL,motordir[0]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps) motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps) motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps) motordir[2] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,motor_back_rightL_joint,p.POSITION_CONTROL,motordir[6]*halfpi*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps) motordir[3] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_front_leftL_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps) motordir[4] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps) motordir[5] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps) motordir[6] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps) p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps) motordir[7] * halfpi * 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,
p.stepSimulation() motordir[0] * (kneeangle + twopi) * float(aa) / steps)
#time.sleep(fixedTimeStep) 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)
else: else:
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi) p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle) p.resetJointState(quadruped, knee_front_leftL_link, motordir[0] * kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi) p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle) p.resetJointState(quadruped, knee_front_leftR_link, motordir[1] * kneeangle)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi) p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle) p.resetJointState(quadruped, knee_back_leftL_link, motordir[2] * kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi) p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle) p.resetJointState(quadruped, knee_back_leftR_link, motordir[3] * kneeangle)
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle)
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle)
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) p.resetJointState(quadruped, knee_back_rightL_link, motordir[6] * kneeangle)
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) p.resetJointState(quadruped, knee_back_rightR_link, motordir[7] * kneeangle)
#p.getNumJoints(1) #p.getNumJoints(1)
if (toeConstraint): if (toeConstraint):
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link,
p.changeConstraint(cid,maxForce=maxKneeForce) p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) p.changeConstraint(cid, maxForce=maxKneeForce)
p.changeConstraint(cid,maxForce=maxKneeForce) cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link,
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.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid,maxForce=maxKneeForce) p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1]) cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link,
p.changeConstraint(cid,maxForce=maxKneeForce) 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])
p.changeConstraint(cid, maxForce=maxKneeForce)
if (1): if (1):
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0,
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_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.setGravity(0,0,-10) p.setGravity(0, 0, -10)
legnumbering = [
motor_front_leftL_joint, motor_front_leftR_joint, motor_back_leftL_joint,
motor_back_leftR_joint, motor_front_rightL_joint, motor_front_rightR_joint,
motor_back_rightL_joint, motor_back_rightR_joint
]
for i in range(8):
legnumbering=[ print(legnumbering[i])
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 #use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) jointIndex=legnumbering[0],
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) controlMode=p.POSITION_CONTROL,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) targetPosition=motordir[0] * 1.57,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) positionGain=kp,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) velocityGain=kd,
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
#stand still #stand still
p.setRealTimeSimulation(useRealTime) p.setRealTimeSimulation(useRealTime)
t = 0.0 t = 0.0
t_end = t + 15 t_end = t + 15
ref_time = time.time() ref_time = time.time()
while (t<t_end): while (t < t_end):
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
if (useRealTime): if (useRealTime):
t = time.time()-ref_time t = time.time() - ref_time
else: else:
t = t+fixedTimeStep t = t + fixedTimeStep
if (useRealTime==0): if (useRealTime == 0):
p.stepSimulation() p.stepSimulation()
time.sleep(fixedTimeStep) time.sleep(fixedTimeStep)
print("quadruped Id = ") print("quadruped Id = ")
print(quadruped) print(quadruped)
p.saveWorld("quadru.py") p.saveWorld("quadru.py")
logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"quadrupedLog.bin",[quadruped]) logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR, "quadrupedLog.bin", [quadruped])
#jump #jump
t = 0.0 t = 0.0
t_end = t + 100 t_end = t + 100
i=0 i = 0
ref_time = time.time() ref_time = time.time()
while (1): while (1):
if (useRealTime): if (useRealTime):
t = time.time()-ref_time t = time.time() - ref_time
else: else:
t = t+fixedTimeStep t = t + fixedTimeStep
if (True): if (True):
target = math.sin(t*speed)*jump_amp+1.57;
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
if (useRealTime==0):
p.stepSimulation()
time.sleep(fixedTimeStep)
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)

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

@@ -10,15 +10,16 @@ import os, fnmatch
import argparse import argparse
from time import sleep from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb') f = open(filename, 'rb')
print('Opened'), print('Opened'),
print(filename) print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',') keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n') fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record # The byte number of one record
sz = struct.calcsize(fmt) sz = struct.calcsize(fmt)
# The type number of one record # The type number of one record
@@ -38,9 +39,9 @@ def readLogFile(filename, verbose = True):
wholeFile = f.read() wholeFile = f.read()
# split by alignment word # split by alignment word
chunks = wholeFile.split(b'\xaa\xbb') chunks = wholeFile.split(b'\xaa\xbb')
print ("num chunks") print("num chunks")
print (len(chunks)) print(len(chunks))
log = list() log = list()
for chunk in chunks: for chunk in chunks:
if len(chunk) == sz: if len(chunk) == sz:
@@ -52,9 +53,10 @@ def readLogFile(filename, verbose = True):
return log return log
clid = p.connect(p.SHARED_MEMORY) clid = p.connect(p.SHARED_MEMORY)
log = readLogFile("LOG00076.TXT"); log = readLogFile("LOG00076.TXT")
recordNum = len(log) recordNum = len(log)
print('record num:'), print('record num:'),
@@ -72,8 +74,6 @@ maxForce = 3.5
kp = .05 kp = .05
kd = .5 kd = .5
quadruped = 1 quadruped = 1
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
@@ -106,21 +106,71 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
motorDir = [1, 1, 1, 1, 1, 1, 1, 1]; motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint] legnumbering = [
motor_front_leftR_joint, motor_front_leftL_joint, motor_back_leftR_joint,
motor_back_leftL_joint, motor_front_rightR_joint, motor_front_rightL_joint,
motor_back_rightR_joint, motor_back_rightL_joint
]
for record in log: for record in log:
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce) jointIndex=legnumbering[0],
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce) controlMode=p.POSITION_CONTROL,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce) targetPosition=motorDir[0] * record[7],
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce) positionGain=kp,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce) velocityGain=kd,
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce) force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce) p.setJointMotorControl2(bodyIndex=quadruped,
p.setGravity(0.000000,0.000000,-10.000000) 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()
p.stepSimulation() p.stepSimulation()
sleep(0.01) sleep(0.01)

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

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

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

@@ -1,9 +1,9 @@
import os, inspect import os, inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
print ("current_dir=" + currentdir) print("current_dir=" + currentdir)
parentdir = os.path.join(currentdir,"../gym") parentdir = os.path.join(currentdir, "../gym")
os.sys.path.insert(0,parentdir) os.sys.path.insert(0, parentdir)
import pybullet as p import pybullet as p
import pybullet_data import pybullet_data
@@ -11,48 +11,52 @@ import pybullet_data
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
useRealTimeSim = 1 useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows) #for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this p.setRealTimeSimulation(useRealTimeSim) # either this
#p.loadURDF("plane.urdf") #p.loadURDF("plane.urdf")
p.loadSDF(os.path.join(pybullet_data.getDataPath(),"stadium.sdf")) p.loadSDF(os.path.join(pybullet_data.getDataPath(), "stadium.sdf"))
car = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"racecar/racecar.urdf")) car = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "racecar/racecar.urdf"))
for i in range (p.getNumJoints(car)): for i in range(p.getNumJoints(car)):
print (p.getJointInfo(car,i)) print(p.getJointInfo(car, i))
inactive_wheels = [3,5,7] inactive_wheels = [3, 5, 7]
wheels = [2] wheels = [2]
for wheel in inactive_wheels: for wheel in inactive_wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
steering = [4,6]
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-10,10,0) steering = [4, 6]
maxForceSlider = p.addUserDebugParameter("maxForce",0,10,10)
steeringSlider = p.addUserDebugParameter("steering",-0.5,0.5,0) targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -10, 10, 0)
maxForceSlider = p.addUserDebugParameter("maxForce", 0, 10, 10)
steeringSlider = p.addUserDebugParameter("steering", -0.5, 0.5, 0)
while (True): while (True):
maxForce = p.readUserDebugParameter(maxForceSlider) maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider) targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider) steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity) #print(targetVelocity)
for wheel in wheels: for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) p.setJointMotorControl2(car,
wheel,
for steer in steering: p.VELOCITY_CONTROL,
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=steeringAngle) targetVelocity=targetVelocity,
force=maxForce)
steering
if (useRealTimeSim==0): for steer in steering:
p.stepSimulation() p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=steeringAngle)
time.sleep(0.01)
steering
if (useRealTimeSim == 0):
p.stepSimulation()
time.sleep(0.01)

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

@@ -2,73 +2,131 @@ import pybullet as p
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
useRealTimeSim = 1 useRealTimeSim = 1
#for video recording (works best on Mac and Linux, not well on Windows) #for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4") #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf") #p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True) car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)): for i in range(p.getNumJoints(car)):
print (p.getJointInfo(car,i)) print(p.getJointInfo(car, i))
for wheel in range(p.getNumJoints(car)): for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0) p.setJointMotorControl2(car, wheel, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
p.getJointInfo(car,wheel) p.getJointInfo(car, wheel)
wheels = [8,15] wheels = [8, 15]
print("----------------") print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10) #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
p.changeConstraint(c,gearRatio=1, maxForce=10000) 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,
p.changeConstraint(c,gearRatio=-1, maxForce=10000) 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,
p.changeConstraint(c,gearRatio=-1, maxForce=10000) 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,
p.changeConstraint(c,gearRatio=1, maxForce=10000) 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])
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,
p.changeConstraint(c,gearRatio=-1, maxForce=10000) 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,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) c = p.createConstraint(car,
p.changeConstraint(c,gearRatio=-1, maxForce=10000) 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])
p.changeConstraint(c, gearRatio=-1, gearAuxLink=15, 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]) steering = [0, 2]
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])
p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity", -50, 50, 0)
steering = [0,2] maxForceSlider = p.addUserDebugParameter("maxForce", 0, 50, 20)
steeringSlider = p.addUserDebugParameter("steering", -1, 1, 0)
targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
while (True): while (True):
maxForce = p.readUserDebugParameter(maxForceSlider) maxForce = p.readUserDebugParameter(maxForceSlider)
targetVelocity = p.readUserDebugParameter(targetVelocitySlider) targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
steeringAngle = p.readUserDebugParameter(steeringSlider) steeringAngle = p.readUserDebugParameter(steeringSlider)
#print(targetVelocity) #print(targetVelocity)
for wheel in wheels: for wheel in wheels:
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce) p.setJointMotorControl2(car,
wheel,
for steer in steering: p.VELOCITY_CONTROL,
p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle) targetVelocity=targetVelocity,
force=maxForce)
steering
if (useRealTimeSim==0): for steer in steering:
p.stepSimulation() p.setJointMotorControl2(car, steer, p.POSITION_CONTROL, targetPosition=-steeringAngle)
time.sleep(0.01)
steering
if (useRealTimeSim == 0):
p.stepSimulation()
time.sleep(0.01)

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

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

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

@@ -11,16 +11,16 @@ import numpy as np
import pybullet import pybullet
from multiprocessing import Process from multiprocessing import Process
camTargetPos = [0,0,0] camTargetPos = [0, 0, 0]
cameraUp = [0,0,1] cameraUp = [0, 0, 1]
cameraPos = [1,1,1] cameraPos = [1, 1, 1]
pitch = -10.0 pitch = -10.0
roll=0 roll = 0
upAxisIndex = 2 upAxisIndex = 2
camDistance = 4 camDistance = 4
pixelWidth = 84 # 320 pixelWidth = 84 # 320
pixelHeight = 84 # 200 pixelHeight = 84 # 200
nearPlane = 0.01 nearPlane = 0.01
farPlane = 100 farPlane = 100
fov = 60 fov = 60
@@ -29,118 +29,122 @@ import matplotlib.pyplot as plt
class BulletSim(): class BulletSim():
def __init__(self, connection_mode, *argv):
self.connection_mode = connection_mode
self.argv = argv
def __enter__(self): def __init__(self, connection_mode, *argv):
print("connecting") self.connection_mode = connection_mode
optionstring='--width={} --height={}'.format(pixelWidth,pixelHeight) self.argv = argv
optionstring += ' --window_backend=2 --render_device=0'
print(self.connection_mode, optionstring,*self.argv) def __enter__(self):
cid = pybullet.connect(self.connection_mode, options=optionstring,*self.argv) print("connecting")
if cid < 0: optionstring = '--width={} --height={}'.format(pixelWidth, pixelHeight)
raise ValueError optionstring += ' --window_backend=2 --render_device=0'
print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,0)
pybullet.resetSimulation() print(self.connection_mode, optionstring, *self.argv)
pybullet.loadURDF("plane.urdf",[0,0,-1]) cid = pybullet.connect(self.connection_mode, options=optionstring, *self.argv)
pybullet.loadURDF("r2d2.urdf") if cid < 0:
pybullet.loadURDF("duck_vhacd.urdf") raise ValueError
pybullet.setGravity(0,0,-10) print("connected")
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
pybullet.resetSimulation()
pybullet.loadURDF("plane.urdf", [0, 0, -1])
pybullet.loadURDF("r2d2.urdf")
pybullet.loadURDF("duck_vhacd.urdf")
pybullet.setGravity(0, 0, -10)
def __exit__(self, *_, **__):
pybullet.disconnect()
def __exit__(self,*_,**__):
pybullet.disconnect()
def test(num_runs=300, shadow=1, log=True, plot=False): def test(num_runs=300, shadow=1, log=True, plot=False):
if log: if log:
logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings") logId = pybullet.startStateLogging(pybullet.STATE_LOGGING_PROFILE_TIMINGS, "renderTimings")
if plot:
plt.ion()
img = np.random.rand(200, 320)
#img = [tandard_normal((50,100))
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],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1. / duration
#print("fps=",fps)
else:
fps = 0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot: if plot:
plt.ion() rgb = img_arr[2]
image.set_data(rgb) #np_img_arr)
img = np.random.rand(200, 320) ax.plot([0])
#img = [tandard_normal((50,100)) #plt.draw()
image = plt.imshow(img,interpolation='none',animated=True,label="blah") #plt.show()
ax = plt.gca() plt.pause(0.01)
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],
renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
stop = time.time()
duration = (stop - start)
if (duration):
fps = 1./duration
#print("fps=",fps)
else:
fps=0
#print("fps=",fps)
#print("duraction=",duration)
#print("fps=",fps)
times[i] = fps
if plot:
rgb = img_arr[2]
image.set_data(rgb)#np_img_arr)
ax.plot([0])
#plt.draw()
#plt.show()
plt.pause(0.01)
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
mean_time = float(np.mean(times))
print("mean: {0} for {1} runs".format(mean_time, num_runs))
print("")
if log:
pybullet.stopStateLogging(logId)
return mean_time
if __name__ == "__main__": if __name__ == "__main__":
res = []
res = [] with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT")
mean_time = test(log=False, plot=True)
res.append(("tiny", mean_time))
with BulletSim(pybullet.DIRECT): with BulletSim(pybullet.DIRECT):
print("\nTesting DIRECT") plugin_fn = os.path.join(
mean_time = test(log=False,plot=True) pybullet.__file__.split("bullet3")[0],
res.append(("tiny",mean_time)) "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")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin", mean_time))
with BulletSim(pybullet.DIRECT): with BulletSim(pybullet.GUI):
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") print("\nTesting GUI")
plugin = pybullet.loadPlugin(plugin_fn,"_tinyRendererPlugin") mean_time = test(log=False)
if plugin < 0: res.append(("egl", mean_time))
print("\nPlugin Failed to load!\n")
sys.exit()
print("\nTesting DIRECT+OpenGL")
mean_time = test(log=True)
res.append(("plugin",mean_time))
with BulletSim(pybullet.GUI):
print("\nTesting GUI")
mean_time = test(log=False)
res.append(("egl",mean_time))
print()
print("rendertest.py")
print("back nenv fps fps_tot")
for r in res:
print(r[0],"\t",1,round(r[1]),"\t",round(r[1]))
print()
print("rendertest.py")
print("back nenv fps fps_tot")
for r in res:
print(r[0], "\t", 1, round(r[1]), "\t", round(r[1]))

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

@@ -15,11 +15,11 @@ import pybullet as p
from itertools import cycle from itertools import cycle
import numpy as np import numpy as np
camTargetPos = [0,0,0] camTargetPos = [0, 0, 0]
cameraUp = [0,0,1] cameraUp = [0, 0, 1]
cameraPos = [1,1,1] cameraPos = [1, 1, 1]
pitch = -10.0 pitch = -10.0
roll=0 roll = 0
upAxisIndex = 2 upAxisIndex = 2
camDistance = 4 camDistance = 4
pixelWidth = 320 pixelWidth = 320
@@ -30,113 +30,126 @@ fov = 60
class TestEnv(gym.Env): class TestEnv(gym.Env):
def __init__(self,
renderer = 'tiny', # ('tiny', 'egl', 'debug')
):
self.action_space = spaces.Discrete(2)
self.iter = cycle(range(0,360,10))
# how we want to show def __init__(
assert renderer in ('tiny', 'egl', 'debug','plugin') self,
self._renderer = renderer renderer='tiny', # ('tiny', 'egl', 'debug')
self._render_width = 84 ):
self._render_height = 84 self.action_space = spaces.Discrete(2)
# connecting self.iter = cycle(range(0, 360, 10))
if self._renderer == "tiny" or self._renderer == "plugin":
optionstring='--width={} --height={}'.format(self._render_width,self._render_height)
p.connect(p.DIRECT, options=optionstring)
if self._renderer == "plugin": # how we want to show
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") assert renderer in ('tiny', 'egl', 'debug', 'plugin')
plugin = p.loadPlugin(plugin_fn,"_tinyRendererPlugin") self._renderer = renderer
if plugin < 0: self._render_width = 84
print("\nPlugin Failed to load! Try installing via `pip install -e .`\n") self._render_height = 84
sys.exit() # connecting
print("plugin =",plugin) if self._renderer == "tiny" or self._renderer == "plugin":
optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
p.connect(p.DIRECT, options=optionstring)
elif self._renderer == "egl": if self._renderer == "plugin":
optionstring='--width={} --height={}'.format(self._render_width,self._render_height) plugin_fn = os.path.join(
optionstring += ' --window_backend=2 --render_device=0' p.__file__.split("bullet3")[0],
p.connect(p.GUI, options=optionstring) "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")
sys.exit()
print("plugin =", plugin)
elif self._renderer == "debug": elif self._renderer == "egl":
#print("Connection: SHARED_MEMORY") optionstring = '--width={} --height={}'.format(self._render_width, self._render_height)
#cid = p.connect(p.SHARED_MEMORY) optionstring += ' --window_backend=2 --render_device=0'
#if (cid<0): p.connect(p.GUI, options=optionstring)
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3,180,-41,[0.52,-0.2,-0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) elif self._renderer == "debug":
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW,0) #print("Connection: SHARED_MEMORY")
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW,0) #cid = p.connect(p.SHARED_MEMORY)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW,0) #if (cid<0):
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1.3, 180, -41, [0.52, -0.2, -0.33])
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
def __del__(self): def __del__(self):
p.disconnect() p.disconnect()
def reset(self): def reset(self):
pass pass
def step(self,action): def step(self, action):
p.stepSimulation() 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],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2]
return np.array(self._observation), 0, 0, {}
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)
else:
env = gym.make(env_id)
env.seed(seed + rank)
env = bench.Monitor(env, logger.get_dir() and os.path.join(logger.get_dir(), str(rank)))
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)])
env.reset()
start = time.time() start = time.time()
for i in range(num_timesteps): yaw = next(self.iter)
action = [env.action_space.sample() for _ in range(num_env)] viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
env.step(action) upAxisIndex)
stop = time.time() aspect = pixelWidth / pixelHeight
duration = (stop - start) projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane)
if (duration): img_arr = p.getCameraImage(pixelWidth,
fps = num_timesteps/duration pixelHeight,
else: viewMatrix,
fps=0 projectionMatrix,
env.close() shadow=1,
return num_env, fps lightDirection=[1, 1, 1],
renderer=p.ER_BULLET_HARDWARE_OPENGL)
#renderer=pybullet.ER_TINY_RENDERER)
self._observation = img_arr[2]
return np.array(self._observation), 0, 0, {}
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)
else:
env = gym.make(env_id)
env.seed(seed + rank)
env = bench.Monitor(env, logger.get_dir() and os.path.join(logger.get_dir(), str(rank)))
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)])
env.reset()
start = time.time()
for i in range(num_timesteps):
action = [env.action_space.sample() for _ in range(num_env)]
env.step(action)
stop = time.time()
duration = (stop - start)
if (duration):
fps = num_timesteps / duration
else:
fps = 0
env.close()
return num_env, fps
if __name__ == "__main__": if __name__ == "__main__":
env_id = "TestEnv" env_id = "TestEnv"
res = [] res = []
for renderer in ('tiny','plugin', 'egl'): for renderer in ('tiny', 'plugin', 'egl'):
for i in (1,8): for i in (1, 8):
tmp = train(env_id,num_env=i,renderer=renderer) tmp = train(env_id, num_env=i, renderer=renderer)
print(renderer,tmp) print(renderer, tmp)
res.append((renderer,tmp)) res.append((renderer, tmp))
print() print()
print("rendertest_sync.py") print("rendertest_sync.py")
print("back nenv fps fps_tot") print("back nenv fps fps_tot")
for renderer,i in res: for renderer, i in res:
print(renderer,'\t', i[0],round(i[1]),'\t',round(i[0]*i[1])) print(renderer, '\t', i[0], round(i[1]), '\t', round(i[0] * i[1]))

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

@@ -3,64 +3,124 @@ import time
import math import math
p.connect(p.GUI) p.connect(p.GUI)
planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593]) planeId = p.loadURDF(fileName="plane.urdf", baseOrientation=[0.25882, 0, 0, 0.96593])
p.loadURDF(fileName="cube.urdf",basePosition=[0,0,2]) p.loadURDF(fileName="cube.urdf", basePosition=[0, 0, 2])
cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4]) cubeId = p.loadURDF(fileName="cube.urdf", baseOrientation=[0, 0, 0, 1], basePosition=[0, 0, 4])
#p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1) #p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=1.0) p.changeDynamics(bodyUniqueId=2, linkIndex=-1, mass=1.0)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
def drawInertiaBox(parentUid, parentLinkIndex):
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);
halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
[-halfExtents[0],halfExtents[1],halfExtents[2]],
[halfExtents[0],-halfExtents[1],halfExtents[2]],
[-halfExtents[0],-halfExtents[1],halfExtents[2]],
[halfExtents[0],halfExtents[1],-halfExtents[2]],
[-halfExtents[0],halfExtents[1],-halfExtents[2]],
[halfExtents[0],-halfExtents[1],-halfExtents[2]],
[-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
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)
drawInertiaBox(cubeId,-1)
t=0 def drawInertiaBox(parentUid, parentLinkIndex):
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)
halfExtents = [boxScaleX, boxScaleY, boxScaleZ]
pts = [[halfExtents[0], halfExtents[1], halfExtents[2]],
[-halfExtents[0], halfExtents[1], halfExtents[2]],
[halfExtents[0], -halfExtents[1], halfExtents[2]],
[-halfExtents[0], -halfExtents[1], halfExtents[2]],
[halfExtents[0], halfExtents[1], -halfExtents[2]],
[-halfExtents[0], halfExtents[1], -halfExtents[2]],
[halfExtents[0], -halfExtents[1], -halfExtents[2]],
[-halfExtents[0], -halfExtents[1], -halfExtents[2]]]
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)
drawInertiaBox(cubeId, -1)
t = 0
while 1: while 1:
t=t+1 t = t + 1
if t > 400: if t > 400:
p.changeDynamics(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01) p.changeDynamics(bodyUniqueId=0, linkIndex=-1, lateralFriction=0.01)
mass1,frictionCoeff1 =p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1) mass1, frictionCoeff1 = p.getDynamicsInfo(bodyUniqueId=planeId, linkIndex=-1)
mass2,frictionCoeff2 =p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1) mass2, frictionCoeff2 = p.getDynamicsInfo(bodyUniqueId=cubeId, linkIndex=-1)
print(mass1, frictionCoeff1)
print (mass1,frictionCoeff1) print(mass2, frictionCoeff2)
print (mass2,frictionCoeff2) time.sleep(1. / 240.)
time.sleep(1./240.) p.stepSimulation()
p.stepSimulation()

52
examples/pybullet/examples/restitution.py
View File

@@ -5,36 +5,36 @@ import pybullet as p
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
cid = p.connect(p.GUI) cid = p.connect(p.GUI)
restitutionId = p.addUserDebugParameter("restitution",0,1,1) restitutionId = p.addUserDebugParameter("restitution", 0, 1, 1)
restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold",0,3,0.2) restitutionVelocityThresholdId = p.addUserDebugParameter("res. vel. threshold", 0, 3, 0.2)
lateralFrictionId = p.addUserDebugParameter("lateral friction",0,1,0.5) lateralFrictionId = p.addUserDebugParameter("lateral friction", 0, 1, 0.5)
spinningFrictionId = p.addUserDebugParameter("spinning friction",0,1,0.03) spinningFrictionId = p.addUserDebugParameter("spinning friction", 0, 1, 0.03)
rollingFrictionId = p.addUserDebugParameter("rolling friction",0,1,0.03) rollingFrictionId = p.addUserDebugParameter("rolling friction", 0, 1, 0.03)
plane = p.loadURDF("plane_with_restitution.urdf") plane = p.loadURDF("plane_with_restitution.urdf")
sphere = p.loadURDF("sphere_with_restitution.urdf",[0,0,2]) sphere = p.loadURDF("sphere_with_restitution.urdf", [0, 0, 2])
p.setRealTimeSimulation(1) p.setRealTimeSimulation(1)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
while (1): while (1):
restitution = p.readUserDebugParameter(restitutionId) restitution = p.readUserDebugParameter(restitutionId)
restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId) restitutionVelocityThreshold = p.readUserDebugParameter(restitutionVelocityThresholdId)
p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold) p.setPhysicsEngineParameter(restitutionVelocityThreshold=restitutionVelocityThreshold)
lateralFriction = p.readUserDebugParameter(lateralFrictionId) lateralFriction = p.readUserDebugParameter(lateralFrictionId)
spinningFriction = p.readUserDebugParameter(spinningFrictionId) spinningFriction = p.readUserDebugParameter(spinningFrictionId)
rollingFriction = p.readUserDebugParameter(rollingFrictionId) rollingFriction = p.readUserDebugParameter(rollingFrictionId)
p.changeDynamics(plane,-1,lateralFriction=1) p.changeDynamics(plane, -1, lateralFriction=1)
p.changeDynamics(sphere,-1,lateralFriction=lateralFriction) p.changeDynamics(sphere, -1, lateralFriction=lateralFriction)
p.changeDynamics(sphere,-1,spinningFriction=spinningFriction) p.changeDynamics(sphere, -1, spinningFriction=spinningFriction)
p.changeDynamics(sphere,-1,rollingFriction=rollingFriction) p.changeDynamics(sphere, -1, rollingFriction=rollingFriction)
p.changeDynamics(plane,-1,restitution=restitution) p.changeDynamics(plane, -1, restitution=restitution)
p.changeDynamics(sphere,-1,restitution=restitution) p.changeDynamics(sphere, -1, restitution=restitution)
pos,orn=p.getBasePositionAndOrientation(sphere) pos, orn = p.getBasePositionAndOrientation(sphere)
#print("pos=") #print("pos=")
#print(pos) #print(pos)
time.sleep(0.01) time.sleep(0.01)

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

@@ -1,30 +1,28 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT p.connect(p.GUI) #or p.SHARED_MEMORY or p.DIRECT
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
huskypos = [0,0,0.1] huskypos = [0, 0, 0.1]
husky = p.loadURDF("husky/husky.urdf",huskypos[0],huskypos[1],huskypos[2]) husky = p.loadURDF("husky/husky.urdf", huskypos[0], huskypos[1], huskypos[2])
numJoints = p.getNumJoints(husky) numJoints = p.getNumJoints(husky)
for joint in range (numJoints) : for joint in range(numJoints):
print (p.getJointInfo(husky,joint)) print(p.getJointInfo(husky, joint))
targetVel = 10 #rad/s targetVel = 10 #rad/s
maxForce = 100 #Newton maxForce = 100 #Newton
for joint in range (2,6) : for joint in range(2, 6):
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, targetVel, maxForce)
for step in range (300): for step in range(300):
p.stepSimulation() p.stepSimulation()
targetVel=-10 targetVel = -10
for joint in range (2,6) : for joint in range(2, 6):
p.setJointMotorControl(husky,joint,p.VELOCITY_CONTROL,targetVel,maxForce) p.setJointMotorControl(husky, joint, p.VELOCITY_CONTROL, targetVel, maxForce)
for step in range (400): for step in range(400):
p.stepSimulation() p.stepSimulation()
p.getContactPoints(husky) p.getContactPoints(husky)
p.disconnect() p.disconnect()

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

@@ -2,25 +2,24 @@ import pybullet as p
import time import time
cid = p.connect(p.SHARED_MEMORY) cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid < 0):
p.connect(p.GUI) p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True) q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll",-1.5,1.5,0) rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0) pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0) yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0) fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0) fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0) fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
while True: while True:
roll = p.readUserDebugParameter(rollId) roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId) pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId) yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId) x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId) y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId) z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll,pitch,yaw])
p.resetBasePositionAndOrientation(q,[x,y,z],orn)
#p.stepSimulation()#not really necessary for this demo, no physics used
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used

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

@@ -1,16 +1,18 @@
import pybullet as p import pybullet as p
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(enableSAT=1) p.setPhysicsEngineParameter(enableSAT=1)
p.loadURDF("cube_concave.urdf",[0,0,-25], globalScaling=50, useFixedBase=True, flags=p.URDF_INITIALIZE_SAT_FEATURES) p.loadURDF("cube_concave.urdf", [0, 0, -25],
p.loadURDF("cube.urdf",[0,0,1], globalScaling=1,flags=p.URDF_INITIALIZE_SAT_FEATURES) globalScaling=50,
p.loadURDF("duck_vhacd.urdf",[1,0,1], globalScaling=1,flags=p.URDF_INITIALIZE_SAT_FEATURES) 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)
while (p.isConnected()): while (p.isConnected()):
p.stepSimulation() p.stepSimulation()
pts = p.getContactPoints() pts = p.getContactPoints()
#print("num contacts = ", len(pts)) #print("num contacts = ", len(pts))
time.sleep(1./240.) time.sleep(1. / 240.)

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

@@ -1,6 +1,6 @@
import pybullet as p import pybullet as p
import math, time import math, time
import difflib,sys import difflib, sys
numSteps = 500 numSteps = 500
numSteps2 = 30 numSteps2 = 30
@@ -10,69 +10,71 @@ verbose = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log") logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
def setupWorld(): def setupWorld():
p.resetSimulation() p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf") p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10]) kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10])
for i in range (p.getNumJoints(kukaId)): for i in range(p.getNumJoints(kukaId)):
p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0) p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
for i in range (numObjects): for i in range(numObjects):
cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2]) cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
#p.changeDynamics(cube,-1,mass=100) #p.changeDynamics(cube,-1,mass=100)
p.stepSimulation() p.stepSimulation()
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
def dumpStateToFile(file): def dumpStateToFile(file):
for i in range (p.getNumBodies()): for i in range(p.getNumBodies()):
pos,orn = p.getBasePositionAndOrientation(i) pos, orn = p.getBasePositionAndOrientation(i)
linVel,angVel = p.getBaseVelocity(i) linVel, angVel = p.getBaseVelocity(i)
txtPos = "pos="+str(pos)+"\n" txtPos = "pos=" + str(pos) + "\n"
txtOrn = "orn="+str(orn)+"\n" txtOrn = "orn=" + str(orn) + "\n"
txtLinVel = "linVel"+str(linVel)+"\n" txtLinVel = "linVel" + str(linVel) + "\n"
txtAngVel = "angVel"+str(angVel)+"\n" txtAngVel = "angVel" + str(angVel) + "\n"
file.write(txtPos) file.write(txtPos)
file.write(txtOrn) file.write(txtOrn)
file.write(txtLinVel) file.write(txtLinVel)
file.write(txtAngVel) file.write(txtAngVel)
def compareFiles(file1,file2):
diff = difflib.unified_diff( def compareFiles(file1, file2):
file1.readlines(), diff = difflib.unified_diff(
file2.readlines(), file1.readlines(),
fromfile='saveFile.txt', file2.readlines(),
tofile='restoreFile.txt', fromfile='saveFile.txt',
) tofile='restoreFile.txt',
numDifferences = 0 )
for line in diff: numDifferences = 0
numDifferences = numDifferences+1 for line in diff:
sys.stdout.write(line) numDifferences = numDifferences + 1
if (numDifferences>0): sys.stdout.write(line)
print("Error:", numDifferences, " lines are different between files.") if (numDifferences > 0):
else: print("Error:", numDifferences, " lines are different between files.")
print("OK, files are identical") else:
print("OK, files are identical")
setupWorld() setupWorld()
for i in range (numSteps): for i in range(numSteps):
p.stepSimulation() p.stepSimulation()
p.saveBullet("state.bullet") p.saveBullet("state.bullet")
if verbose: if verbose:
p.setInternalSimFlags(1) p.setInternalSimFlags(1)
p.stepSimulation() p.stepSimulation()
if verbose: if verbose:
p.setInternalSimFlags(0) p.setInternalSimFlags(0)
print("contact points=") print("contact points=")
for q in p.getContactPoints(): for q in p.getContactPoints():
print(q) print(q)
for i in range (numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("saveFile.txt", "w")
file = open("saveFile.txt","w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
################################# #################################
setupWorld() setupWorld()
@@ -80,58 +82,55 @@ setupWorld()
#both restore from file or from in-memory state should work #both restore from file or from in-memory state should work
p.restoreState(fileName="state.bullet") p.restoreState(fileName="state.bullet")
stateId = p.saveState() stateId = p.saveState()
print("stateId=",stateId) print("stateId=", stateId)
p.removeState(stateId) p.removeState(stateId)
stateId = p.saveState() stateId = p.saveState()
print("stateId=",stateId) print("stateId=", stateId)
if verbose: if verbose:
p.setInternalSimFlags(1) p.setInternalSimFlags(1)
p.stepSimulation() p.stepSimulation()
if verbose: if verbose:
p.setInternalSimFlags(0) p.setInternalSimFlags(0)
print("contact points restored=") print("contact points restored=")
for q in p.getContactPoints(): for q in p.getContactPoints():
print(q) print(q)
for i in range (numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("restoreFile.txt", "w")
file = open("restoreFile.txt","w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
p.restoreState(stateId) p.restoreState(stateId)
if verbose: if verbose:
p.setInternalSimFlags(1) p.setInternalSimFlags(1)
p.stepSimulation() p.stepSimulation()
if verbose: if verbose:
p.setInternalSimFlags(0) p.setInternalSimFlags(0)
print("contact points restored=") print("contact points restored=")
for q in p.getContactPoints(): for q in p.getContactPoints():
print(q) print(q)
for i in range (numSteps2): for i in range(numSteps2):
p.stepSimulation() p.stepSimulation()
file = open("restoreFile2.txt", "w")
file = open("restoreFile2.txt","w")
dumpStateToFile(file) dumpStateToFile(file)
file.close() file.close()
file1 = open("saveFile.txt","r") file1 = open("saveFile.txt", "r")
file2 = open("restoreFile.txt","r") file2 = open("restoreFile.txt", "r")
compareFiles(file1,file2) compareFiles(file1, file2)
file1.close() file1.close()
file2.close() file2.close()
file1 = open("saveFile.txt","r") file1 = open("saveFile.txt", "r")
file2 = open("restoreFile2.txt","r") file2 = open("restoreFile2.txt", "r")
compareFiles(file1,file2) compareFiles(file1, file2)
file1.close() file1.close()
file2.close() file2.close()
p.stopStateLogging(logId) p.stopStateLogging(logId)
#while (p.getConnectionInfo()["isConnected"]): #while (p.getConnectionInfo()["isConnected"]):
# time.sleep(1) # time.sleep(1)

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

@@ -1,42 +1,41 @@
import pybullet as p import pybullet as p
p.connect(p.GUI) p.connect(p.GUI)
r2d2 = p.loadURDF("r2d2.urdf",[0,0,1]) r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
for l in range (p.getNumJoints(r2d2)): for l in range(p.getNumJoints(r2d2)):
print(p.getJointInfo(r2d2,l)) print(p.getJointInfo(r2d2, l))
p.loadURDF("r2d2.urdf",[2,0,1]) p.loadURDF("r2d2.urdf", [2, 0, 1])
p.loadURDF("r2d2.urdf",[4,0,1]) p.loadURDF("r2d2.urdf", [4, 0, 1])
p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
segLinkIndex=1 segLinkIndex = 1
verbose = 0 verbose = 0
while (1): while (1):
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
#for k in keys: #for k in keys:
# print("key=",k,"state=", keys[k]) # print("key=",k,"state=", keys[k])
if ord('1') in keys: if ord('1') in keys:
state = keys[ord('1')] state = keys[ord('1')]
if (state & p.KEY_WAS_RELEASED): if (state & p.KEY_WAS_RELEASED):
verbose = 1-verbose verbose = 1 - verbose
if ord('s') in keys: if ord('s') in keys:
state = keys[ord('s')] state = keys[ord('s')]
if (state & p.KEY_WAS_RELEASED): if (state & p.KEY_WAS_RELEASED):
segLinkIndex = 1-segLinkIndex segLinkIndex = 1 - segLinkIndex
#print("segLinkIndex=",segLinkIndex) #print("segLinkIndex=",segLinkIndex)
flags = 0 flags = 0
if (segLinkIndex): if (segLinkIndex):
flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
img = p.getCameraImage(320,200,flags=flags) img = p.getCameraImage(320, 200, flags=flags)
#print(img[0],img[1]) #print(img[0],img[1])
seg=img[4] seg = img[4]
if (verbose): if (verbose):
for pixel in seg: for pixel in seg:
if (pixel>=0): if (pixel >= 0):
obUid = pixel & ((1<<24)-1) obUid = pixel & ((1 << 24) - 1)
linkIndex = (pixel >> 24)-1 linkIndex = (pixel >> 24) - 1
print("obUid=",obUid,"linkIndex=",linkIndex) print("obUid=", obUid, "linkIndex=", linkIndex)
p.stepSimulation()
p.stepSimulation()

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

@@ -6,17 +6,29 @@ p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
visualShift = [0,0,0] visualShift = [0, 0, 0]
collisionShift = [0,0,0] collisionShift = [0, 0, 0]
inertiaShift = [0,0,-0.5] inertiaShift = [0, 0, -0.5]
meshScale=[1,1,1] meshScale = [1, 1, 1]
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,fileName="cube.obj", rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=visualShift, meshScale=meshScale) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="cube.obj", collisionFramePosition=collisionShift,meshScale=meshScale) fileName="cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=visualShift,
meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
fileName="cube.obj",
collisionFramePosition=collisionShift,
meshScale=meshScale)
p.createMultiBody(baseMass=1,baseInertialFramePosition=inertiaShift,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [0,0,1], useMaximalCoordinates=False) p.createMultiBody(baseMass=1,
baseInertialFramePosition=inertiaShift,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 1],
useMaximalCoordinates=False)
while (1): while (1):
p.stepSimulation() p.stepSimulation()
time.sleep(1./240.) time.sleep(1. / 240.)

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

@@ -1,15 +1,14 @@
import pybullet as p import pybullet as p
import pybullet import pybullet
import time import time
p.connect(p.GUI) p.connect(p.GUI)
p.loadURDF("toys/concave_box.urdf") p.loadURDF("toys/concave_box.urdf")
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
for i in range (10): for i in range(10):
p.loadURDF("sphere_1cm.urdf",[i*0.02,0,0.5]) p.loadURDF("sphere_1cm.urdf", [i * 0.02, 0, 0.5])
p.loadURDF("duck_vhacd.urdf") p.loadURDF("duck_vhacd.urdf")
timeStep = 1./240. timeStep = 1. / 240.
p.setTimeStep(timeStep) p.setTimeStep(timeStep)
while (1): while (1):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

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

@@ -1,34 +1,34 @@
import pybullet as p import pybullet as p
import time import time
useMaximalCoordinates=False useMaximalCoordinates = False
flags = p.URDF_ENABLE_SLEEPING flags = p.URDF_ENABLE_SLEEPING
p.connect(p.GUI) p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates) p.loadURDF("plane100.urdf", flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags) #p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1 r2d2 = -1
for k in range (5): for k in range(5):
for i in range (5): for i in range(5):
r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags) r2d2 = p.loadURDF("r2d2.urdf", [k * 2, i * 2, 1],
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES + flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards #enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING) #p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range(p.getNumJoints(r2d2)):
for j in range (p.getNumJoints(r2d2)): p.setJointMotorControl2(r2d2, j, p.VELOCITY_CONTROL, targetVelocity=0)
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0) print("r2d2=", r2d2)
print("r2d2=",r2d2) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) timestep = 1. / 240.
timestep = 1./240.
p.setTimeStep(timestep) p.setTimeStep(timestep)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
while p.isConnected(): while p.isConnected():
p.stepSimulation() p.stepSimulation()
time.sleep(timestep) time.sleep(timestep)
#force the object to wake up #force the object to wake up
p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP) p.changeDynamics(r2d2, -1, activationState=p.ACTIVATION_STATE_WAKE_UP)

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

@@ -2,125 +2,141 @@ import pybullet as p
import time import time
import math import math
# This simple snake logic is from some 15 year old Havok C++ demo # This simple snake logic is from some 15 year old Havok C++ demo
# Thanks to Michael Ewert! # Thanks to Michael Ewert!
p.connect(p.GUI) p.connect(p.GUI)
plane = p.createCollisionShape(p.GEOM_PLANE) plane = p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,plane) p.createMultiBody(0, plane)
useMaximalCoordinates = True useMaximalCoordinates = True
sphereRadius = 0.25 sphereRadius = 0.25
#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) #colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]])
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1 mass = 1
visualShapeId = -1 visualShapeId = -1
link_Masses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
indices=[]
jointTypes=[]
axis=[]
for i in range (36): link_Masses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
indices = []
jointTypes = []
axis = []
for i in range(36):
link_Masses.append(1) link_Masses.append(1)
linkCollisionShapeIndices.append(colBoxId) linkCollisionShapeIndices.append(colBoxId)
linkVisualShapeIndices.append(-1) linkVisualShapeIndices.append(-1)
linkPositions.append([0,sphereRadius*2.0+0.01,0]) linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0])
linkOrientations.append([0,0,0,1]) linkOrientations.append([0, 0, 0, 1])
linkInertialFramePositions.append([0,0,0]) linkInertialFramePositions.append([0, 0, 0])
linkInertialFrameOrientations.append([0,0,0,1]) linkInertialFrameOrientations.append([0, 0, 0, 1])
indices.append(i) indices.append(i)
jointTypes.append(p.JOINT_REVOLUTE) jointTypes.append(p.JOINT_REVOLUTE)
axis.append([0,0,1]) axis.append([0, 0, 1])
basePosition = [0,0,1] basePosition = [0, 0, 1]
baseOrientation = [0,0,0,1] baseOrientation = [0, 0, 0, 1]
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis, useMaximalCoordinates=useMaximalCoordinates) sphereUid = p.createMultiBody(mass,
colBoxId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis,
useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
anistropicFriction = [1,0.01,0.01] anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction) p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid) p.getNumJoints(sphereUid)
for i in range (p.getNumJoints(sphereUid)): for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid,i) p.getJointInfo(sphereUid, i)
p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=anistropicFriction) p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)
dt = 1./240. dt = 1. / 240.
SNAKE_NORMAL_PERIOD=0.1#1.5 SNAKE_NORMAL_PERIOD = 0.1 #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD m_wavePeriod = SNAKE_NORMAL_PERIOD
m_waveLength=4 m_waveLength = 4
m_wavePeriod=1.5 m_wavePeriod = 1.5
m_waveAmplitude=0.4 m_waveAmplitude = 0.4
m_waveFront = 0.0 m_waveFront = 0.0
#our steering value #our steering value
m_steering = 0.0 m_steering = 0.0
m_segmentLength = sphereRadius*2.0 m_segmentLength = sphereRadius * 2.0
forward = 0 forward = 0
while (1): while (1):
keys = p.getKeyboardEvents() keys = p.getKeyboardEvents()
for k,v in keys.items(): for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = -.2 m_steering = -.2
if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0 m_steering = 0
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)): if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)):
m_steering = .2 m_steering = .2
if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)): if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)):
m_steering = 0 m_steering = 0
amp = 0.2 amp = 0.2
offset = 0.6 offset = 0.6
numMuscles = p.getNumJoints(sphereUid) numMuscles = p.getNumJoints(sphereUid)
scaleStart = 1.0 scaleStart = 1.0
#start of the snake with smaller waves. #start of the snake with smaller waves.
#I think starting the wave at the tail would work better ( while it still goes from head to tail ) #I think starting the wave at the tail would work better ( while it still goes from head to tail )
if( m_waveFront < m_segmentLength*4.0 ): if (m_waveFront < m_segmentLength * 4.0):
scaleStart = m_waveFront/(m_segmentLength*4.0) scaleStart = m_waveFront / (m_segmentLength * 4.0)
segment = numMuscles-1 segment = numMuscles - 1
#we simply move a sin wave down the body of the snake. #we simply move a sin wave down the body of the snake.
#this snake may be going backwards, but who can tell ;) #this snake may be going backwards, but who can tell ;)
for joint in range (p.getNumJoints(sphereUid)): for joint in range(p.getNumJoints(sphereUid)):
segment = joint#numMuscles-1-joint segment = joint #numMuscles-1-joint
#map segment to phase #map segment to phase
phase = (m_waveFront - (segment+1)*m_segmentLength)/ m_waveLength phase = (m_waveFront - (segment + 1) * m_segmentLength) / m_waveLength
phase -= math.floor(phase) phase -= math.floor(phase)
phase *= math.pi * 2.0 phase *= math.pi * 2.0
#map phase to curvature #map phase to curvature
targetPos = math.sin( phase ) * scaleStart * m_waveAmplitude targetPos = math.sin(phase) * scaleStart * m_waveAmplitude
#// steer snake by squashing +ve or -ve side of sin curve #// steer snake by squashing +ve or -ve side of sin curve
if( m_steering > 0 and targetPos < 0 ): if (m_steering > 0 and targetPos < 0):
targetPos *= 1.0/( 1.0 + m_steering ) targetPos *= 1.0 / (1.0 + m_steering)
if( m_steering < 0 and targetPos > 0 ): if (m_steering < 0 and targetPos > 0):
targetPos *= 1.0/( 1.0 - m_steering ) targetPos *= 1.0 / (1.0 - m_steering)
#set our motor #set our motor
p.setJointMotorControl2(sphereUid,joint,p.POSITION_CONTROL,targetPosition=targetPos+m_steering,force=30) p.setJointMotorControl2(sphereUid,
joint,
p.POSITION_CONTROL,
targetPosition=targetPos + m_steering,
force=30)
#wave keeps track of where the wave is in time #wave keeps track of where the wave is in time
m_waveFront += dt/m_wavePeriod * m_waveLength; m_waveFront += dt / m_wavePeriod * m_waveLength
p.stepSimulation() p.stepSimulation()
time.sleep(dt) time.sleep(dt)

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

@@ -3,29 +3,28 @@ import time
p.connect(p.GUI) p.connect(p.GUI)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001) p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG,
globalCFM=0.000001)
#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) #p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001)
p.loadURDF("plane.urdf") p.loadURDF("plane.urdf")
radius = 0.025 radius = 0.025
distance = 1.86 distance = 1.86
yaw=135 yaw = 135
pitch=-11 pitch = -11
targetPos=[0,0,0] targetPos = [0, 0, 0]
p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200) p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200)
p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos) p.resetDebugVisualizerCamera(distance, yaw, pitch, targetPos)
objectId = -1 objectId = -1
for i in range (10): for i in range(10):
objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius]) objectId = p.loadURDF("cube_small.urdf", [1, 1, radius + i * 2 * radius])
p.changeDynamics(objectId,-1,100) p.changeDynamics(objectId, -1, 100)
timeStep = 1./240. timeStep = 1. / 240.
p.setGravity(0,0,-10) p.setGravity(0, 0, -10)
while (p.isConnected()): while (p.isConnected()):
p.stepSimulation() p.stepSimulation()
time.sleep(timeStep) time.sleep(timeStep)

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