AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
erwincoumans 2019-05-09 09:41:32 -07:00
commit 2f0ee2870d
370 changed files with 71108 additions and 22971 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.style.yapf Normal file
View File

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

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

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

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

@ -2,8 +2,6 @@ import sys
sys.path.append(".")
import dump
header = """/* Copyright (C) 2011 Erwin Coumans & Charlie C
*
* This software is provided 'as-is', without any express or implied
@ -30,23 +28,25 @@ dtList = dump.DataTypeList
out = "autogenerated/"
spaces = 4
def addSpaces(file, space):
for i in range(0, space):
file.write(" ")
for i in range(0, space):
file.write(" ")
def write(file, spaces, string):
addSpaces(file, spaces)
file.write(string)
addSpaces(file, spaces)
file.write(string)
###################################################################################
blender = open(out+"bullet.h", 'w')
blender = open(out + "bullet.h", 'w')
blender.write(header)
blender.write("#ifndef __BULLET_H__\n")
blender.write("#define __BULLET_H__\n")
#for dt in dtList:
# blender.write("struct %s;\n"%dt.filename)
###################################################################################
blender.write("namespace Bullet {\n")
@ -61,26 +61,25 @@ typedef struct bInvalidHandle {
blender.write(strUnRes)
for dt in dtList:
write(blender, 4, "class %s;\n"%dt.name)
write(blender, 4, "class %s;\n" % dt.name)
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")
write(blender, 4, "public:\n")
for i in dt.dataTypes:
write(blender, 8, i+";\n")
blender.write("// -------------------------------------------------- //\n")
write(blender, 4, "class %s\n" % dt.name)
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("#endif//__BULLET_H__")
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("minor_version:%s" % str(sys.version_info[1]))
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("long_version:%s" % '.'.join(map(lambda x:str(x), sys.version_info[0:3])))
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("py_inc_dir:%s" % distutils.sysconfig.get_python_inc())
print("site_packages_dir:%s" % distutils.sysconfig.get_python_lib(plat_specific=1))
for e in distutils.sysconfig.get_config_vars ('LIBDIR'):
if e != None:
print("py_lib_dir:%s" % e)
break
for e in distutils.sysconfig.get_config_vars('LIBDIR'):
if e != None:
print("py_lib_dir:%s" % e)
break

439
data/dinnerware/generate.py
View File

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

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

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

5
data/threecubes/threecubes.py
View File

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

34
data/threecubes/threecubes_org.py
View File

@ -3,20 +3,30 @@ useEGL = False
useEGLGUI = False
if useEGL:
if useEGLGUI:
p.connect(p.GUI, "window_backend=2")
else:
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
if useEGLGUI:
p.connect(p.GUI, "window_backend=2")
else:
p.connect(p.DIRECT)
p.loadPlugin("eglRendererPlugin")
else:
p.connect(p.GUI)
p.connect(p.GUI)
p.loadURDF("threecubes.urdf", flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
while (1):
viewmat= [0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574, 0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004, 0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0]
projmat= [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
p.getCameraImage(64,64, viewMatrix=viewmat, projectionMatrix=projmat, flags=p.ER_NO_SEGMENTATION_MASK )
p.stepSimulation()
viewmat = [
0.642787516117096, -0.4393851161003113, 0.6275069713592529, 0.0, 0.766044557094574,
0.36868777871131897, -0.5265407562255859, 0.0, -0.0, 0.8191521167755127, 0.5735764503479004,
0.0, 2.384185791015625e-07, 2.384185791015625e-07, -5.000000476837158, 1.0
]
projmat = [
0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0,
0.0, 0.0, -0.02000020071864128, 0.0
]
p.getCameraImage(64,
64,
viewMatrix=viewmat,
projectionMatrix=projmat,
flags=p.ER_NO_SEGMENTATION_MASK)
p.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
if(__name__=="__main__"):
# Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
HeadAttributes=HeadMatch.group(1);
FullDocumentHead=HeadMatch.group(2);
BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
BodyAttributes=BodyMatch.group(1);
FullPreviewBody=BodyMatch.group(2);
ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
ReplacementList=[
("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)",DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
];
for Macro,Replacement in ReplacementList:
FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'"));
# Generate code which sets body and head attributes appropriately
for Element,AttributeCode in [("head",HeadAttributes),("body",BodyAttributes)]:
FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n";
for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"",AttributeCode):
FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
if (__name__ == "__main__"):
# Assemble the script which embeds the Markdeep page into the preview blog
PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
HeadAttributes = HeadMatch.group(1)
FullDocumentHead = HeadMatch.group(2)
BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
BodyAttributes = BodyMatch.group(1)
FullPreviewBody = BodyMatch.group(2)
ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
for Macro, Replacement in ReplacementList:
FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
Macro,
Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
# Generate code which sets body and head attributes appropriately
for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
1) + "\",\"" + Match.group(2) + "\");\r\n"
open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
# Concatenate all the scripts together
SourceFileList=[
"PrepareHTML.full.js",
"SetMarkdeepMode.js",
"markdeep.min.js",
"DisplayMarkdeepOutput.js",
"InvokeMathJax.js"
];
OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]);
OutputFile=open("MarkdeepUtility.js","wb");
OutputFile.write(OutputCode.encode("utf-8"));
OutputFile.close();
print("Done.");
# Concatenate all the scripts together
SourceFileList = [
"PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
"InvokeMathJax.js"
]
OutputCode = "\r\n\r\n".join([
"// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
for SourceFile in SourceFileList
])
OutputFile = open("MarkdeepUtility.js", "wb")
OutputFile.write(OutputCode.encode("utf-8"))
OutputFile.close()
print("Done.")

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

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

2
examples/BlockSolver/BlockSolverExample.cpp
View File

@ -140,7 +140,7 @@ void BlockSolverExample::createMultiBodyStack()
tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
btMultiBody* body = createMultiBody(mass, tr, boxShape);
}
if (0)
if (/* DISABLES CODE */ (0))
{
btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr;

5
examples/ExampleBrowser/CMakeLists.txt
View File

@ -94,6 +94,9 @@ LINK_LIBRARIES(
BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
)
add_definitions(-DINCLUDE_CLOTH_DEMOS)
IF (WIN32)
INCLUDE_DIRECTORIES(
${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/glad
@ -346,6 +349,8 @@ SET(BulletExampleBrowser_SRCS
../MultiBody/InvertedPendulumPDControl.cpp
../MultiBody/InvertedPendulumPDControl.h
../MultiBody/MultiBodyConstraintFeedback.cpp
../SoftDemo/SoftDemo.cpp
../SoftDemo/SoftDemo.h
../MultiBody/MultiDofDemo.cpp
../MultiBody/MultiDofDemo.h
../RigidBody/RigidBodySoftContact.cpp

8
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@ -326,7 +326,10 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
m_robotSim.loadSoftBody("bunny.obj", 0.1, 0.1, 0.02);
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
@ -402,7 +405,8 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
m_robotSim.loadSoftBody("bunny.obj", 0.3, 10.0, 0.1);
b3RobotSimulatorLoadSoftBodyArgs args(0.3, 10, 0.1);
m_robotSim.loadSoftBody("bunny.obj", args);
}
}
virtual void exitPhysics()

24
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -314,6 +314,30 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_initialPosition[0] = startPosX;
command->m_loadSoftBodyArguments.m_initialPosition[1] = startPosY;
command->m_loadSoftBodyArguments.m_initialPosition[2] = startPosZ;
command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_POSITION;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_initialOrientation[0] = startOrnX;
command->m_loadSoftBodyArguments.m_initialOrientation[1] = startOrnY;
command->m_loadSoftBodyArguments.m_initialOrientation[2] = startOrnZ;
command->m_loadSoftBodyArguments.m_initialOrientation[3] = startOrnW;
command->m_updateFlags |= LOAD_SOFT_BODY_INITIAL_ORIENTATION;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -614,6 +614,8 @@ extern "C"
B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);

45
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -1398,12 +1398,51 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
}
case CMD_LOAD_SOFT_BODY_FAILED:
{
b3Warning("loadSoftBody failed");
break;
B3_PROFILE("CMD_LOAD_SOFT_BODY_FAILED");
if (m_data->m_verboseOutput)
{
b3Printf("Server failed loading the SoftBody...\n");
}
break;
}
case CMD_LOAD_SOFT_BODY_COMPLETED:
{
break;
B3_PROFILE("CMD_LOAD_SOFT_BODY_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Server loading the SoftBody OK\n");
}
b3Assert(serverCmd.m_numDataStreamBytes);
if (serverCmd.m_numDataStreamBytes > 0)
{
bParse::btBulletFile bf(
this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
serverCmd.m_numDataStreamBytes);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
if (bf.ok())
{
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
}
else
{
b3Warning("Robot description not received when loading soft body!");
}
}
break;
}
case CMD_SYNC_USER_DATA_FAILED:
{

9
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -1079,7 +1079,12 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
case CMD_LOAD_SOFT_BODY_COMPLETED:
{
break;
int bodyUniqueId = serverCmd.m_loadSoftBodyResultArguments.m_objectUniqueId;
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
bodyJoints->m_baseName = "baseLink";
break;
}
case CMD_SYNC_USER_DATA_FAILED:
{
@ -1541,7 +1546,7 @@ int PhysicsDirect::getNumUserData(int bodyUniqueId) const
void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex <= 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
{
*keyOut = 0;
*userDataIdOut = -1;

401
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -122,24 +122,6 @@ btScalar gRhsClamp = 1.f;
#include "../CommonInterfaces/CommonFileIOInterface.h"
struct UrdfLinkNameMapUtil
{
btMultiBody* m_mb;
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
btDefaultSerializer* m_memSerializer;
UrdfLinkNameMapUtil() : m_mb(0), m_memSerializer(0)
{
}
virtual ~UrdfLinkNameMapUtil()
{
}
};
class b3ThreadPool
{
public:
@ -2861,7 +2843,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
@ -3074,7 +3056,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_data->m_strings.push_back(baseName);
mb->setBaseName(baseName->c_str());
#if 0
btAlignedObjectArray<char> urdf;
mb->dumpUrdf(urdf);
@ -3085,7 +3067,7 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
fclose(f);
}
#endif
}
else
{
@ -3357,73 +3339,77 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
btMultiBody* mb = bodyHandle ? bodyHandle->m_multiBody : 0;
if (mb)
if(!bodyHandle) return 0;
if (bodyHandle->m_multiBody)
{
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
btMultiBody* mb = bodyHandle->m_multiBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
util->m_mb = mb;
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
ser.startSerialization();
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(), 0);
ser.m_skipPointers.insert(mb->getBaseCollider(), 0);
if (mb->getBaseName())
{
util->m_memSerializer->registerNameForPointer(mb->getBaseName(), mb->getBaseName());
ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
}
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i = 0; i < mb->getNumLinks(); i++)
{
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider, 0);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName);
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName, mb->getLink(i).m_jointName);
ser.m_skipPointers.insert(mb->getLink(i).m_collider, 0);
ser.registerNameForPointer(mb->getLink(i).m_linkName, mb->getLink(i).m_linkName);
ser.registerNameForPointer(mb->getLink(i).m_jointName, mb->getLink(i).m_jointName);
}
util->m_memSerializer->registerNameForPointer(mb->getBaseName(), mb->getBaseName());
ser.registerNameForPointer(mb->getBaseName(), mb->getBaseName());
int len = mb->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len, 1);
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
btChunk* chunk = ser.allocate(len, 1);
const char* structType = mb->serialize(chunk->m_oldPtr, &ser);
ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
streamSizeInBytes = ser.getCurrentBufferSize();
}
else
{
btRigidBody* rb = bodyHandle ? bodyHandle->m_rigidBody : 0;
if (rb)
{
UrdfLinkNameMapUtil utilBla;
UrdfLinkNameMapUtil* util = &utilBla;
else if(bodyHandle->m_rigidBody){
btRigidBody* rb = bodyHandle->m_rigidBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
util->m_memSerializer = &ser;
util->m_memSerializer->startSerialization();
util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str());
ser.startSerialization();
ser.registerNameForPointer(bodyHandle->m_rigidBody, bodyHandle->m_bodyName.c_str());
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
for (int i = 0; i < bodyHandle->m_rigidBodyJoints.size(); i++)
{
const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i);
util->m_memSerializer->registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str());
util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str());
ser.registerNameForPointer(con, bodyHandle->m_rigidBodyJointNames[i].c_str());
ser.registerNameForPointer(&con->getRigidBodyB(), bodyHandle->m_rigidBodyLinkNames[i].c_str());
const btRigidBody& bodyA = con->getRigidBodyA();
int len = con->calculateSerializeBufferSize();
btChunk* chunk = util->m_memSerializer->allocate(len, 1);
const char* structType = con->serialize(chunk->m_oldPtr, util->m_memSerializer);
util->m_memSerializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con);
btChunk* chunk = ser.allocate(len, 1);
const char* structType = con->serialize(chunk->m_oldPtr, &ser);
ser.finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, (void*)con);
}
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
streamSizeInBytes = ser.getCurrentBufferSize();
}
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if(bodyHandle->m_softBody){
//minimum serialization, registerNameForPointer
btSoftBody* sb = bodyHandle->m_softBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
ser.startSerialization();
ser.registerNameForPointer(sb, bodyHandle->m_bodyName.c_str());
int len = sb->calculateSerializeBufferSize();
btChunk* chunk = ser.allocate(len, 1);
const char* structType = sb->serialize(chunk->m_oldPtr, &ser);
ser.finalizeChunk(chunk, structType, BT_SOFTBODY_CODE, sb);
streamSizeInBytes = ser.getCurrentBufferSize();
}
#endif
return streamSizeInBytes;
}
@ -5530,7 +5516,11 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
{
int usedHandle = usedHandles[i];
InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (body && (body->m_multiBody || body->m_rigidBody || body->m_softBody))
#else
if (body && (body->m_multiBody || body->m_rigidBody))
#endif
{
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle;
}
@ -6330,7 +6320,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
//we store the state details in the shared memory block, to reduce status size
SendActualStateSharedMemoryStorage* stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
//make sure the storage fits, otherwise
btAssert(sizeof(SendActualStateSharedMemoryStorage) < bufferSizeInBytes);
if (sizeof(SendActualStateSharedMemoryStorage) > bufferSizeInBytes)
@ -6342,11 +6332,12 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
{
btMultiBody* mb = body->m_multiBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks();
serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
@ -6382,24 +6373,24 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
//base position in world space, carthesian
//base position in world space, cartesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
//base orientation, quaternion x,y,z,w, in world space, cartesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian)
//base linear velocity (in world space, cartesian)
stateDetails->m_actualStateQdot[0] = mb->getBaseVel()[0];
stateDetails->m_actualStateQdot[1] = mb->getBaseVel()[1];
stateDetails->m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
//base angular velocity (in world space, cartesian)
stateDetails->m_actualStateQdot[3] = mb->getBaseOmega()[0];
stateDetails->m_actualStateQdot[4] = mb->getBaseOmega()[1];
stateDetails->m_actualStateQdot[5] = mb->getBaseOmega()[2];
@ -6551,59 +6542,104 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
hasStatus = true;
}
else
{
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
else if (body && body->m_rigidBody){
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;
serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
btTransform tr = rb->getWorldTransform();
//base position in world space, carthesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
//base orientation, quaternion x,y,z,w, in world space, carthesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion
btTransform tr = rb->getWorldTransform();
//base position in world space, cartesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base linear velocity (in world space, carthesian)
stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0];
stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1];
stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2];
//base orientation, quaternion x,y,z,w, in world space, cartesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base angular velocity (in world space, carthesian)
stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0];
stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1];
stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
//base linear velocity (in world space, cartesian)
stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0];
stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1];
stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2];
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
//base angular velocity (in world space, cartesian)
stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0];
stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1];
stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
hasStatus = true;
}
else
{
//b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
}
}
return hasStatus;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody)
{
btSoftBody* sb = body->m_softBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;
serverCmd.m_numDataStreamBytes = sizeof(SendActualStateSharedMemoryStorage);
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
btTransform tr = sb->getWorldTransform();
//base position in world space, cartesian
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, cartesian
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
int totalDegreeOfFreedomQ = 7; //pos + quaternion
int totalDegreeOfFreedomU = 6; //3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
hasStatus = true;
}
#endif
else
{
//b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
hasStatus = true;
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
@ -7171,7 +7207,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes>0 && serverStatusOut.m_numDataStreamBytes==0)
{
@ -7179,7 +7215,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
BT_PROFILE("autogenerateGraphicsObjects");
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
}
BT_PROFILE("createBodyInfoStream");
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
@ -7281,17 +7317,34 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
btAssert(loadSoftBodyArgs.m_fileName);
btVector3 initialPos(0, 0, 0);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
{
initialPos[0] = loadSoftBodyArgs.m_initialPosition[0];
initialPos[1] = loadSoftBodyArgs.m_initialPosition[1];
initialPos[2] = loadSoftBodyArgs.m_initialPosition[2];
}
btQuaternion initialOrn(0, 0, 0, 1);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
{
initialOrn[0] = loadSoftBodyArgs.m_initialOrientation[0];
initialOrn[1] = loadSoftBodyArgs.m_initialOrientation[1];
initialOrn[2] = loadSoftBodyArgs.m_initialOrientation[2];
initialOrn[3] = loadSoftBodyArgs.m_initialOrientation[3];
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
{
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
{
mass = clientCmd.m_loadSoftBodyArguments.m_mass;
mass = clientCmd.m_loadSoftBodyArguments.m_mass;
}
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
{
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
}
{
@ -7310,7 +7363,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
if (shapes.size() > 0)
if (!shapes.empty())
{
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices;
@ -7333,9 +7386,11 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->generateBendingConstraints(2, pm);
psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5;
//turn on softbody vs softbody collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711, 0, 0, 0.70711));
psb->translate(btVector3(-0.05, 0, 1.0));
psb->rotate(initialOrn);
psb->translate(initialPos);
psb->scale(btVector3(scale, scale, scale));
psb->setTotalMass(mass, true);
@ -7350,6 +7405,19 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
#ifdef ENABLE_LINK_MAPPER
if (m_data->m_urdfLinkNameMapper.size())
{
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
}
#endif
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
b3Notification notification;
notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
@ -7482,6 +7550,17 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
return hasStatus;
}
void setDefaultRootWorldAABB(SharedMemoryStatus& serverCmd)
{
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
}
bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@ -7496,13 +7575,7 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
btMultiBody* mb = body->m_multiBody;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks();
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
setDefaultRootWorldAABB(serverCmd);
if (body->m_multiBody->getBaseCollider())
{
@ -7544,21 +7617,14 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
}
}
}
else
else if (body && body->m_rigidBody)
{
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0;
setDefaultRootWorldAABB(serverCmd);
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1;
if (rb->getCollisionShape())
{
btTransform tr = rb->getWorldTransform();
@ -7573,8 +7639,15 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
}
}
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
setDefaultRootWorldAABB(serverCmd);
}
#endif
return hasStatus;
}
@ -7605,7 +7678,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
int numSteps = 0;
if (m_data->m_numSimulationSubSteps > 0)
{
@ -7632,7 +7705,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
for (int i=0;i<numIslands;i++)
{
serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
@ -7724,7 +7797,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setCanWakeup(false);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
@ -7815,7 +7888,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
@ -7830,7 +7903,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
}
}
else
{
@ -7918,7 +7991,7 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
@ -8009,13 +8082,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
rb->setAnisotropicFriction(anisotropicFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
{
rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
@ -8171,6 +8244,14 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
}
hasStatus = true;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
//todo: @fuchuyuan implement dynamics info
b3Warning("Softbody dynamics info not set!!!");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
}
#endif
return hasStatus;
}
@ -8430,7 +8511,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics;
}
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
return hasStatus;
@ -8524,7 +8605,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
{
int posVarCount = mb->getLink(i).m_posVarCount;
bool hasPosVar = posVarCount > 0;
for (int j = 0; j < posVarCount; j++)
{
if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
@ -8553,7 +8634,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
mb->setJointVelMultiDof(i, vel);
}
}
bool hasVel = true;
for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
{
@ -8576,7 +8657,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
}
}
posVarCountIndex += mb->getLink(i).m_posVarCount;
uDofIndex += mb->getLink(i).m_dofCount;
}
@ -8947,7 +9028,7 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
{
#ifdef STATIC_LINK_SPD_PLUGIN
{
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ,
clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot);
if (rbdModel)
@ -8964,9 +9045,9 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
}
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
}
#endif
@ -9438,7 +9519,7 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
int numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
m_data->m_dynamicsWorld->removeMultiBody(bodyHandle->m_multiBody);
numCollisionObjects = m_data->m_dynamicsWorld->getNumCollisionObjects();
delete bodyHandle->m_multiBody;
bodyHandle->m_multiBody = 0;
serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
@ -10496,6 +10577,18 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId);
//int totalBytesPerVisualShape = sizeof (b3VisualShapeData);
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
//set serverCmd.m_sendVisualShapeArgs when totalNumVisualShapes is zero
if (totalNumVisualShapes==0) {
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = 0;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 0;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@ -10531,6 +10624,10 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
b3Warning("failed to get shape info");
}
}
}
return hasStatus;
}
@ -10849,7 +10946,7 @@ bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct Share
bool hasStatus = true;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_REMOVE_STATE_FAILED;
if (clientCmd.m_loadStateArguments.m_stateId >= 0)
{
if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
@ -11189,7 +11286,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -490,7 +490,9 @@ enum EnumLoadSoftBodyUpdateFlags
LOAD_SOFT_BODY_FILE_NAME = 1,
LOAD_SOFT_BODY_UPDATE_SCALE = 2,
LOAD_SOFT_BODY_UPDATE_MASS = 4,
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8,
LOAD_SOFT_BODY_INITIAL_POSITION = 16,
LOAD_SOFT_BODY_INITIAL_ORIENTATION = 32
};
enum EnumSimParamInternalSimFlags
@ -507,6 +509,8 @@ struct LoadSoftBodyArgs
double m_scale;
double m_mass;
double m_collisionMargin;
double m_initialPosition[3];
double m_initialOrientation[4];
};
struct b3LoadSoftBodyResultArgs

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -952,7 +952,7 @@ struct b3ForwardDynamicsAnalyticsIslandData
double m_remainingLeastSquaresResidual;
};
#define MAX_ISLANDS_ANALYTICS 1024
#define MAX_ISLANDS_ANALYTICS 64
struct b3ForwardDynamicsAnalyticsArgs
{

2
examples/SharedMemory/b3PluginManager.cpp
View File

@ -112,7 +112,7 @@ struct b3PluginManagerInternalData
b3BulletDefaultFileIO m_defaultFileIO;
b3PluginManagerInternalData()
: m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0), m_activeFileIOPluginUid(-1)
: m_physicsDirect(0), m_rpcCommandProcessorInterface(0), m_activeNotificationsBufferIndex(0), m_activeRendererPluginUid(-1), m_activeCollisionPluginUid(-1), m_numNotificationPlugins(0), m_activeFileIOPluginUid(-1)
{
}
};

10
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@ -1134,7 +1134,7 @@ void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string&
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, const struct b3RobotSimulatorLoadSoftBodyArgs& args)
{
if (!isConnected())
{
@ -1143,9 +1143,11 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
}
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSoftBodySetScale(command, scale);
b3LoadSoftBodySetMass(command, mass);
b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
b3LoadSoftBodySetScale(command, args.m_scale);
b3LoadSoftBodySetMass(command, args.m_mass);
b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

39
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@ -52,6 +52,43 @@ struct b3RobotSimulatorLoadSdfFileArgs
}
};
struct b3RobotSimulatorLoadSoftBodyArgs
{
btVector3 m_startPosition;
btQuaternion m_startOrientation;
double m_scale;
double m_mass;
double m_collisionMargin;
b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin)
{
}
b3RobotSimulatorLoadSoftBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
{
b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
}
b3RobotSimulatorLoadSoftBodyArgs()
{
b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
}
b3RobotSimulatorLoadSoftBodyArgs(double scale, double mass, double collisionMargin)
: m_startPosition(btVector3(0, 0, 0)),
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin)
{
}
};
struct b3RobotSimulatorLoadFileResults
{
btAlignedObjectArray<int> m_uniqueObjectIds;
@ -647,7 +684,7 @@ public:
int getConstraintUniqueId(int serialIndex);
void loadSoftBody(const std::string &fileName, double scale, double mass, double collisionMargin);
void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();

50
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

@ -1261,7 +1261,7 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomU = numDegreeOfFreedomU;
serverStatus.m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
for (int i = 0; i < numDegreeOfFreedomQ; i++)
{
serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQ[i] = stat->actualstateq(i);
@ -1274,7 +1274,7 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
{
serverStatus.m_sendActualStateArgs.m_rootLocalInertialFrame[i] = stat->rootlocalinertialframe(i);
}
for (int i = 0; i < numLinks * 6; i++)
for (int i = 0; i < numLinks * 7; i++)
{
serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i);
}
@ -1413,6 +1413,22 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
break;
}
case CMD_SDF_LOADING_COMPLETED:
{
converted = true;
const ::pybullet_grpc::SdfLoadedStatus* stat = &grpcReply.sdfstatus();
int numBodies = stat->bodyuniqueids_size();
if (numBodies > MAX_SDF_BODIES)
{
printf("SDF exceeds body capacity: %d > %d", numBodies, MAX_SDF_BODIES);
}
serverStatus.m_sdfLoadedArgs.m_numBodies = numBodies;
for (int i = 0; i < numBodies; i++)
{
serverStatus.m_sdfLoadedArgs.m_bodyUniqueIds[i] = stat->bodyuniqueids(i);
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
converted = true;
@ -1447,6 +1463,29 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
serverStatus.m_sendPixelDataArguments.m_startingPixelIndex = cam->startingpixelindex();
break;
}
case CMD_GET_DYNAMICS_INFO_COMPLETED:
{
converted = true;
const ::pybullet_grpc::GetDynamicsStatus* stat = &grpcReply.getdynamicsstatus();
serverStatus.m_dynamicsInfo.m_mass = stat->mass();
serverStatus.m_dynamicsInfo.m_lateralFrictionCoeff = stat->lateralfriction();
serverStatus.m_dynamicsInfo.m_spinningFrictionCoeff = stat->spinningfriction();
serverStatus.m_dynamicsInfo.m_rollingFrictionCoeff = stat->rollingfriction();
serverStatus.m_dynamicsInfo.m_restitution = stat->restitution();
serverStatus.m_dynamicsInfo.m_linearDamping = stat->lineardamping();
serverStatus.m_dynamicsInfo.m_angularDamping = stat->angulardamping();
serverStatus.m_dynamicsInfo.m_contactStiffness = stat->contactstiffness();
serverStatus.m_dynamicsInfo.m_contactDamping = stat->contactdamping();
serverStatus.m_dynamicsInfo.m_localInertialDiagonal[0] = stat->localinertiadiagonal().x();
serverStatus.m_dynamicsInfo.m_localInertialDiagonal[1] = stat->localinertiadiagonal().y();
serverStatus.m_dynamicsInfo.m_localInertialDiagonal[2] = stat->localinertiadiagonal().z();
serverStatus.m_dynamicsInfo.m_frictionAnchor = stat->frictionanchor();
serverStatus.m_dynamicsInfo.m_ccdSweptSphereRadius = stat->ccdsweptsphereradius();
serverStatus.m_dynamicsInfo.m_contactProcessingThreshold = stat->contactprocessingthreshold();
serverStatus.m_dynamicsInfo.m_activationState = stat->activationstate();
break;
}
default:
{
#endif //ALLOW_GRPC_STATUS_CONVERSION
@ -1658,7 +1697,10 @@ bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferSer
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
converted = true;
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
SharedMemoryStatus* status = (SharedMemoryStatus*)&serverStatus;
status->m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)status;
int bodyUniqueId;
int numLinks;
@ -1706,7 +1748,7 @@ bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferSer
{
stat->add_rootlocalinertialframe(rootLocalInertialFramePtr[i]);
}
for (int i = 0; i < numLinks * 6; i++)
for (int i = 0; i < numLinks * 7; i++)
{
stat->add_linklocalinertialframes(linkLocalInertialFrames[i]);
}

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

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

52
examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp
View File

@ -388,35 +388,37 @@ struct WrapperFileIO : public CommonFileIOInterface
int childHandle = childFileIO->fileOpen(fileName, mode);
if (childHandle>=0)
{
int fileSize = childFileIO->getFileSize(childHandle);
char* buffer = 0;
if (fileSize)
{
buffer = m_cachedFiles.allocateBuffer(fileSize);
if (buffer)
{
int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize);
if (readBytes!=fileSize)
{
if (readBytes<fileSize)
{
fileSize = readBytes;
} else
{
printf("WrapperFileIO error: reading more bytes (%d) then reported file size (%d) of file %s.\n", readBytes, fileSize, fileName);
}
}
} else
{
fileSize=0;
}
}
//potentially register a zero byte file, or files that only can be read partially
if (m_enableFileCaching)
{
int fileSize = childFileIO->getFileSize(childHandle);
char* buffer = 0;
if (fileSize)
{
buffer = m_cachedFiles.allocateBuffer(fileSize);
if (buffer)
{
int readBytes = childFileIO->fileRead(childHandle, buffer, fileSize);
if (readBytes!=fileSize)
{
if (readBytes<fileSize)
{
fileSize = readBytes;
} else
{
printf("WrapperFileIO error: reading more bytes (%d) then reported file size (%d) of file %s.\n", readBytes, fileSize, fileName);
}
}
} else
{
fileSize=0;
}
}
//potentially register a zero byte file, or files that only can be read partially
m_cachedFiles.registerFile(fileName, buffer, fileSize);
}
childFileIO->fileClose(childHandle);
break;
}

69
examples/SharedMemory/plugins/grpcPlugin/grpcPlugin.cpp
View File

@ -11,6 +11,9 @@
#include <stdio.h>
#include <mutex>
#include <thread>
#include <grpc++/grpc++.h>
#include <grpc/support/log.h>
#include "../../../Utils/b3Clock.h"
@ -44,6 +47,9 @@ public:
if (server_)
{
server_->Shutdown();
m_requestThreadCancelled = true;
m_requestThread->join();
delete m_requestThread;
// Always shutdown the completion queue after the server.
cq_->Shutdown();
server_ = 0;
@ -64,6 +70,10 @@ public:
server_ = m_builder.BuildAndStart();
std::cout << "grpcPlugin Bullet Physics GRPC server listening on " << hostNamePort << std::endl;
//Start the thread to gather the requests.
m_requestThreadCancelled = false;
m_requestThread = new std::thread(&ServerImpl::GatherRequests, this);
// Proceed to the server's main loop.
InitRpcs(comProc);
}
@ -72,25 +82,13 @@ public:
bool HandleSingleRpc()
{
CallData::CallStatus status = CallData::CallStatus::CREATE;
std::lock_guard<std::mutex> guard(m_queueMutex);
if (!m_requestQueue.empty()) {
void* tag = m_requestQueue.front();
m_requestQueue.pop_front();
status = static_cast<CallData*>(tag)->Proceed();
}
{
void* tag; // uniquely identifies a request.
bool ok;
// Block waiting to read the next event from the completion queue. The
// event is uniquely identified by its tag, which in this case is the
// memory address of a CallData instance.
// The return value of Next should always be checked. This return value
// tells us whether there is any kind of event or cq_ is shutting down.
grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
{
//GPR_ASSERT(cq_->Next(&tag, &ok));
GPR_ASSERT(ok);
status = static_cast<CallData*>(tag)->Proceed();
}
}
return status == CallData::CallStatus::TERMINATE;
}
@ -206,7 +204,9 @@ private:
{
GPR_ASSERT(status_ == FINISH);
// Once in the FINISH state, deallocate ourselves (CallData).
CallData::CallStatus tmpStatus = status_;
delete this;
return tmpStatus;
}
return status_;
}
@ -250,6 +250,39 @@ private:
std::unique_ptr<ServerCompletionQueue> cq_;
PyBulletAPI::AsyncService service_;
std::unique_ptr<Server> server_;
// Mutex to protect access to the request queue variables (m_requestQueue,
// m_requestThread, m_requestThreadCancelled).
std::mutex m_queueMutex;
// List of outstanding request tags.
std::list<void*> m_requestQueue;
// Whether or not the gathering thread is cancelled.
bool m_requestThreadCancelled;
// Thread to gather requests from the completion queue.
std::thread* m_requestThread;
void GatherRequests() {
void* tag; // uniquely identifies a request.
bool ok;
while(!m_requestThreadCancelled) {
// Block waiting to read the next event from the completion queue. The
// event is uniquely identified by its tag, which in this case is the
// memory address of a CallData instance.
// The return value of Next should always be checked. This return value
// tells us whether there is any kind of event or cq_ is shutting down.
grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
{
GPR_ASSERT(ok);
std::lock_guard<std::mutex> guard(m_queueMutex);
m_requestQueue.push_back(tag);
}
}
}
};
struct grpcMyClass

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

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

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

@ -5,73 +5,70 @@ import math
useGui = True
if (useGui):
p.connect(p.GUI)
p.connect(p.GUI)
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.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf",[3,3,1])
p.loadURDF("r2d2.urdf", [3, 3, 1])
rayFrom=[]
rayTo=[]
rayIds=[]
rayFrom = []
rayTo = []
rayIds = []
numRays = 1024
rayLen = 13
rayHitColor = [1,0,0]
rayMissColor = [0,1,0]
rayHitColor = [1, 0, 0]
rayMissColor = [0, 1, 0]
replaceLines = True
for i in range (numRays):
rayFrom.append([0,0,1])
rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1])
if (replaceLines):
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
else:
rayIds.append(-1)
for i in range(numRays):
rayFrom.append([0, 0, 1])
rayTo.append([
rayLen * math.sin(2. * math.pi * float(i) / numRays),
rayLen * math.cos(2. * math.pi * float(i) / numRays), 1
])
if (replaceLines):
rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor))
else:
rayIds.append(-1)
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
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):
p.stopStateLogging(timingLog)
p.stopStateLogging(timingLog)

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

@ -4,28 +4,26 @@ import os
import time
GRAVITY = -9.8
dt = 1e-3
iters=2000
iters = 2000
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0,0,GRAVITY)
p.setGravity(0, 0, GRAVITY)
p.setTimeStep(dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf",
cubeStartPos,
cubeStartOrientation)
#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)
cubeStartPos = [0, 0, 1.13]
cubeStartOrientation = p.getQuaternionFromEuler([0., 0, 0])
botId = p.loadURDF("biped/biped2d_pybullet.urdf", cubeStartPos, cubeStartOrientation)
#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.stepSimulation()
#import ipdb
@ -33,8 +31,8 @@ for joint in range (p.getNumJoints(botId)):
import time
p.setRealTimeSimulation(1)
while (1):
#p.stepSimulation()
#p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
p.setGravity(0,0,GRAVITY)
time.sleep(1/240.)
#p.stepSimulation()
#p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
p.setGravity(0, 0, GRAVITY)
time.sleep(1 / 240.)
time.sleep(1000)

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

@ -2,14 +2,14 @@ import pybullet as p
import time
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)
p.setGravity(0,0,-10)
timeStep = 1./240.
p.setGravity(0, 0, -10)
timeStep = 1. / 240.
p.setTimeStep(timeStep)
p.changeDynamics(cube2,-1,mass=1)
p.changeDynamics(cube2, -1, mass=1)
#now cube2 will have a floating base and move
while (p.isConnected()):
p.stepSimulation()
time.sleep(timeStep)
p.stepSimulation()
time.sleep(timeStep)

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

@ -1,43 +1,41 @@
import pybullet as p
import time
p.connect(p.GUI)
planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0])
planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1])
planeUidA = p.loadURDF("plane_transparent.urdf", [0, 0, 0])
planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, -1])
texUid = p.loadTexture("tex256.png")
p.changeVisualShape(planeUidA,-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(planeUidA, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, rgbaColor=[1, 1, 1, 0.5])
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
width = 256
height = 256
pixels = [255]*width*height*3
pixels = [255] * width * height * 3
colorR = 0
colorG = 0
colorB = 0
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
blue=0
blue = 0
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
for i in range (100000):
p.stepSimulation()
for i in range (width):
for j in range(height):
pixels[(i+j*width)*3+0]=i
pixels[(i+j*width)*3+1]=(j+blue)%256
pixels[(i+j*width)*3+2]=blue
blue=blue+1
p.changeTexture(texUid, pixels,width,height)
start = time.time()
p.getCameraImage(300,300,renderer=p.ER_BULLET_HARDWARE_OPENGL)
end = time.time()
print("rendering duraction")
print(end-start)
for i in range(100000):
p.stepSimulation()
for i in range(width):
for j in range(height):
pixels[(i + j * width) * 3 + 0] = i
pixels[(i + j * width) * 3 + 1] = (j + blue) % 256
pixels[(i + j * width) * 3 + 2] = blue
blue = blue + 1
p.changeTexture(texUid, pixels, width, height)
start = time.time()
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL)
end = time.time()
print("rendering duraction")
print(end - start)
p.stopStateLogging(logId)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,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
p.connect(p.GUI)
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
collisionFilterMask = 0
p.setCollisionFilterGroupMask(cubeId,-1,collisionFilterGroup,collisionFilterMask)
p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask)
enableCollision = 1
p.setCollisionFilterPair(planeId, cubeId,-1,-1,enableCollision )
p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision)
p.setRealTimeSimulation(1)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
while (p.isConnected()):
time.sleep(1./240.)
p.setGravity(0,0,-10)
time.sleep(1. / 240.)
p.setGravity(0, 0, -10)

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

@ -2,14 +2,13 @@ import pybullet as p
import time
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("r2d2.urdf",[0,0,1])
p.loadURDF("r2d2.urdf", [0, 0, 1])
p.stopStateLogging(logId)
p.resetSimulation();
logId = p.startStateLogging(p.STATE_REPLAY_ALL_COMMANDS,"commandLog.bin")
while(p.isConnected()):
time.sleep(1./240.)
p.resetSimulation()
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.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube_small.urdf",0,0,1)
p.setGravity(0,0,-10)
cubeId = p.loadURDF("cube_small.urdf", 0, 0, 1)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
print (cid)
print (p.getConstraintUniqueId(0))
prev=[0,0,1]
a=-math.pi
cid = p.createConstraint(cubeId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 1])
print(cid)
print(p.getConstraintUniqueId(0))
prev = [0, 0, 1]
a = -math.pi
while 1:
a=a+0.01
if (a>math.pi):
a=-math.pi
time.sleep(.01)
p.setGravity(0,0,-10)
pivot=[a,0,1]
orn = p.getQuaternionFromEuler([a,0,0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
a = a + 0.01
if (a > math.pi):
a = -math.pi
time.sleep(.01)
p.setGravity(0, 0, -10)
pivot = [a, 0, 1]
orn = p.getQuaternionFromEuler([a, 0, 0])
p.changeConstraint(cid, pivot, jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)

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

@ -1,34 +1,29 @@
import pybullet as p
p.connect(p.GUI)
useMaximalCoordinates = False
p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates )
p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("sphere2.urdf",[0,0,1])
p.loadURDF("cube.urdf",[0,0,1], useMaximalCoordinates=useMaximalCoordinates )
p.setGravity(0,3,-10)
while(1):
p.stepSimulation()
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]
p.loadURDF("cube.urdf", [0, 0, 1], useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 3, -10)
while (1):
p.stepSimulation()
pts = p.getContactPoints()
print("totalNormalForce=",totalNormalForce)
print("totalLateralFrictionForce=",totalLateralFrictionForce)
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("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.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh:
vertices=[ [-0.246350,-0.246483,-0.000624],
[-0.151407, -0.176325, 0.172867],
[ -0.246350, 0.249205, -0.000624],
[ -0.151407, 0.129477, 0.172867],
[ 0.249338, -0.246483, -0.000624],
[ 0.154395, -0.176325, 0.172867],
[ 0.249338, 0.249205, -0.000624],
[ 0.154395, 0.129477, 0.172867]
vertices = [[-0.246350, -0.246483, -0.000624], [-0.151407, -0.176325, 0.172867],
[-0.246350, 0.249205, -0.000624], [-0.151407, 0.129477, 0.172867],
[0.249338, -0.246483, -0.000624], [0.154395, -0.176325, 0.172867],
[0.249338, 0.249205, -0.000624], [0.154395, 0.129477, 0.172867]]
indices = [
0, 3, 2, 3, 6, 2, 7, 4, 6, 5, 0, 4, 6, 0, 2, 3, 5, 7, 0, 1, 3, 3, 7, 6, 7, 5, 4, 5, 1, 0, 6, 4,
0, 3, 1, 5
]
indices=[0,3,2,
3,6,2,
7,4,6,
5,0,4,
6,0,2,
3,5,7,
0,1,3,
3,7,6,
7,5,4,
5,1,0,
6,4,0,
3,1,5]
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH,vertices=vertices,indices=indices)
stoneId = p.createCollisionShape(p.GEOM_MESH, vertices=vertices, indices=indices)
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
segmentStart = 0
for i in range (segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
segmentStart=segmentStart-1
for i in range (segmentLength):
height = 0
if (i%2):
height=1
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height])
segmentStart=segmentStart-1
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
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):
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)
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range (segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
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
for i in range(segmentLength):
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):
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]
linkCollisionShapeIndices=[colBoxId]
linkVisualShapeIndices=[-1]
linkPositions=[[0,0,0]]
linkOrientations=[[0,0,0,1]]
linkInertialFramePositions=[[0,0,0]]
linkInertialFrameOrientations=[[0,0,0,1]]
indices=[0]
jointTypes=[p.JOINT_REVOLUTE]
axis=[[1,0,0]]
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
linkPositions = [[0, 0, 0]]
linkOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1]]
indices = [0]
jointTypes = [p.JOINT_REVOLUTE]
axis = [[1, 0, 0]]
baseOrientation = [0,0,0,1]
for i in range (segmentLength):
boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)
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
baseOrientation = [0, 0, 0, 1]
for i in range(segmentLength):
boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
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):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)

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

@ -3,145 +3,140 @@ import time
import math
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
if (cid < 0):
p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000")
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
p.setTimeStep(1./120.)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.setPhysicsEngineParameter(contactBreakingThreshold=0.04)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#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]
meshScale=[0.1,0.1,0.1]
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices=[
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,1.000000,1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
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)
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
collisionShapeId = p.createCollisionShape(
shapeType=p.GEOM_BOX, halfExtents=meshScale
) #MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
batchPositions = []
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])
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],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
bodyUid = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 0, 2],
batchPositions=batchPositions,
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.syncBodyInfo()
print("numBodies=",p.getNumBodies())
print("numBodies=", p.getNumBodies())
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
while (1):
p.stepSimulation()
#time.sleep(1./120.)
#p.getCameraImage(320,200)

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

@ -1,56 +1,73 @@
import pybullet as p
import time
p.connect(p.GUI)
p.createCollisionShape(p.GEOM_PLANE)
p.createMultiBody(0,0)
p.createMultiBody(0, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
link_Masses=[1]
linkCollisionShapeIndices=[colBoxId]
linkVisualShapeIndices=[-1]
linkPositions=[[0,0,0.11]]
linkOrientations=[[0,0,0,1]]
linkInertialFramePositions=[[0,0,0]]
linkInertialFrameOrientations=[[0,0,0,1]]
indices=[0]
jointTypes=[p.JOINT_REVOLUTE]
axis=[[0,0,1]]
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
linkPositions = [[0, 0, 0.11]]
linkOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1]]
indices = [0]
jointTypes = [p.JOINT_REVOLUTE]
axis = [[0, 0, 1]]
for i in range (3):
for j in range (3):
for k in range (3):
basePosition = [1+i*5*sphereRadius,1+j*5*sphereRadius,1+k*5*sphereRadius+1]
baseOrientation = [0,0,0,1]
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,basePosition,baseOrientation)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)
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)
for i in range(3):
for j in range(3):
for k in range(3):
basePosition = [
1 + i * 5 * sphereRadius, 1 + j * 5 * sphereRadius, 1 + k * 5 * sphereRadius + 1
]
baseOrientation = [0, 0, 0, 1]
if (k & 2):
sphereUid = p.createMultiBody(mass, colSphereId, visualShapeId, basePosition,
baseOrientation)
else:
sphereUid = p.createMultiBody(mass,
colBoxId,
visualShapeId,
basePosition,
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
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.getNumJoints(sphereUid)
for i in range (p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid,i)
while (1):
keys = p.getKeyboardEvents()
print(keys)
for i in range(p.getNumJoints(sphereUid)):
p.getJointInfo(sphereUid, i)
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.createMultiBody(0,0)
#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.resetDebugVisualizerCamera(15, -346, -16, [-15, 0, 1])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
#a few different ways to create a mesh:
#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
boxHalfWidth = 2.5
boxHalfHeight = 0.1
segmentLength = 5
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[boxHalfLength, boxHalfWidth, boxHalfHeight])
mass = 1
visualShapeId = -1
segmentStart = 0
for i in range (segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
segmentStart=segmentStart-1
for i in range (segmentLength):
height = 0
if (i%2):
height=1
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height])
segmentStart=segmentStart-1
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
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):
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)
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range (segmentLength):
p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
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
for i in range(segmentLength):
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):
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]
linkCollisionShapeIndices=[colBoxId]
linkVisualShapeIndices=[-1]
linkPositions=[[0,0,0]]
linkOrientations=[[0,0,0,1]]
linkInertialFramePositions=[[0,0,0]]
linkInertialFrameOrientations=[[0,0,0,1]]
indices=[0]
jointTypes=[p.JOINT_REVOLUTE]
axis=[[1,0,0]]
link_Masses = [1]
linkCollisionShapeIndices = [colBoxId]
linkVisualShapeIndices = [-1]
linkPositions = [[0, 0, 0]]
linkOrientations = [[0, 0, 0, 1]]
linkInertialFramePositions = [[0, 0, 0]]
linkInertialFrameOrientations = [[0, 0, 0, 1]]
indices = [0]
jointTypes = [p.JOINT_REVOLUTE]
axis = [[1, 0, 0]]
baseOrientation = [0,0,0,1]
for i in range (segmentLength):
boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)
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
baseOrientation = [0, 0, 0, 1]
for i in range(segmentLength):
boxId = p.createMultiBody(0,
colSphereId,
-1, [segmentStart, 0, -0.1],
baseOrientation,
linkMasses=link_Masses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=indices,
linkJointTypes=jointTypes,
linkJointAxis=axis)
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):
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)
camData = p.getDebugVisualizerCamera()
viewMat = camData[2]
projMat = camData[3]
p.getCameraImage(256,
256,
viewMatrix=viewMat,
projectionMatrix=projMat,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
keys = p.getKeyboardEvents()
p.stepSimulation()
#print(keys)
time.sleep(0.01)

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

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

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

@ -2,167 +2,180 @@ import pybullet as p
import time
import math
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera()
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
farPlane = 10000
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])]
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]))
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]]
rayFrom = camPos
oneOverWidth = float(1)/float(width)
oneOverHeight = float(1)/float(height)
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]]
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
return rayFrom,rayTo
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])]
invLen = farPlane * 1. / (math.sqrt(rayForward[0] * rayForward[0] + rayForward[1] *
rayForward[1] + rayForward[2] * rayForward[2]))
rayForward = [invLen * rayForward[0], invLen * rayForward[1], invLen * rayForward[2]]
rayFrom = camPos
oneOverWidth = float(1) / float(width)
oneOverHeight = float(1) / float(height)
dHor = [horizon[0] * oneOverWidth, horizon[1] * oneOverWidth, horizon[2] * oneOverWidth]
dVer = [vertical[0] * oneOverHeight, vertical[1] * oneOverHeight, vertical[2] * oneOverHeight]
rayToCenter = [
rayFrom[0] + rayForward[0], rayFrom[1] + rayForward[1], rayFrom[2] + rayForward[2]
]
rayTo = [
rayFrom[0] + rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0] + float(mouseX) * dHor[0] -
float(mouseY) * dVer[0], rayFrom[1] + rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1] +
float(mouseX) * dHor[1] - float(mouseY) * dVer[1], rayFrom[2] + rayForward[2] -
0.5 * horizon[2] + 0.5 * vertical[2] + float(mouseX) * dHor[2] - float(mouseY) * dVer[2]
]
return rayFrom, rayTo
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
if (cid < 0):
p.connect(p.GUI)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setTimeStep(1./120.)
p.setTimeStep(1. / 120.)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
#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]
meshScale=[0.1,0.1,0.1]
shift = [0, -0.02, 0]
meshScale = [0.1, 0.1, 0.1]
vertices=[
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,1.000000,1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[1.000000,1.000000,-1.000000],
[1.000000,1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[-1.000000,-1.000000,-1.000000],
[-1.000000,-1.000000,1.000000],
[1.000000,-1.000000,1.000000],
[1.000000,-1.000000,-1.000000],
[-1.000000,1.000000,-1.000000],
[-1.000000,1.000000,1.000000],
[1.000000,1.000000,1.000000],
[1.000000,1.000000,-1.000000]
vertices = [[-1.000000, -1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [-1.000000, 1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [1.000000, -1.000000, -1.000000],
[1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, 1.000000, -1.000000],
[-1.000000, 1.000000, 1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, -1.000000], [1.000000, 1.000000, -1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, -1.000000, 1.000000],
[-1.000000, -1.000000, -1.000000], [-1.000000, -1.000000, 1.000000],
[1.000000, -1.000000, 1.000000], [1.000000, -1.000000, -1.000000],
[-1.000000, 1.000000, -1.000000], [-1.000000, 1.000000, 1.000000],
[1.000000, 1.000000, 1.000000], [1.000000, 1.000000, -1.000000]]
normals = [[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 1.000000], [0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[0.000000, 0.000000, -1.000000], [0.000000, 0.000000, -1.000000],
[-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[-1.000000, 0.000000, 0.000000], [-1.000000, 0.000000, 0.000000],
[1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[1.000000, 0.000000, 0.000000], [1.000000, 0.000000, 0.000000],
[0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[0.000000, -1.000000, 0.000000], [0.000000, -1.000000, 0.000000],
[0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000],
[0.000000, 1.000000, 0.000000], [0.000000, 1.000000, 0.000000]]
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)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
rgbaColor=[1, 1, 1, 1],
specularColor=[0.4, .4, 0],
visualFramePosition=shift,
meshScale=meshScale,
vertices=vertices,
indices=indices,
uvs=uvs,
normals=normals)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
vertices=vertices,
collisionFramePosition=shift,
meshScale=meshScale)
texUid = p.loadTexture("tex256.png")
rangex = 1
rangey = 1
for i in range (rangex):
for j in range (rangey ):
bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*2,1], useMaximalCoordinates=True)
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for i in range(rangex):
for j in range(rangey):
bodyUid = p.createMultiBody(baseMass=1,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[((-rangex / 2) + i) * meshScale[0] * 2,
(-rangey / 2 + j) * meshScale[1] * 2, 1],
useMaximalCoordinates=True)
p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.stopStateLogging(logId)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
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
while (1):
p.getCameraImage(320,200)
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom,rayTo=getRayFromTo(mouseX,mouseY)
rayInfo = p.rayTest(rayFrom,rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid>=1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
currentColor+=1
if (currentColor>=len(colors)):
currentColor=0
p.getCameraImage(320, 200)
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
if (objectUid >= 1):
#p.removeBody(objectUid)
p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0

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

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

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

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

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

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

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

@ -7,7 +7,8 @@ import os, fnmatch
import argparse
from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
print('Opened'),
@ -44,19 +45,19 @@ def readLogFile(filename, verbose = True):
if verbose:
print("num chunks:")
print(len(chunks))
for chunk in chunks:
print("len(chunk)=",len(chunk)," sz = ", sz)
print("len(chunk)=", len(chunk), " sz = ", sz)
if len(chunk) == sz:
print("chunk #",chunkIndex)
chunkIndex=chunkIndex+1
print("chunk #", chunkIndex)
chunkIndex = chunkIndex + 1
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
if verbose:
print(" ",keys[i],"=",values[i])
print(" ", keys[i], "=", values[i])
log.append(record)
else:
print("Error, expected aabb terminal")
@ -65,16 +66,16 @@ def readLogFile(filename, verbose = True):
numArgs = len(sys.argv)
print ('Number of arguments:', numArgs, 'arguments.')
print ('Argument List:', str(sys.argv))
print('Number of arguments:', numArgs, 'arguments.')
print('Argument List:', str(sys.argv))
fileName = "log.bin"
if (numArgs>1):
if (numArgs > 1):
fileName = sys.argv[1]
print("filename=")
print(fileName)
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
from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
print('Opened'),
@ -37,20 +38,20 @@ def readLogFile(filename, verbose = True):
chunks = wholeFile.split(b'\xaa\xbb')
log = list()
if verbose:
print("num chunks:")
print(len(chunks))
print("num chunks:")
print(len(chunks))
chunkIndex = 0
for chunk in chunks:
print("len(chunk)=",len(chunk)," sz = ", sz)
print("len(chunk)=", len(chunk), " sz = ", sz)
if len(chunk) == sz:
print("chunk #",chunkIndex)
chunkIndex=chunkIndex+1
print("chunk #", chunkIndex)
chunkIndex = chunkIndex + 1
values = struct.unpack(fmt, chunk)
record = list()
for i in range(ncols):
record.append(values[i])
if verbose:
print(" ",keys[i],"=",values[i])
print(" ", keys[i], "=", values[i])
log.append(record)
@ -59,19 +60,19 @@ def readLogFile(filename, verbose = True):
numArgs = len(sys.argv)
print ('Number of arguments:', numArgs, 'arguments.')
print ('Argument List:', str(sys.argv))
print('Number of arguments:', numArgs, 'arguments.')
print('Argument List:', str(sys.argv))
fileName = "data/example_log_vr.bin"
if (numArgs>1):
fileName = sys.argv[1]
if (numArgs > 1):
fileName = sys.argv[1]
print("filename=")
print(fileName)
verbose = True
log = readLogFile(fileName,verbose)
log = readLogFile(fileName, verbose)
# the index of the first integer in the vr log file for packed buttons
firstPackedButtonIndex = 13
@ -83,26 +84,27 @@ numPackedButtons = 7
buttonMask = 7
for record in log:
# indices of buttons that are down
buttonDownIndices = []
# indices of buttons that are triggered
buttonTriggeredIndices = []
# indices of buttons that are released
buttonReleasedIndices = []
buttonIndex = 0
for packedButtonIndex in range(firstPackedButtonIndex, firstPackedButtonIndex+numPackedButtons):
for packButtonShift in range(numGroupedButtons):
buttonEvent = buttonMask & record[packedButtonIndex]
if buttonEvent & 1:
buttonDownIndices.append(buttonIndex)
elif buttonEvent & 2:
buttonTriggeredIndices.append(buttonIndex)
elif buttonEvent & 4:
buttonReleasedIndices.append(buttonIndex)
record[packedButtonIndex] = record[packedButtonIndex] >> 3
buttonIndex += 1
if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices):
print ('timestamp: ', record[1])
print ('button is down: ', buttonDownIndices)
print ('button is triggered: ', buttonTriggeredIndices)
print ('button is released: ', buttonReleasedIndices)
# indices of buttons that are down
buttonDownIndices = []
# indices of buttons that are triggered
buttonTriggeredIndices = []
# indices of buttons that are released
buttonReleasedIndices = []
buttonIndex = 0
for packedButtonIndex in range(firstPackedButtonIndex,
firstPackedButtonIndex + numPackedButtons):
for packButtonShift in range(numGroupedButtons):
buttonEvent = buttonMask & record[packedButtonIndex]
if buttonEvent & 1:
buttonDownIndices.append(buttonIndex)
elif buttonEvent & 2:
buttonTriggeredIndices.append(buttonIndex)
elif buttonEvent & 4:
buttonReleasedIndices.append(buttonIndex)
record[packedButtonIndex] = record[packedButtonIndex] >> 3
buttonIndex += 1
if len(buttonDownIndices) or len(buttonTriggeredIndices) or len(buttonReleasedIndices):
print('timestamp: ', record[1])
print('button is down: ', buttonDownIndices)
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 time
import pkgutil
@ -7,41 +6,48 @@ egl = pkgutil.get_loader('eglRenderer')
p.connect(p.DIRECT)
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_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.setGravity(0, 0, -10)
p.loadURDF("plane.urdf", [0, 0, -1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camTargetPos = [0, 0, 0]
camDistance = 4
pitch = -10.0
roll=0
roll = 0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
stop = time.time()
print ("stepSimulation %f" % (stop - start))
for yaw in range(0, 360, 10):
start = time.time()
p.stepSimulation()
stop = time.time()
print("stepSimulation %f" % (stop - start))
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
projectionMatrix = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0]
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)
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll,
upAxisIndex)
projectionMatrix = [
1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0,
-1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0
]
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)

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

@ -4,48 +4,50 @@ import time
p.connect(p.GUI)
p.setPhysicsEngineParameter(allowedCcdPenetration=0.0)
terrain_mass = 0
terrain_visual_shape_id = -1
terrain_position = [0, 0, 0]
terrain_orientation = [0, 0, 0, 1]
terrain_collision_shape_id = p.createCollisionShape(shapeType=p.GEOM_MESH,
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
sphereRadius = 0.005
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
colSphereId = p.createCollisionShape(p.GEOM_SPHERE, radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[sphereRadius, sphereRadius, sphereRadius])
mass = 1
visualShapeId = -1
for i in range(5):
for j in range(5):
for k in range(5):
#if (k&2):
sphereUid = p.createMultiBody(
mass,
colSphereId,
visualShapeId, [-i * 5 * sphereRadius, j * 5 * sphereRadius, k * 2 * sphereRadius + 1],
useMaximalCoordinates=useMaximalCoordinates)
#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):
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)
p.setGravity(0, 0, -10)
pts = p.getContactPoints()
print("num points=",len(pts))
print("num points=", len(pts))
print(pts)
while (p.isConnected()):
time.sleep(1./240.)
p.stepSimulation()
time.sleep(1. / 240.)
p.stepSimulation()

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

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

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

@ -3,18 +3,17 @@ import time
p.connect(p.GUI)
fileIO = p.loadPlugin("fileIOPlugin")
if (fileIO>=0):
p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
objs= p.loadSDF("pickup/model.sdf")
dobot =objs[0]
p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1])
else:
print("fileIOPlugin is disabled.")
if (fileIO >= 0):
p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO])
objs = p.loadSDF("pickup/model.sdf")
dobot = objs[0]
p.changeVisualShape(dobot, -1, rgbaColor=[1, 1, 1, 1])
else:
print("fileIOPlugin is disabled.")
p.setPhysicsEngineParameter(enableFileCaching=False)
while (1):
p.stepSimulation()
time.sleep(1./240.)
p.stepSimulation()
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
#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()
print("joint state without sensor")
print(p.getJointState(0,0))
p.enableJointForceTorqueSensor(hinge,hingeJointIndex)
print(p.getJointState(0, 0))
p.enableJointForceTorqueSensor(hinge, hingeJointIndex)
p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0,0))
p.setGravity(0,0,0)
print(p.getJointState(0, 0))
p.setGravity(0, 0, 0)
p.stepSimulation()
print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0,0))
print(p.getJointState(0, 0))
p.disconnect()

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

@ -5,42 +5,42 @@ import math
p.connect(p.GUI)
useMaximalCoordinates = False
p.setGravity(0,0,-10)
plane=p.loadURDF("plane.urdf",[0,0,-1],useMaximalCoordinates=useMaximalCoordinates)
p.setGravity(0, 0, -10)
plane = p.loadURDF("plane.urdf", [0, 0, -1], useMaximalCoordinates=useMaximalCoordinates)
p.setRealTimeSimulation(0)
velocity = 1
num = 40
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_TINY_RENDERER,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_TINY_RENDERER, 0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (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])
for i in range(num):
print("progress:", i, num)
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
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])
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):
time.sleep(0.01)
time.sleep(0.01)

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

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

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

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

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

@ -2,51 +2,68 @@ import pybullet as p
import time
p.connect(p.GUI)
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)
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]
basePositionB = [1.5,0,1]
obA=-1
obB=-1
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]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1])
obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB )
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth=3
colorRGB=[1,0,0]
lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0)
pitch=0
yaw=0
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch>=3.1415*2.):
pitch=0
yaw+= 0.01
if (yaw>=3.1415*2.):
yaw=0
baseOrientationB = p.getQuaternionFromEuler([yaw,pitch,0])
if (obB>=0):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
pitch += 0.01
if (pitch >= 3.1415 * 2.):
pitch = 0
yaw += 0.01
if (yaw >= 3.1415 * 2.):
yaw = 0
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
if (obB >= 0):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
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.)
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1,
bodyB=-1,
distance=100,
collisionShapeA=geom,
collisionShapeB=geomBox,
collisionShapePositionA=[0.5, 0, 1],
collisionShapePositionB=basePositionB,
collisionShapeOrientationB=baseOrientationB)
#pts = p.getClosestPoints(bodyA=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:
#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]
print(curTexUid)
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):
p.getCameraImage(320,200)
p.changeVisualShape(plane,-1,textureUniqueId=curTexUid)
for i in range (100):
p.getCameraImage(320,200)
for i in range(100):
p.getCameraImage(320, 200)
p.changeVisualShape(plane, -1, textureUniqueId=curTexUid)
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
usePort = True
if (usePort):
id = p.connect(p.GRPC,"localhost:12345")
id = p.connect(p.GRPC, "localhost:12345")
else:
id = p.connect(p.GRPC,"localhost")
print("id=",id)
id = p.connect(p.GRPC, "localhost")
print("id=", id)
if (id<0):
print("Cannot connect to GRPC server")
exit(0)
print ("Connected to GRPC")
if (id < 0):
print("Cannot connect to GRPC server")
exit(0)
print("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf")
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")
#start the GRPC server at hostname, port
if (id<0):
print("Cannot load grpcPlugin")
exit(0)
if (id < 0):
print("Cannot load grpcPlugin")
exit(0)
if usePort:
p.executePluginCommand(id, "localhost:12345")
p.executePluginCommand(id, "localhost:12345")
else:
p.executePluginCommand(id, "localhost")
p.executePluginCommand(id, "localhost")
while p.isConnected():
if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC])
else:
dt = 1./240.
time.sleep(dt)
if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC])
else:
dt = 1. / 240.
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
c = p.connect(p.SHARED_MEMORY)
print(c)
if (c<0):
p.connect(p.GUI)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/MPL.xml")
hand=objects[0]
hand = objects[0]
#clamp in range 400-600
#minV = 400
#maxV = 600
minVarray = [275,280,350,290]
maxVarray = [450,550,500,400]
minVarray = [275, 280, 350, 290]
maxVarray = [450, 550, 500, 400]
pinkId = 0
middleId = 1
@ -32,82 +32,93 @@ thumbId = 3
p.setRealTimeSimulation(1)
def getSerialOrNone(portname):
try:
return serial.Serial(port=portname,baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
except:
return None
try:
return serial.Serial(port=portname,
baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
except:
return None
def convertSensor(x, fingerIndex):
minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex]
v = minV
try:
v = float(x)
except ValueError:
v = minV
if (v<minV):
v=minV
if (v>maxV):
v=maxV
b = (v-minV)/float(maxV-minV)
return (b)
minV = minVarray[fingerIndex]
maxV = maxVarray[fingerIndex]
v = minV
try:
v = float(x)
except ValueError:
v = minV
if (v < minV):
v = minV
if (v > maxV):
v = maxV
b = (v - minV) / float(maxV - minV)
return (b)
ser = None
portindex = 0
while (ser is None and portindex < 30):
portname = 'COM'+str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is None):
portname = "/dev/cu.usbmodem14"+str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is not None):
print("COnnected!")
portindex = portindex+1
portname = 'COM' + str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is None):
portname = "/dev/cu.usbmodem14" + str(portindex)
print(portname)
ser = getSerialOrNone(portname)
if (ser is not None):
print("COnnected!")
portindex = portindex + 1
if (ser is None):
ser = serial.Serial(port = "/dev/cu.usbmodem1421",baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
pi=3.141592
ser = serial.Serial(port="/dev/cu.usbmodem1421",
baudrate=115200,
parity=serial.PARITY_ODD,
stopbits=serial.STOPBITS_TWO,
bytesize=serial.SEVENBITS)
pi = 3.141592
if (ser is not None and ser.isOpen()):
while True:
while ser.inWaiting() > 0:
line = str(ser.readline())
words = line.split(",")
if (len(words)==6):
pink = convertSensor(words[1],pinkId)
middle = convertSensor(words[2],middleId)
index = convertSensor(words[3],indexId)
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)
while True:
while ser.inWaiting() > 0:
line = str(ser.readline())
words = line.split(",")
if (len(words) == 6):
pink = convertSensor(words[1], pinkId)
middle = convertSensor(words[2], middleId)
index = convertSensor(words[3], indexId)
thumb = convertSensor(words[4], thumbId)
p.setJointMotorControl2(hand,24,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,26,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,28,p.POSITION_CONTROL,middle)
p.setJointMotorControl2(hand,40,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,42,p.POSITION_CONTROL,pink)
p.setJointMotorControl2(hand,44,p.POSITION_CONTROL,pink)
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)
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)
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, 26, p.POSITION_CONTROL, middle)
p.setJointMotorControl2(hand, 28, p.POSITION_CONTROL, middle)
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:
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)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0,0,-10)
sleep(0.01) # Time in seconds.
else:
p.stepSimulation()
if (useRealTimeSimulation):
p.setGravity(0, 0, -10)
sleep(0.01) # Time in seconds.
else:
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 time
useGUI = True
if useGUI:
p.connect(p.GUI)
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
p.connect(p.DIRECT)
useZUp = False
useYUp = not useZUp
showJointMotorTorques = False
showJointMotorTorques = False
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 pdControllerStable import PDControllerStableMultiDof
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerStable import PDControllerStableMultiDof
explicitPD = PDControllerExplicitMultiDof(p)
stablePD = PDControllerStableMultiDof(p)
p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
p.resetDebugVisualizerCamera(cameraDistance=7,
cameraYaw=-94,
cameraPitch=-14,
cameraTargetPosition=[0.31, 0.03, -1.16])
import pybullet_data
p.setTimeOut(10000)
useMotionCapture=False
useMotionCaptureReset=False#not useMotionCapture
useExplicitPD = True
useMotionCapture = False
useMotionCaptureReset = False #not useMotionCapture
useExplicitPD = True
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(numSolverIterations=30)
#p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
#explicit PD control requires small timestep
timeStep = 1./600.
timeStep = 1. / 600.
#timeStep = 1./240.
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_walk.txt"
#p.loadURDF("plane.urdf",[0,0,-1.03])
print("path = ", path)
with open(path, 'r') as f:
motion_dict = json.load(f)
with open(path, 'r') as f:
motion_dict = json.load(f)
#print("motion_dict = ", motion_dict)
print("len motion=", len(motion_dict))
print(motion_dict['Loop'])
numFrames = len(motion_dict['Frames'])
print("#frames = ", numFrames)
numFrames = len(motion_dict['Frames'])
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)
forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]]
p.addUserDebugText("Stable PD",
[startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]],
[0, 0, 0])
p.addUserDebugText("Spherical Drive",
[startLocations[1][0], startLocations[1][1] + 1, startLocations[1][2]],
[0, 0, 0])
p.addUserDebugText("Explicit PD",
[startLocations[2][0], startLocations[2][1] + 1, startLocations[2][2]],
[0, 0, 0])
p.addUserDebugText("Kinematic",
[startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]],
[0, 0, 0])
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",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
startPose = [
2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229,
0.009485003066, -0.04756001538, -0.004475001447, 1, 0, 0, 0, 0.9649395871, 0.02436898957,
-0.05755497537, 0.2549218909, -0.249116, 0.9993661511, 0.009952001505, 0.03265400494,
0.01009800153, 0.9854981188, -0.06440700776, 0.09324301124, -0.1262970152, 0.170571,
0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699, -0.391532, 0.9828788495,
0.1013909845, -0.05515999155, 0.143618978, 0.9659421276, 0.1884590249, -0.1422460188,
0.105854014, 0.581348
]
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])
startVel = [
1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799, -0.1050802848, 0,
0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0, 1.114409628, 0.3618553952,
-0.4505575061, 0, -1.725374735, -0.5052852598, -0.8555179722, -0.2221173515, 0, -0.1837617357,
0.00171895706, 0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0,
-4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535, 0.1313680236,
-0.7236998534, 0, -0.5278312973
]
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)
p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
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])
index0 = 7
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition = [
startPose[index0 + 1], startPose[index0 + 2], startPose[index0 + 3], startPose[index0 + 0]
]
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,
1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153,
0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495,
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
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)
startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952,
-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
#print(ji)
print("joint[", j, "].type=", jointTypes[ji[2]])
print("joint[", j, "].name=", ji[1])
p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
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)
index0=7
for j in range (p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid,j)
targetPosition=[0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
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])
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
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
#rightElbow=4
@ -191,249 +242,320 @@ leftElbow=13
import time
kpOrg = [
0, 0, 0, 0, 0, 0, 0, 1000, 1000, 1000, 1000, 100, 100, 100, 100, 500, 500, 500, 500, 500, 400,
400, 400, 400, 400, 400, 400, 400, 300, 500, 500, 500, 500, 500, 400, 400, 400, 400, 400, 400,
400, 400, 300
]
kdOrg = [
0, 0, 0, 0, 0, 0, 0, 100, 100, 100, 100, 10, 10, 10, 10, 50, 50, 50, 50, 50, 40, 40, 40, 40,
40, 40, 40, 40, 30, 50, 50, 50, 50, 50, 40, 40, 40, 40, 40, 40, 40, 40, 30
]
kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
once = True
p.getCameraImage(320, 200)
once=True
p.getCameraImage(320,200)
while (p.isConnected()):
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:
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
#getQuaternionSlerp
kp=kpMotor
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)
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
#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]])
#print("duration=",frameData[0])
#print(pos=[frameData])
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)
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
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 time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
p.setPhysicsEngineParameter(numSubSteps=1)
p.loadURDF("plane.urdf")
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
[0.710965, 0.218117, 0.519402, -0.420923])
jointPositions = [
-0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000,
-0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000,
0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000,
-0.007374, 0.000000
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
@ -26,11 +31,10 @@ p.setRealTimeSimulation(0)
print("Starting benchmark")
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):
p.stepSimulation()
p.stepSimulation()
p.stopStateLogging(logId)
print("ended benchmark")
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
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
cid = p.connect(p.GUI)
if (cid < 0):
cid = p.connect(p.GUI)
p.resetSimulation()
@ -11,36 +11,41 @@ useRealTime = 0
p.setRealTimeSimulation(useRealTime)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
p.loadSDF("stadium.sdf")
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
human = obUids[0]
for i in range(p.getNumJoints(human)):
p.setJointMotorControl2(human, i, p.POSITION_CONTROL, targetPosition=0, force=500)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle", -4, 4, -1)
maxForceId = p.addUserDebugParameter("maxForce", 0, 500, 10)
for i in range (p.getNumJoints(human)):
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=500)
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL", -4, 4, -1)
maxForceLeftId = p.addUserDebugParameter("maxForceL", 0, 500, 10)
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
maxForceId = p.addUserDebugParameter("maxForce",0,500,10)
kneeAngleTargetLeftId = p.addUserDebugParameter("kneeAngleL",-4,4,-1)
maxForceLeftId = p.addUserDebugParameter("maxForceL",0,500,10)
kneeJointIndex=11
kneeJointIndexLeft=18
kneeJointIndex = 11
kneeJointIndexLeft = 18
while (1):
time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce)
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
p.setJointMotorControl2(human,kneeJointIndexLeft,p.POSITION_CONTROL,targetPosition=kneeAngleTargetLeft,force=maxForceLeft)
time.sleep(0.01)
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(human,
kneeJointIndex,
p.POSITION_CONTROL,
targetPosition=kneeAngleTarget,
force=maxForce)
kneeAngleTargetLeft = p.readUserDebugParameter(kneeAngleTargetLeftId)
maxForceLeft = p.readUserDebugParameter(maxForceLeftId)
p.setJointMotorControl2(human,
kneeJointIndexLeft,
p.POSITION_CONTROL,
targetPosition=kneeAngleTargetLeft,
force=maxForceLeft)
if (useRealTime==0):
p.stepSimulation()
if (useRealTime == 0):
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")
humanoid = obUids[1]
gravId = p.addUserDebugParameter("gravity",-10,10,-10)
jointIds=[]
paramIds=[]
gravId = p.addUserDebugParameter("gravity", -10, 10, -10)
jointIds = []
paramIds = []
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)):
p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
for j in range(p.getNumJoints(humanoid)):
p.changeDynamics(humanoid, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid, j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4, 0))
p.setRealTimeSimulation(1)
while(1):
p.setGravity(0,0,p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
time.sleep(0.01)
while (1):
p.setGravity(0, 0, p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
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
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#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]
p.setGravity(0,0,0)
p.setGravity(0, 0, 0)
while 1:
p.stepSimulation()
for i in range (1):
pos = [0,0,1.26]
orn = p.getQuaternionFromEuler([0,0,3.14])
p.stepSimulation()
for i in range(1):
pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
for i in range (numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1)
sleep(0.05)
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
velocityGain=1)
sleep(0.05)

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

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

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

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

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

@ -3,7 +3,7 @@ plot = True
import time
if (plot):
import matplotlib.pyplot as plt
import matplotlib.pyplot as plt
import math
verbose = False
@ -19,137 +19,146 @@ bullet.setTimeStep(delta_t)
# Switch between URDF with/without FIXED joints
with_fixed_joints = True
if with_fixed_joints:
id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
id_revolute_joints = [0, 3]
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
robot_base,
robot_orientation,
useFixedBase=True)
else:
id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
robot_base, robot_orientation, useFixedBase=True)
id_revolute_joints = [0, 1]
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
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)
bullet.changeDynamics(id_robot,0,linearDamping=0, angularDamping=0)
bullet.changeDynamics(id_robot,1,linearDamping=0, angularDamping=0)
jointTypeNames = [
"JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED",
"JOINT_POINT2POINT", "JOINT_GEAR"
]
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"]
# Disable the motors for torque control:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0])
bullet.setJointMotorControlArray(id_robot,
id_revolute_joints,
bullet.VELOCITY_CONTROL,
forces=[0.0, 0.0])
# Target Positions:
start = 0.0
end = 1.0
steps = int((end-start)/delta_t)
t = [0]*steps
q_pos_desired = [[0.]* steps,[0.]* steps]
q_vel_desired = [[0.]* steps,[0.]* steps]
q_acc_desired = [[0.]* steps,[0.]* steps]
steps = int((end - start) / delta_t)
t = [0] * steps
q_pos_desired = [[0.] * steps, [0.] * steps]
q_vel_desired = [[0.] * steps, [0.] * steps]
q_acc_desired = [[0.] * steps, [0.] * steps]
for s in range(steps):
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[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])
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[1][s] = -1. / (2. * math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
q_pos = [[0.]* steps,[0.]* steps]
q_vel = [[0.]* steps,[0.]* steps]
q_tor = [[0.]* steps,[0.]* steps]
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 = [[0.] * steps, [0.] * steps]
q_tor = [[0.] * steps, [0.] * steps]
# Do Torque Control:
for i in range(len(t)):
# Read Sensor States:
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]
# Read Sensor States:
joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
# Computing the torque from inverse dynamics:
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]]
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
if (verbose):
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)
q_vel[0][i] = joint_states[0][1]
q_vel[1][i] = joint_states[1][1]
# Set the Joint Torques:
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]])
# Computing the torque from inverse dynamics:
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
bullet.stepSimulation()
if (verbose):
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:
if plot:
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)
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()
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)
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):
bullet.stepSimulation()
time.sleep(0.01)
bullet.stepSimulation()
time.sleep(0.01)

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

@ -4,35 +4,34 @@ import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
if (numJoints != 7):
exit()
#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
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
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
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
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):
p.resetJointState(kukaId,i,rp[i])
for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
useNullSpace = 1
@ -46,46 +45,73 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
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)
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
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)
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
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
print(p.getJointInfo(husky,i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
husky = p.loadURDF("husky/husky.urdf", [0.290388, 0.329902, -0.310270],
[0.002328, -0.000984, 0.996491, 0.083659])
for i in range(p.getNumJoints(husky)):
print(p.getJointInfo(husky, i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749, 0.345564, 0.120208, 0.002327,
-0.000988, 0.996491, 0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
jointPositions = [3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
cid = p.createConstraint(husky, -1, kukaId, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0., 0., -.5],
[0, 0, 0, 1])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
baseorn = p.getQuaternionFromEuler([3.1415, 0, 0.3])
baseorn = [0, 0, 0, 1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
if (numJoints != 7):
exit()
#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
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
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
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
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):
p.resetJointState(kukaId,i,rp[i])
for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,-10)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
p.setGravity(0, 0, -10)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 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.
#This can be used to test the IK result accuracy.
useSimulation = 0
@ -64,106 +64,130 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 15
basepos =[0,0,0]
basepos = [0, 0, 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')
#(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')
#(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]
wheelDeltasTurn=[1,-1,1,-1]
wheelDeltasFwd=[1,1,1,1]
wheelVelocities = [0, 0, 0, 0]
wheelDeltasTurn = [1, -1, 1, -1]
wheelDeltasFwd = [1, 1, 1, 1]
while 1:
keys = p.getKeyboardEvents()
shift = 0.01
wheelVelocities=[0,0,0,0]
speed = 1.0
for k in keys:
if ord('s') in keys:
p.saveWorld("state.py")
if ord('a') in keys:
basepos = basepos=[basepos[0],basepos[1]-shift,basepos[2]]
if ord('d') in keys:
basepos = basepos=[basepos[0],basepos[1]+shift,basepos[2]]
keys = p.getKeyboardEvents()
shift = 0.01
wheelVelocities = [0, 0, 0, 0]
speed = 1.0
for k in keys:
if ord('s') in keys:
p.saveWorld("state.py")
if ord('a') in keys:
basepos = basepos = [basepos[0], basepos[1] - shift, basepos[2]]
if ord('d') in keys:
basepos = basepos = [basepos[0], basepos[1] + shift, basepos[2]]
if p.B3G_LEFT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] - speed*wheelDeltasTurn[i]
if p.B3G_RIGHT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] +speed*wheelDeltasTurn[i]
if p.B3G_UP_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] + speed*wheelDeltasFwd[i]
if p.B3G_DOWN_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] -speed*wheelDeltasFwd[i]
if p.B3G_LEFT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasTurn[i]
if p.B3G_RIGHT_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasTurn[i]
if p.B3G_UP_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] + speed * wheelDeltasFwd[i]
if p.B3G_DOWN_ARROW in keys:
for i in range(len(wheels)):
wheelVelocities[i] = wheelVelocities[i] - speed * wheelDeltasFwd[i]
baseorn = p.getQuaternionFromEuler([0,0,ang])
for i in range(len(wheels)):
p.setJointMotorControl2(husky,wheels[i],p.VELOCITY_CONTROL,targetVelocity=wheelVelocities[i], force=1000)
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
if (useRealTimeSimulation):
t = time.time()#(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
#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)]
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])
baseorn = p.getQuaternionFromEuler([0, 0, ang])
for i in range(len(wheels)):
p.setJointMotorControl2(husky,
wheels[i],
p.VELOCITY_CONTROL,
targetVelocity=wheelVelocities[i],
force=1000)
#p.resetBasePositionAndOrientation(kukaId,basepos,baseorn)#[0,0,0,1])
if (useRealTimeSimulation):
t = time.time() #(dt, micro) = datetime.utcnow().strftime('%Y-%m-%d %H:%M:%S.%f').split('.')
#t = (dt.second/60.)*2.*math.pi
else:
t = t + 0.001
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
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)]
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 (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
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
sawyerId = p.loadURDF("pole.urdf",[0,0,0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1])
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd=[0.1,0.1,0.1,0.1]
jd = [0.1, 0.1, 0.1, 0.1]
p.setGravity(0,0,0)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
ikSolver = 0
@ -30,30 +29,35 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 1
while 1:
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second/60.)*2.*math.pi
else:
t=t+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)
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId,i,jointPoses[qIndex-7])
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)
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
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
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):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def getMotorJointStates(robot):
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
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_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
joint_states = p.getJointStates(robot, 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_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
def setJointPosition(robot, position, kp=1.0, kv=0.3):
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
targetPositions=position, targetVelocities=zero_vec,
positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
num_joints = p.getNumJoints(robot)
zero_vec = [0.0] * num_joints
if len(position) == num_joints:
p.setJointMotorControlArray(robot,
range(num_joints),
p.POSITION_CONTROL,
targetPositions=position,
targetVelocities=zero_vec,
positionGains=[kp] * num_joints,
velocityGains=[kv] * num_joints)
else:
print("Not setting torque. "
"Expected torque vector of "
"length {}, got {}".format(num_joints, len(torque)))
def multiplyJacobian(robot, jacobian, vector):
result = [0.0, 0.0, 0.0]
i = 0
for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
result = [0.0, 0.0, 0.0]
i = 0
for c in range(len(vector)):
if p.getJointInfo(robot, c)[3] > -1:
for r in range(3):
result[r] += jacobian[r][i] * vector[c]
i += 1
return result
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
@ -50,14 +57,14 @@ p.resetSimulation()
p.setTimeStep(time_step)
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",[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("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)
kukaEndEffectorIndex = numJoints - 1
@ -69,7 +76,10 @@ p.stepSimulation()
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
result = p.getLinkState(kukaId,
kukaEndEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
@ -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)
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_vt)
print ("Link linear velocity of CoM from linearJacobian * q_dot:")
print (multiplyJacobian(kukaId, jac_t, vel))
print ("Link angular velocity of CoM from getLinkState:")
print (link_vr)
print ("Link angular velocity of CoM from angularJacobian * q_dot:")
print (multiplyJacobian(kukaId, jac_r, vel))
print("Link linear velocity of CoM from getLinkState:")
print(link_vt)
print("Link linear velocity of CoM from linearJacobian * q_dot:")
print(multiplyJacobian(kukaId, jac_t, vel))
print("Link angular velocity of CoM from getLinkState:")
print(link_vr)
print("Link angular velocity of CoM from angularJacobian * q_dot:")
print(multiplyJacobian(kukaId, jac_r, vel))

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

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

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

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

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

@ -2,15 +2,15 @@ import pybullet as p
import struct
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
print('Opened'),
print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record
sz = struct.calcsize(fmt)
# The type number of one record
@ -41,11 +41,12 @@ def readLogFile(filename, verbose = True):
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadSDF("kuka_iiwa/kuka_with_gripper.sdf")
p.loadURDF("tray/tray.urdf",[0,0,0])
p.loadURDF("block.urdf",[0,0,2])
p.loadURDF("tray/tray.urdf", [0, 0, 0])
p.loadURDF("block.urdf", [0, 0, 2])
log = readLogFile("data/block_grasp_log.bin")
@ -58,26 +59,26 @@ print(recordNum)
print('item num:'),
print(itemNum)
def Step(stepIndex):
for objectId in range(objectNum):
record = log[stepIndex*objectNum+objectId]
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
for objectId in range(objectNum):
record = log[stepIndex * objectNum + objectId]
Id = record[2]
pos = [record[3], record[4], record[5]]
orn = [record[6], record[7], record[8], record[9]]
p.resetBasePositionAndOrientation(Id, pos, orn)
numJoints = p.getNumJoints(Id)
for i in range(numJoints):
jointInfo = p.getJointInfo(Id, i)
qIndex = jointInfo[3]
if qIndex > -1:
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:
stepIndex = int(p.readUserDebugParameter(stepIndexId))
Step(stepIndex)
p.stepSimulation()
Step(stepIndex)
stepIndex = int(p.readUserDebugParameter(stepIndexId))
Step(stepIndex)
p.stepSimulation()
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)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3],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.loadURDF("plane.urdf", [0, 0, -0.3], useFixedBase=True)
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0], useFixedBase=True)
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
if (numJoints != 7):
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
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
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
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
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
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):
p.resetJointState(kukaId,i,rp[i])
for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])
p.setGravity(0,0,-10)
t=0.
prevPose=[0,0,0]
prevPose1=[0,0,0]
p.setGravity(0, 0, -10)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
useNullSpace = 0
@ -47,50 +47,68 @@ p.setRealTimeSimulation(useRealTimeSimulation)
#use 0 for no-removal
trailDuration = 15
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)
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)
for i in range(5):
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])
print("Body %d's name is %s." % (i, p.getBodyInfo(i)[1]))
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
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)
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
from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
print('Opened'),
print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record
sz = struct.calcsize(fmt)
# The type number of one record
@ -49,13 +50,14 @@ def readLogFile(filename, verbose = True):
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
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("plane.urdf", [0, 0, -0.3])
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])
log = readLogFile("LOG0001.txt")
@ -67,14 +69,14 @@ print('item num:'),
print(itemNum)
for record in log:
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
sleep(0.0005)
Id = record[2]
pos = [record[3], record[4], record[5]]
orn = [record[6], record[7], record[8], record[9]]
p.resetBasePositionAndOrientation(Id, pos, orn)
numJoints = p.getNumJoints(Id)
for i in range(numJoints):
jointInfo = p.getJointInfo(Id, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id, i, record[qIndex - 7 + 17])
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)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
bunnyId = p.loadSoftBody("bunny.obj")
p.loadURDF("cube_small.urdf",[1,0,1])
p.loadURDF("cube_small.urdf", [1, 0, 1])
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
p.setRealTimeSimulation(1)
while p.isConnected():
p.setGravity(0,0,-10)
if (useRealTimeSimulation):
p.setGravity(0, 0, -10)
if (useRealTimeSimulation):
sleep(0.01) # Time in seconds.
#p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL )
else:
p.stepSimulation()
sleep(0.01) # Time in seconds.
#p.getCameraImage(320,200,renderer=p.ER_BULLET_HARDWARE_OPENGL )
else:
p.stepSimulation()

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

@ -4,25 +4,23 @@ p.connect(p.GUI)
p.resetSimulation()
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")
p.loadURDF("plane.urdf")
print("load r2d2.urdf")
p.loadURDF("r2d2.urdf",0,0,1)
p.loadURDF("r2d2.urdf", 0, 0, 1)
print("load kitchen/1.sdf")
p.loadSDF("kitchens/1.sdf")
print("load 100 times plate.urdf")
for i in range (100):
p.loadURDF("dinnerware/plate.urdf",0,i,1)
for i in range(100):
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)
print("stopped state logging")
p.getCameraImage(320,200)
p.getCameraImage(320, 200)
while (1):
p.stepSimulation()
p.stepSimulation()

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

@ -1,11 +1,11 @@
import pybullet as p
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0) :
p.connect(p.GUI)
if (cid < 0):
p.connect(p.GUI)
p.loadURDF("plane.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()

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

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

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

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

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

@ -1,7 +1,9 @@
import pybullet as p
import numpy as np
class Minitaur:
def __init__(self, urdfRootPath=''):
self.urdfRootPath = urdfRootPath
self.reset()
@ -26,9 +28,8 @@ class Minitaur:
self.motorIdList.append(self.jointNameToId['motor_back_rightL_joint'])
self.motorIdList.append(self.jointNameToId['motor_back_rightR_joint'])
def reset(self):
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath,0,0,.2)
self.quadruped = p.loadURDF("%s/quadruped/minitaur.urdf" % self.urdfRootPath, 0, 0, .2)
self.kp = 1
self.kd = 0.1
self.maxForce = 3.5
@ -38,10 +39,14 @@ class Minitaur:
self.buildJointNameToIdDict()
self.buildMotorIdList()
def setMotorAngleById(self, motorId, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=motorId, controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=motorId,
controlMode=p.POSITION_CONTROL,
targetPosition=desiredAngle,
positionGain=self.kp,
velocityGain=self.kd,
force=self.maxForce)
def setMotorAngleByName(self, motorName, desiredAngle):
self.setMotorAngleById(self.jointNameToId[motorName], desiredAngle)
@ -49,53 +54,107 @@ class Minitaur:
def resetPose(self):
kneeFrictionForce = 0
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
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_leftL_joint'],
self.motorDir[0] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_leftL_link'],
self.motorDir[0] * kneeangle)
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_leftR_joint'],
self.motorDir[1] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_leftR_link'],
self.motorDir[1] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_front_leftR_link'], self.quadruped,
self.jointNameToId['knee_front_leftL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0] * halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_leftL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_leftL_joint'],
self.motorDir[2] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_leftL_link'],
self.motorDir[2] * kneeangle)
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_leftR_joint'],
self.motorDir[3] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_leftR_link'],
self.motorDir[3] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_back_leftR_link'], self.quadruped,
self.jointNameToId['knee_back_leftL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_back_leftL_joint', self.motorDir[2] * halfpi)
self.setMotorAngleByName('motor_back_leftR_joint', self.motorDir[3] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_leftL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_leftR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_rightL_joint'],
self.motorDir[4] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_rightL_link'],
self.motorDir[4] * kneeangle)
p.resetJointState(self.quadruped, self.jointNameToId['motor_front_rightR_joint'],
self.motorDir[5] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_front_rightR_link'],
self.motorDir[5] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_front_rightR_link'],
self.quadruped, self.jointNameToId['knee_front_rightL_link'],
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_front_rightL_joint', self.motorDir[4] * halfpi)
self.setMotorAngleByName('motor_front_rightR_joint', self.motorDir[5] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_rightL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_front_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightL_joint'],
self.motorDir[6] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightL_link'],
self.motorDir[6] * kneeangle)
p.resetJointState(self.quadruped, self.jointNameToId['motor_back_rightR_joint'],
self.motorDir[7] * halfpi)
p.resetJointState(self.quadruped, self.jointNameToId['knee_back_rightR_link'],
self.motorDir[7] * kneeangle)
p.createConstraint(self.quadruped, self.jointNameToId['knee_back_rightR_link'], self.quadruped,
self.jointNameToId['knee_back_rightL_link'], p.JOINT_POINT2POINT, [0, 0, 0],
[0, 0.005, 0.2], [0, 0.01, 0.2])
self.setMotorAngleByName('motor_back_rightL_joint', self.motorDir[6] * halfpi)
self.setMotorAngleByName('motor_back_rightR_joint', self.motorDir[7] * halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_rightL_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,
jointIndex=self.jointNameToId['knee_back_rightR_link'],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=kneeFrictionForce)
def getBasePosition(self):
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()
return np.asarray(position)
def is_fallen():
global minitaur
orientation = minitaur.getBaseOrientation()
@ -22,13 +23,16 @@ def is_fallen():
localUp = rotMat[6:]
return np.dot(np.asarray([0, 0, 1]), np.asarray(localUp)) < 0
def evaluate_desired_motorAngle_8Amplitude8Phase(i, params):
nMotors = 8
speed = 0.35
for jthMotor in range(nMotors):
joint_values[jthMotor] = math.sin(i*speed + params[nMotors + jthMotor])*params[jthMotor]*+1.57
joint_values[jthMotor] = math.sin(i * speed +
params[nMotors + jthMotor]) * params[jthMotor] * +1.57
return joint_values
def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
speed = 0.35
phaseDiff = params[2]
@ -43,29 +47,37 @@ def evaluate_desired_motorAngle_2Amplitude4Phase(i, params):
joint_values = [a0, a1, a2, a3, a4, a5, a6, a7]
return joint_values
def evaluate_desired_motorAngle_hop(i, params):
amplitude = params[0]
speed = params[1]
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
a1 = math.sin(i * speed) * 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]
return joint_values
evaluate_func_map['evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
evaluate_func_map['evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
evaluate_func_map[
'evaluate_desired_motorAngle_8Amplitude8Phase'] = evaluate_desired_motorAngle_8Amplitude8Phase
evaluate_func_map[
'evaluate_desired_motorAngle_2Amplitude4Phase'] = evaluate_desired_motorAngle_2Amplitude4Phase
evaluate_func_map['evaluate_desired_motorAngle_hop'] = evaluate_desired_motorAngle_hop
def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep=0.01, maxNumSteps=10000, sleepTime=0):
def evaluate_params(evaluateFunc,
params,
objectiveParams,
urdfRoot='',
timeStep=0.01,
maxNumSteps=10000,
sleepTime=0):
print('start evaluation')
beforeTime = time.time()
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("%s/plane.urdf" % urdfRoot)
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
global minitaur
minitaur = Minitaur(urdfRoot)
@ -95,5 +107,6 @@ def evaluate_params(evaluateFunc, params, objectiveParams, urdfRoot='', timeStep
final_distance = np.linalg.norm(start_position - current_position())
finalReturn = final_distance - alpha * total_energy
elapsedTime = time.time() - beforeTime
print ("trial for ", params, " final_distance", final_distance, "total_energy", total_energy, "finalReturn", finalReturn, "elapsed_time", elapsedTime)
print("trial for ", params, " final_distance", final_distance, "total_energy", total_energy,
"finalReturn", finalReturn, "elapsed_time", elapsedTime)
return finalReturn

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

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

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

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

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

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

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

@ -1,4 +1,3 @@
import pybullet as p
from pdControllerExplicit import PDControllerExplicitMultiDof
from pdControllerExplicit import PDControllerExplicit
@ -6,107 +5,145 @@ from pdControllerStable import PDControllerStable
import time
useMaximalCoordinates=False
useMaximalCoordinates = False
p.connect(p.GUI)
pole = p.loadURDF("cartpole.urdf", [0,0,0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0,1,0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0,2,0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0,3,0], useMaximalCoordinates=useMaximalCoordinates)
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
pole2 = p.loadURDF("cartpole.urdf", [0, 1, 0], useMaximalCoordinates=useMaximalCoordinates)
pole3 = p.loadURDF("cartpole.urdf", [0, 2, 0], useMaximalCoordinates=useMaximalCoordinates)
pole4 = p.loadURDF("cartpole.urdf", [0, 3, 0], useMaximalCoordinates=useMaximalCoordinates)
exPD = PDControllerExplicitMultiDof(p)
sPD = PDControllerStable(p)
for i in range(p.getNumJoints(pole2)):
#disable default constraint-based motors
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=0)
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)):
#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))
#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)
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]
textColor = [1, 1, 1]
shift = 0.05
p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift,0,-.1],textColor,parentObjectUniqueId=pole2,parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift,0,.1],textColor,parentObjectUniqueId=pole4,parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift,0,-.1],textColor,parentObjectUniqueId=pole3,parentLinkIndex=1)
p.addUserDebugText("explicit PD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole,
parentLinkIndex=1)
p.addUserDebugText("explicit PD plugin", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole2,
parentLinkIndex=1)
p.addUserDebugText("stablePD", [shift, 0, .1],
textColor,
parentObjectUniqueId=pole4,
parentLinkIndex=1)
p.addUserDebugText("position constraint", [shift, 0, -.1],
textColor,
parentObjectUniqueId=pole3,
parentLinkIndex=1)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
kpPoleId = p.addUserDebugParameter("kpPole",0,500,1200)
kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
desiredPosPoleId = p.addUserDebugParameter("desiredPosPole", -10, 10, 0)
desiredVelPoleId = p.addUserDebugParameter("desiredVelPole", -10, 10, 0)
kpPoleId = p.addUserDebugParameter("kpPole", 0, 500, 1200)
kdPoleId = p.addUserDebugParameter("kdPole", 0, 300, 100)
maxForcePoleId = p.addUserDebugParameter("maxForcePole", 0, 5000, 1000)
pd = p.loadPlugin("pdControlPlugin")
p.setGravity(0,0,-10)
p.setGravity(0, 0, -10)
useRealTimeSim = False
p.setRealTimeSimulation(useRealTimeSim)
timeStep = 0.001
while p.isConnected():
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
#p.getCameraImage(320,200)
timeStep = p.readUserDebugParameter(timeStepId)
p.setTimeStep(timeStep)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
kpCart = p.readUserDebugParameter(kpCartId)
kdCart = p.readUserDebugParameter(kdCartId)
maxForceCart = p.readUserDebugParameter(maxForceCartId)
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
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]])
desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
kpPole = p.readUserDebugParameter(kpPoleId)
kdPole = p.readUserDebugParameter(kdPoleId)
maxForcePole = p.readUserDebugParameter(maxForcePoleId)
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)
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]])
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):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
qError = [0,0,0, 0,0,0,0]
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
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
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
q1 = [curPos[0], curPos[1], curPos[2], curOrn[0], curOrn[1], curOrn[2], curOrn[3]]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [
baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
]
qError = [0, 0, 0, 0, 0, 0, 0]
qIndex = 7
qdotIndex = 7
zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
for i in range(numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos = js[0]
jointVel = js[1]
q1 += jointPos
if len(js[0]) == 1:
desiredPos = desiredPositions[qIndex]
qdiff = desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1 += jointVel
qIndex += 1
qdotIndex += 1
if len(js[0]) == 4:
desiredPos = [
desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredPositions[qIndex + 3]
]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel = [
desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
desiredVelocities[qdotIndex + 2]
]
zeroAccelerations += [0., 0., 0., 0.]
qIndex += 4
qdotIndex += 4
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):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
forces = Kp.dot(qError) + Kd.dot(qdotError)
maxF = np.array(maxForces)
forces = np.clip(forces, -maxF , maxF )
return forces
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
for i in range(numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
q = np.array(q1)
qdot = np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
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
class PDControllerStableMultiDof(object):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
#q1 = [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 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
qError = [0,0,0, 0,0,0,0]
qIndex=7
qdotIndex=7
zeroAccelerations=[0,0,0, 0,0,0,0]
for i in range (numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos=js[0]
jointVel = js[1]
q1+=jointPos
if len(js[0])==1:
desiredPos=desiredPositions[qIndex]
qdiff=desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1+=jointVel
qIndex+=1
qdotIndex+=1
if len(js[0])==4:
desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
qdot1+=jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
zeroAccelerations+=[0.,0.,0.,0.]
qIndex+=4
qdotIndex+=4
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)
#np.savetxt("pb_qError.csv", qError, delimiter=",")
#np.savetxt("pb_p.csv", p, delimiter=",")
d = Kd.dot(qdoterr)
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = len(jointIndices) #self._pb.getNumJoints(bodyUniqueId)
curPos, curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
#q1 = [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 = [0,0,0, 0,0,0,0]
baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
qdot1 = [
baseLinVel[0], baseLinVel[1], baseLinVel[2], baseAngVel[0], baseAngVel[1], baseAngVel[2], 0
]
qError = [0, 0, 0, 0, 0, 0, 0]
qIndex = 7
qdotIndex = 7
zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
for i in range(numJoints):
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos = js[0]
jointVel = js[1]
q1 += jointPos
if len(js[0]) == 1:
desiredPos = desiredPositions[qIndex]
qdiff = desiredPos - jointPos[0]
qError.append(qdiff)
zeroAccelerations.append(0.)
qdot1 += jointVel
qIndex += 1
qdotIndex += 1
if len(js[0]) == 4:
desiredPos = [
desiredPositions[qIndex], desiredPositions[qIndex + 1], desiredPositions[qIndex + 2],
desiredPositions[qIndex + 3]
]
axis = self._pb.getAxisDifferenceQuaternion(desiredPos, jointPos)
jointVelNew = [jointVel[0], jointVel[1], jointVel[2], 0]
qdot1 += jointVelNew
qError.append(axis[0])
qError.append(axis[1])
qError.append(axis[2])
qError.append(0)
desiredVel = [
desiredVelocities[qdotIndex], desiredVelocities[qdotIndex + 1],
desiredVelocities[qdotIndex + 2]
]
zeroAccelerations += [0., 0., 0., 0.]
qIndex += 4
qdotIndex += 4
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)
#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):
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range (numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot=np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
tau = p + d - Kd.dot(qddot) * timeStep
maxF = np.array(maxForces)
forces = np.clip(tau, -maxF , maxF )
#print("c=",c)
return tau
def __init__(self, pb):
self._pb = pb
def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds,
maxForces, timeStep):
numJoints = self._pb.getNumJoints(bodyUniqueId)
jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
q1 = []
qdot1 = []
zeroAccelerations = []
for i in range(numJoints):
q1.append(jointStates[i][0])
qdot1.append(jointStates[i][1])
zeroAccelerations.append(0)
q = np.array(q1)
qdot = np.array(qdot1)
qdes = np.array(desiredPositions)
qdotdes = np.array(desiredVelocities)
qError = qdes - q
qdotError = qdotdes - qdot
Kp = np.diagflat(kps)
Kd = np.diagflat(kds)
p = Kp.dot(qError)
d = Kd.dot(qdotError)
forces = p + d
M1 = self._pb.calculateMassMatrix(bodyUniqueId, q1)
M2 = np.array(M1)
M = (M2 + Kd * timeStep)
c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
c = np.array(c1)
A = M
b = -c + p + d
qddot = np.linalg.solve(A, b)
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 math
import numpy as np
p.connect(p.GUI)
plane = p.loadURDF("plane100.urdf")
cube = p.loadURDF("cube.urdf",[0,0,1])
def getRayFromTo(mouseX,mouseY):
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera()
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
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
cube = p.loadURDF("cube.urdf", [0, 0, 1])
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]]
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(
)
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])
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]
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])
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])
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]
cornersX = [0,width,width,0]
cornersY = [0,0,height,height]
corners3D=[]
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])
imgW = int(width/10)
imgH = int(height/10)
cornersX = [0, width, width, 0]
cornersY = [0, 0, height, height]
corners3D = []
img = p.getCameraImage(imgW,imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer=img[2]
imgW = int(width / 10)
imgH = int(height / 10)
img = p.getCameraImage(imgW, imgH, renderer=p.ER_BULLET_HARDWARE_OPENGL)
rgbBuffer = img[2]
depthBuffer = img[3]
print("rgbBuffer.shape=",rgbBuffer.shape)
print("depthBuffer.shape=",depthBuffer.shape)
print("rgbBuffer.shape=", rgbBuffer.shape)
print("depthBuffer.shape=", depthBuffer.shape)
#disable rendering temporary makes adding objects faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
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)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 0)
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)
for i in range (4):
w = cornersX[i]
h = cornersY[i]
rayFrom,rayTo, _= getRayFromTo(w,h)
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
newTo = (0.01/l)*vec+rf
#print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom,newTo,[1,0,0])
corners3D.append(newTo)
for i in range(4):
w = cornersX[i]
h = cornersY[i]
rayFrom, rayTo, _ = getRayFromTo(w, h)
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt - rf
l = np.sqrt(np.dot(vec, vec))
newTo = (0.01 / l) * vec + rf
#print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
corners3D.append(newTo)
count = 0
stepX=5
stepY=5
for w in range(0,imgW,stepX):
for h in range (0,imgH,stepY):
count+=1
if ((count % 100)==0):
print(count,"out of ", imgW*imgH/(stepX*stepY))
rayFrom,rayTo, alpha = getRayFromTo(w*(width/imgW),h*(height/imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
depthImg = float(depthBuffer[h,w])
far=1000.
near=0.01
depth = far * near / (far - (far - near) * depthImg)
depth/=math.cos(alpha)
newTo = (depth/l)*vec+rf
p.addUserDebugLine(rayFrom,newTo,[1,0,0])
mb = p.createMultiBody(baseMass=0,baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = newTo, useMaximalCoordinates=True)
color = rgbBuffer[h,w]
color=[color[0]/255.,color[1]/255.,color[2]/255.,1]
p.changeVisualShape(mb,-1,rgbaColor=color)
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)
stepX = 5
stepY = 5
for w in range(0, imgW, stepX):
for h in range(0, imgH, stepY):
count += 1
if ((count % 100) == 0):
print(count, "out of ", imgW * imgH / (stepX * stepY))
rayFrom, rayTo, alpha = getRayFromTo(w * (width / imgW), h * (height / imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt - rf
l = np.sqrt(np.dot(vec, vec))
depthImg = float(depthBuffer[h, w])
far = 1000.
near = 0.01
depth = far * near / (far - (far - near) * depthImg)
depth /= math.cos(alpha)
newTo = (depth / l) * vec + rf
p.addUserDebugLine(rayFrom, newTo, [1, 0, 0])
mb = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=newTo,
useMaximalCoordinates=True)
color = rgbBuffer[h, w]
color = [color[0] / 255., color[1] / 255., color[2] / 255., 1]
p.changeVisualShape(mb, -1, rgbaColor=color)
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")
#p.removeBody(plane)
#p.removeBody(cube)
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)
t = time.time()+0.1
t = time.time() + 0.1
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "haha")
while (time.time()<t):
p.submitProfileTiming("pythontest")
while (time.time() < t):
p.submitProfileTiming("pythontest")
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)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
p.setGravity(0, 0, 0)
bearStartPos1 = [-3.3, 0, 0]
bearStartOrientation1 = p.getQuaternionFromEuler([0, 0, 0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
bearStartPos2 = [0, 0, 0]
bearStartOrientation2 = p.getQuaternionFromEuler([0, 0, 0])
bearId2 = p.loadURDF("teddy_large.urdf", bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300, 300, renderer=p.ER_BULLET_HARDWARE_OPENGL, flags=p.ER_USE_PROJECTIVE_TEXTURE, projectiveTextureView=viewMat, projectiveTextureProj=projMat)
p.setGravity(0,0,0)
else:
p.stepSimulation()
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
p.getCameraImage(300,
300,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
flags=p.ER_USE_PROJECTIVE_TEXTURE,
projectiveTextureView=viewMat,
projectiveTextureProj=projMat)
p.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):
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
mass=dyn[0]
frictionCoeff=dyn[1]
inertia = dyn[2]
if (mass>0):
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]]]
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)
dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
mass = dyn[0]
frictionCoeff = dyn[1]
inertia = dyn[2]
if (mass > 0):
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]]]
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)
toeConstraint = True
@ -48,12 +107,12 @@ useMaximalCoordinates = False
useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1./100
fixedTimeStep = 1. / 100
numSolverIterations = 50
if (useMaximalCoordinates):
fixedTimeStep = 1./500
numSolverIterations = 200
fixedTimeStep = 1. / 500
numSolverIterations = 200
speed = 10
amplitude = 0.8
@ -64,32 +123,37 @@ kp = 1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
p.connect(p.GUI)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0,angle,0])
p.loadURDF("plane.urdf",[0,0,0],orn)
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0,0,0)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4])
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_USE_IMPLICIT_CYLINDER)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
useFixedBase=False,
useMaximalCoordinates=useMaximalCoordinates,
flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
@ -116,183 +180,286 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir=[-1,-1,-1,-1,1,1,1,1]
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4*halfpi
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped,-1)
mass=dyn[0]
friction=dyn[1]
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal",localInertiaDiagonal)
print("localInertiaDiagonal", localInertiaDiagonal)
#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):
# 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])
for i in range (nJoints):
drawInertiaBox(quadruped,i, [0,1,0])
for i in range(nJoints):
drawInertiaBox(quadruped, i, [0, 1, 0])
if (useMaximalCoordinates):
steps = 400
for aa in range (steps):
p.setJointMotorControl2(quadruped,motor_front_leftL_joint,p.POSITION_CONTROL,motordir[0]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightL_joint,p.POSITION_CONTROL,motordir[6]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftL_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightR_link,p.POSITION_CONTROL,motordir[7]*kneeangle*float(aa)/steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
steps = 400
for aa in range(steps):
p.setJointMotorControl2(quadruped, motor_front_leftL_joint, p.POSITION_CONTROL,
motordir[0] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_leftR_joint, p.POSITION_CONTROL,
motordir[1] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftL_joint, p.POSITION_CONTROL,
motordir[2] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_leftR_joint, p.POSITION_CONTROL,
motordir[3] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightL_joint, p.POSITION_CONTROL,
motordir[4] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_front_rightR_joint, p.POSITION_CONTROL,
motordir[5] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightL_joint, p.POSITION_CONTROL,
motordir[6] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, motor_back_rightR_joint, p.POSITION_CONTROL,
motordir[7] * halfpi * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftL_link, p.POSITION_CONTROL,
motordir[0] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_leftR_link, p.POSITION_CONTROL,
motordir[1] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftL_link, p.POSITION_CONTROL,
motordir[2] * kneeangle * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_leftR_link, p.POSITION_CONTROL,
motordir[3] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightL_link, p.POSITION_CONTROL,
motordir[4] * (kneeangle) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_front_rightR_link, p.POSITION_CONTROL,
motordir[5] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightL_link, p.POSITION_CONTROL,
motordir[6] * (kneeangle + twopi) * float(aa) / steps)
p.setJointMotorControl2(quadruped, knee_back_rightR_link, p.POSITION_CONTROL,
motordir[7] * kneeangle * float(aa) / steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
p.resetJointState(quadruped, motor_front_leftL_joint, motordir[0] * halfpi)
p.resetJointState(quadruped, knee_front_leftL_link, motordir[0] * kneeangle)
p.resetJointState(quadruped, motor_front_leftR_joint, motordir[1] * halfpi)
p.resetJointState(quadruped, knee_front_leftR_link, motordir[1] * kneeangle)
p.resetJointState(quadruped, motor_back_leftL_joint, motordir[2] * halfpi)
p.resetJointState(quadruped, knee_back_leftL_link, motordir[2] * kneeangle)
p.resetJointState(quadruped, motor_back_leftR_joint, motordir[3] * halfpi)
p.resetJointState(quadruped, knee_back_leftR_link, motordir[3] * kneeangle)
p.resetJointState(quadruped, motor_front_rightL_joint, motordir[4] * halfpi)
p.resetJointState(quadruped, knee_front_rightL_link, motordir[4] * kneeangle)
p.resetJointState(quadruped, motor_front_rightR_joint, motordir[5] * halfpi)
p.resetJointState(quadruped, knee_front_rightR_link, motordir[5] * kneeangle)
p.resetJointState(quadruped, motor_back_rightL_joint, motordir[6] * halfpi)
p.resetJointState(quadruped, knee_back_rightL_link, motordir[6] * kneeangle)
p.resetJointState(quadruped, motor_back_rightR_joint, motordir[7] * halfpi)
p.resetJointState(quadruped, knee_back_rightR_link, motordir[7] * kneeangle)
#p.getNumJoints(1)
if (toeConstraint):
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
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)
cid = p.createConstraint(quadruped, knee_front_leftR_link, quadruped, knee_front_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_front_rightR_link, quadruped, knee_front_rightL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
p.changeConstraint(cid, maxForce=maxKneeForce)
cid = p.createConstraint(quadruped, knee_back_leftR_link, quadruped, knee_back_leftL_link,
p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.005, 0.1], [0, 0.01, 0.1])
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):
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_leftL_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_leftR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightL_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_front_rightR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftL_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_leftR_link, p.VELOCITY_CONTROL, 0, kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightL_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.setJointMotorControl(quadruped, knee_back_rightR_link, p.VELOCITY_CONTROL, 0,
kneeFrictionForce)
p.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
]
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):
print (legnumbering[i])
for i in range(8):
print(legnumbering[i])
#use the Minitaur leg numbering
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*1.57,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * 1.57,
positionGain=kp,
velocityGain=kd,
force=maxForce)
#stand still
p.setRealTimeSimulation(useRealTime)
t = 0.0
t_end = t + 15
ref_time = time.time()
while (t<t_end):
p.setGravity(0,0,-10)
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
if (useRealTime==0):
p.stepSimulation()
time.sleep(fixedTimeStep)
while (t < t_end):
p.setGravity(0, 0, -10)
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (useRealTime == 0):
p.stepSimulation()
time.sleep(fixedTimeStep)
print("quadruped Id = ")
print(quadruped)
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
t = 0.0
t_end = t + 100
i=0
i = 0
ref_time = time.time()
while (1):
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
if (True):
target = math.sin(t*speed)*jump_amp+1.57;
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[0],controlMode=p.POSITION_CONTROL,targetPosition=motordir[0]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[1],controlMode=p.POSITION_CONTROL,targetPosition=motordir[1]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[2],controlMode=p.POSITION_CONTROL,targetPosition=motordir[2]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[3],controlMode=p.POSITION_CONTROL,targetPosition=motordir[3]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[4],controlMode=p.POSITION_CONTROL,targetPosition=motordir[4]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[5],controlMode=p.POSITION_CONTROL,targetPosition=motordir[5]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[6],controlMode=p.POSITION_CONTROL,targetPosition=motordir[6]*target,positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMode=p.POSITION_CONTROL,targetPosition=motordir[7]*target,positionGain=kp, velocityGain=kd, force=maxForce)
if (useRealTime==0):
p.stepSimulation()
time.sleep(fixedTimeStep)
if (useRealTime):
t = time.time() - ref_time
else:
t = t + fixedTimeStep
if (True):
target = math.sin(t * speed) * jump_amp + 1.57
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[0] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[1] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[2] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[3] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[4] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[5] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[6] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motordir[7] * target,
positionGain=kp,
velocityGain=kd,
force=maxForce)
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
from time import sleep
def readLogFile(filename, verbose = True):
def readLogFile(filename, verbose=True):
f = open(filename, 'rb')
print('Opened'),
print(filename)
keys = f.readline().decode('utf8').rstrip('\n').split(',')
fmt = f.readline().decode('utf8').rstrip('\n')
# The byte number of one record
sz = struct.calcsize(fmt)
# The type number of one record
@ -38,9 +39,9 @@ def readLogFile(filename, verbose = True):
wholeFile = f.read()
# split by alignment word
chunks = wholeFile.split(b'\xaa\xbb')
print ("num chunks")
print (len(chunks))
print("num chunks")
print(len(chunks))
log = list()
for chunk in chunks:
if len(chunk) == sz:
@ -52,9 +53,10 @@ def readLogFile(filename, verbose = True):
return log
clid = p.connect(p.SHARED_MEMORY)
log = readLogFile("LOG00076.TXT");
log = readLogFile("LOG00076.TXT")
recordNum = len(log)
print('record num:'),
@ -72,8 +74,6 @@ maxForce = 3.5
kp = .05
kd = .5
quadruped = 1
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
@ -106,21 +106,71 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
motorDir = [1, 1, 1, 1, 1, 1, 1, 1];
legnumbering=[motor_front_leftR_joint,motor_front_leftL_joint,motor_back_leftR_joint,motor_back_leftL_joint,motor_front_rightR_joint,motor_front_rightL_joint,motor_back_rightR_joint,motor_back_rightL_joint]
motorDir = [1, 1, 1, 1, 1, 1, 1, 1]
legnumbering = [
motor_front_leftR_joint, motor_front_leftL_joint, motor_back_leftR_joint,
motor_back_leftL_joint, motor_front_rightR_joint, motor_front_rightL_joint,
motor_back_rightR_joint, motor_back_rightL_joint
]
for record in log:
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[0], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[0]*record[7], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[1], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[1]*record[8], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[2], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[2]*record[9], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[3], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[3]*record[10], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[4], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[4]*record[11], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[5], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[5]*record[12], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[6], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[6]*record[13], positionGain=kp, velocityGain=kd, force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped, jointIndex=legnumbering[7], controlMode=p.POSITION_CONTROL, targetPosition=motorDir[7]*record[14], positionGain=kp, velocityGain=kd, force=maxForce)
p.setGravity(0.000000,0.000000,-10.000000)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[0],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[0] * record[7],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[1],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[1] * record[8],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[2],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[2] * record[9],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[3],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[3] * record[10],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[4],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[4] * record[11],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[5],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[5] * record[12],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[6],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[6] * record[13],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=legnumbering[7],
controlMode=p.POSITION_CONTROL,
targetPosition=motorDir[7] * record[14],
positionGain=kp,
velocityGain=kd,
force=maxForce)
p.setGravity(0.000000, 0.000000, -10.000000)
p.stepSimulation()
p.stepSimulation()
sleep(0.01)

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

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

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

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

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