add yapf style and apply yapf to format all Python files
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
@@ -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()
|
||||
|
||||
|
||||
|
||||
@@ -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.)
|
||||
|
||||
@@ -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(" ")
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user