def handle_camera(self):
self.view.clippingplanes = (self.view.clippingplanes[0], self.zoomMax)
cam = self.view.camera
moveSpd = self.moveSpd * cam.dist
if self.zoomInCam:
cam.dist = max(cam.dist / self.zoomSpd, self.zoomMin)
elif self.zoomOutCam:
cam.dist = min(cam.dist * self.zoomSpd, self.zoomMax)
elif self.forwardCam:
delta = op.mul(self.get_camera_dir(self.zeroZ), moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
elif self.backCam:
delta = op.mul(self.get_camera_dir(self.zeroZ), -moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
elif self.leftCam:
delta = op.mul(self.get_left_dir(self.zeroZ), moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
elif self.rightCam:
delta = op.mul(self.get_left_dir(self.zeroZ), -moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
elif self.raiseCam:
delta = (0, 0, moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
elif self.sinkCam:
delta = (0, 0, -moveSpd)
self.look_at(op.add(self.get_camera_pos(), delta),
op.add(cam.tgt, delta))
def calculate_workspace_free(robot,obstacles,end_effector,point_local):
"""Calculate the reachable workspace of the end effector point whose coordinates
are `point_local` on link `end_effector`. Ensure that the robot is collision
free with itself and with environment obstacles
"""
global lower_corner,upper_corner
resolution = (20,20,20)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
feasible = collision_free(robot,obstacles)
if feasible:
wp = end_effector.getWorldPosition(point_local)
index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(wp,lower_corner),invcellsize)]
if all(i>=0 and i<r for (i,r) in zip(index,resolution)):
reachable[tuple(index)] = 1.0
# print(index)
num_samples = 100000
rand_positions = np.random.rand(num_samples, 3)
rand_positions[:,0] = rand_positions[:,0] * vectorops.sub(upper_corner, lower_corner)[0] + lower_corner[0]
rand_positions[:,1] = rand_positions[:,1] * vectorops.sub(upper_corner, lower_corner)[1] + lower_corner[1]
rand_positions[:,2] = rand_positions[:,2] * upper_corner[2]
for i in range(num_samples):
index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(rand_positions[i],lower_corner),invcellsize)]
for obstacle in obstacles:
if obstacle.geometry().distance_point(rand_positions[i]).d <= 0:
reachable[tuple(index)] = 0.0
else:
reachable[tuple(index)] = 1.0
return reachable
def draw_hull(hull,scale=0.01):
"""Draws a ConvexHull with 3D texture coordinates"""
if len(hull.simplices)==0:
print "Hull with no simplices?"
return
centroid = sum([hull.points[v] for v in hull.vertices],[0.0]*3) / len(hull.vertices) * scale
vmin = [min([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)]
vmax = [max([hull.points[v][i] for v in hull.vertices])*scale for i in range(3)]
uvwscale = [1.0/(b-a) if b != a else 1.0 for (a,b) in zip(vmin,vmax)]
for simplex in hull.simplices:
ia,ib,ic = simplex
a = vectorops.mul(hull.points[ia].tolist(),scale)
b = vectorops.mul(hull.points[ib].tolist(),scale)
c = vectorops.mul(hull.points[ic].tolist(),scale)
n = vectorops.cross(vectorops.sub(b,a),vectorops.sub(c,a))
if vectorops.dot(n,vectorops.sub(centroid,a)) > 0:
b,c = c,b
n = vectorops.mul(n,-1)
try:
n = vectorops.mul(n,1.0/vectorops.norm(n))
glNormal3f(*n)
except ZeroDivisionError:
pass
glBegin(GL_TRIANGLES)
glTexCoord3f(uvwscale[0]*(a[0]-vmin[0]),uvwscale[1]*(a[1]-vmin[1]),uvwscale[2]*(a[2]-vmin[2]))
glVertex3f(*a)
glTexCoord3f(uvwscale[0]*(b[0]-vmin[0]),uvwscale[1]*(b[1]-vmin[1]),uvwscale[2]*(b[2]-vmin[2]))
glVertex3f(*b)
glTexCoord3f(uvwscale[0]*(c[0]-vmin[0]),uvwscale[1]*(c[1]-vmin[1]),uvwscale[2]*(c[2]-vmin[2]))
glVertex3f(*c)
glEnd()
def drawConfig(self, q=None):
if self.drawJointLimitColors:
testq = q
if q is None:
testq = self.robot.getConfig()
oldColors = []
for i in xrange(self.robot.numLinks()):
oldColors.append(self.robot.link(i).appearance().getColor())
if self.robot.getJointType(i) == 'normal':
a, b = self.jointLimits[0][i], self.jointLimits[1][i]
u = 1.0
u = min(1, (testq[i] - a) / (self.jointLimitColoringRatio *
(b - a)))
u = min(1, (b - testq[i]) / (self.jointLimitColoringRatio *
(b - a)))
if u != 1:
u = max(u, 0.0)
c = vectorops.add(
vectorops.mul(oldColors[i], u),
vectorops.mul(self.jointLimitColor, 1 - u))
self.robot.link(i).appearance().setColor(*c)
if q is not None:
self.robot.setConfig(q)
self.robot.drawGL()
if self.drawJointLimitColors:
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(*oldColors[i])
def substep(self, dt):
twist = self.twist
#compute the angular velocity of the shell in the motor frame
motorBody = self.sim.body(self.robot.link(5))
shellBody = self.sim.body(self.robot.link(8))
motorTwist = motorBody.getVelocity()
shellTwist = shellBody.getVelocity()
motorXform = motorBody.getTransform()
shellXform = shellBody.getTransform()
shellRelativeAvel = so3.apply(
so3.inv(motorXform[0]), vectorops.sub(shellTwist[0],
motorTwist[0]))
#print "Relative angular vel",shellRelativeAvel
desiredTurnSpeed = twist[2] * self.velocityLimits[0]
desiredDriveSpeed = 0
if twist[0] == 0 or twist[0] * self.motorSpeeds[1] < 0: #stop
desiredDriveSpeed = 0
else:
desiredDriveSpeed = self.motorSpeeds[
1] + twist[0] * self.accelLimits[1] * self.dt
#print "Turn des",desiredTurnSpeed, "drive des",desiredDriveSpeed
#clamp speeds to limits
desiredTurnSpeed = max(-self.velocityLimits[0],
min(desiredTurnSpeed, self.velocityLimits[0]))
desiredDriveSpeed = max(-self.velocityLimits[1],
min(desiredDriveSpeed, self.velocityLimits[1]))
terr = desiredTurnSpeed - self.motorSpeeds[0]
derr = desiredDriveSpeed - self.motorSpeeds[1]
#clamp desired accelerations to limits
terr = max(-self.accelLimits[0] * self.dt,
min(terr, self.accelLimits[0] * self.dt))
derr = max(-self.accelLimits[1] * self.dt,
min(derr, self.accelLimits[1] * self.dt))
self.motorSpeeds[0] += terr
self.motorSpeeds[1] += derr
#apply locked velocity control to bring shell up to relative speed
#this is the desired angular velocity of the shell in the motor
#coordinates
desiredShellAvel = [self.motorSpeeds[1], 0, self.motorSpeeds[0]]
#print "Desired angular vel",desiredShellAvel
relativeVelError = vectorops.sub(desiredShellAvel, shellRelativeAvel)
wrenchlocal = vectorops.mul(relativeVelError, self.velocityLockGain)
#local wrench is k*(wdes-wrel)
wrench = so3.apply(motorXform[0], wrenchlocal)
#print "Wrench to shell",wrench
motorBody.applyWrench([0, 0, 0], vectorops.mul(wrench, -1.0))
shellBody.applyWrench([0, 0, 0], wrench)
#disable PID controllers
self.controller.setTorque([0, 0, 0])
#apply rolling friction forces
shellBody.applyWrench([0, 0, 0],
vectorops.mul(shellTwist[0],
-self.rollingFrictionCoeff))
return
def gen_mul(a, b):
"""Multiplies lists or scalars in a unified way. With lists
the multiplication is elementwise."""
if isinstance(a, np.ndarray):
return np.dot(a, b)
elif hasattr(a, '__iter__'):
return vectorops.mul(a, b)
elif hasattr(b, '__iter__'):
return vectorops.mul(b, a)
return a * b
def gen_mul(a,b): """Multiplies lists or scalars in a unified way. With lists the multiplication is elementwise.""" if isinstance(a,np.ndarray): return np.dot(a,b) elif hasattr(a,'__iter__'): return vectorops.mul(a,b) elif hasattr(b,'__iter__'): return vectorops.mul(b,a) return a*b
def get_left_dir(self, zeroZ=False):
dir = op.cross([0, 0, 1], self.get_camera_dir())
if zeroZ:
dir = (dir[0], dir[1], 0)
if op.norm(dir) > 1e-6:
dir = op.mul(dir, 1 / op.norm(dir))
return dir
def get_camera_pos(self):
cam = self.view.camera
z = math.sin(-cam.rot[1])
x = math.sin(cam.rot[2]) * math.cos(cam.rot[1])
y = math.cos(cam.rot[2]) * math.cos(cam.rot[1])
pos = [x, y, z]
return op.add(cam.tgt, op.mul(pos, cam.dist))
def segment_segment_intersection(seg1, seg2, umin=0, umax=1, vmin=0, vmax=1):
"""Given 2 2D segments (a1,b1) and (a2,b2) returns whether the portion of
seg1 in the range [umin,umax] overlaps seg2 in the range [vmin,vmax].
Returns the point of intersection or None if no intersection exists
"""
a1, b1 = seg1
a2, b2 = seg2
assert len(a1) == 2
assert len(b1) == 2
assert len(a2) == 2
assert len(b2) == 2
d = vectorops.sub(b1, a1)
a, b = vectorops.sub(a2, a1), vectorops.sub(b2, a1)
#now it's centered at a1
#u*d = a + v*(b-a)
e = vectorops.sub(b2, a2)
A = np.array([d, vectorops.mul(e, -1)]).T
try:
uv = np.dot(np.linalg.inv(A), a)
except np.linalg.linalg.LinAlgError:
return None
u, v = uv
if u < umin or u > umax:
return None
if v < umin or v > umax:
return None
return vectorops.interpolate(a1, b1, u)
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def getCommandVelocity(self, q, dq, dt):
"""Gets the command velocity from the current state of the
robot.
"""
eP = self.getSensedError(q)
#vcmd = hP*eP + hD*eV + hI*eI
vP = gen_mul(self.hP, eP)
vcmd = vP
#D term
vcur = self.getSensedVelocity(q, dq, dt)
eD = None
if vcur != None:
eD = vectorops.sub(vcur, self.dxdes)
vD = gen_mul(self.hD, eD)
vcmd = vectorops.add(vcmd, vD)
#I term
if self.eI != None:
vI = gen_mul(self.hI, self.eI)
vcmd = vectorops.add(vcmd, vI)
#print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI
#do acceleration limiting
if vcur != None:
dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt)
dvnorm = vectorops.norm(dvcmd)
if dvnorm > self.dvcmdmax:
vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt)
print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd
#do velocity limiting
vnorm = vectorops.norm(vcmd)
if vnorm > self.vcmdmax:
vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm)
print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd
return vcmd
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def printStatus(self,q):
"""Prints a status printout summarizing all tasks' errors."""
priorities = set()
names = dict()
errors = dict()
totalerrors = dict()
for t in self.taskList:
if t.weight==0: continue
priorities.add(t.level)
s = t.name
if len(s) > 8:
s = s[0:8]
err = t.getSensedError(q)
names.setdefault(t.level,[]).append(s)
errors.setdefault(t.level,[]).append("%.3f"%(vectorops.norm(err)),)
werrsq = vectorops.normSquared(vectorops.mul(err,t.weight))
totalerrors[t.level] = totalerrors.get(t.level,0.0) + werrsq
cols = 5
colwidth = 10
for p in priorities:
print "Priority",p,"weighted error^2",totalerrors[p]
pnames = names[p]
perrs = errors[p]
start = 0
while start < len(pnames):
last = min(start+cols,len(pnames))
print " Name: ",
for i in range(start,last):
print pnames[i],' '*(colwidth-len(pnames[i])),
print
print " Error: ",
for i in range(start,last):
print perrs[i],' '*(colwidth-len(perrs[i])),
print
start=last
def scaleToBounds( val, bmin, bmax, origin=None): """Cap the ray starting origin in the direction val against bounding box limits. """ if origin != None: if isinstance(bmin,np.ndarray): bmin -= origin else: bmin = vectorops.sub(bmin,origin) if isinstance(bmax,np.ndarray): bmax -= origin else: bmax = vectorops.sub(bmax,origin) return scaleToBounds(val,bmin,bmax) umax = 1.0 for vali,ai,bi in zip(val,bmin,bmax): assert bi >= ai if vali < ai: umax = ai/vali if vali > bi: umax = bi/vali if umax==1.0: return val assert umax >= 0 print "Scaling to velocity maximum, factor",umax if isinstance(val,np.ndarray): return val*umax else: return vectorops.mul(val,umax)
def _randomlyRotateCamera(self, min_r=0, max_r=70, dist=DIST_FROM_BOARD):
"""
Randomly rotates camera about x-axis and zooms out from the center of the
tabletop
:param: min_r: the minimum random rotation in degrees
:param: max_r: the maximum random rotation in degrees
:param: dist: the distance to zoom out from center of the table
:return: the angle of rotation sampled
"""
min_r = math.radians(min_r)
max_r = math.radians(max_r)
table_center = self.chessEngine.getTableCenter()
table_R = so3.from_axis_angle(([1, 0, 0], -math.pi))
table_t = table_center
rot_deg = random.uniform(min_r, max_r)
zoom_out_R = so3.from_axis_angle(([1, 0, 0], rot_deg))
zoom_out_t = vectorops.mul(
[0, math.sin(rot_deg), -math.cos(rot_deg)], dist)
xform = se3.mul((table_R, table_t), (zoom_out_R, zoom_out_t))
sensing.set_sensor_xform(self.sensor, xform)
return rot_deg
def scaleToBounds(val, bmin, bmax, origin=None):
"""Cap the ray starting origin in the direction val against bounding box limits.
"""
if origin != None:
if isinstance(bmin, np.ndarray):
bmin -= origin
else:
bmin = vectorops.sub(bmin, origin)
if isinstance(bmax, np.ndarray):
bmax -= origin
else:
bmax = vectorops.sub(bmax, origin)
return scaleToBounds(val, bmin, bmax)
umax = 1.0
for vali, ai, bi in zip(val, bmin, bmax):
assert bi >= ai
if vali < ai:
umax = ai / vali
if vali > bi:
umax = bi / vali
if umax == 1.0: return val
assert umax >= 0
print "Scaling to velocity maximum, factor", umax
if isinstance(val, np.ndarray):
return val * umax
else:
return vectorops.mul(val, umax)
def advance(self, q, dq, dt):
""" Updates internal state: accumulates iterm and updates x_last
"""
if self.weight > 0:
eP = self.getSensedError(q)
# update iterm
if self.eI == None:
self.eI = vectorops.mul(eP, dt)
else:
self.eI = vectorops.madd(self.eI, eP, dt)
einorm = vectorops.norm(self.eI)
if einorm > self.eImax:
self.eI = vectorops.mul(self.eI, self.eImax / einorm)
#update qLast
self.qLast = q
return
def cal_reactive_vel(self, pos_hand, pos_target, vel_ratio):
reactive_vel = [0.0, 0.0, 0.0]
pos_diff = vectorops.sub(tuple(pos_target), tuple(pos_hand))
pos_diff_norm = vectorops.norm(pos_diff)
if pos_diff_norm >= 0.02:
vel = vectorops.mul(pos_diff, vel_ratio)
reactive_vel = list(vel)
return reactive_vel
def advance(self, q, dq, dt): """ Updates internal state: accumulates iterm and updates x_last """ if self.weight > 0: eP = self.getSensedError(q) # update iterm if self.eI == None: self.eI = vectorops.mul(eP,dt) else: self.eI = vectorops.madd(self.eI, eP, dt) einorm = vectorops.norm(self.eI) if einorm > self.eImax: self.eI = vectorops.mul(self.eI,self.eImax/einorm) #update qLast self.qLast = q return
def simulateForceSpring(self, kP=10.0):
if not self.forceApplicationMode: return
self.sim.updateWorld()
body = self.sim.body(self.forceObject)
T = self.forceObject.getTransform()
wp = se3.apply(T, self.forceLocalPoint)
f = vectorops.mul(vectorops.sub(self.forceAnchorPoint, wp), kP)
body.applyForceAtLocalPoint(f, self.forceLocalPoint)
def get_camera_dir(self, zeroZ=False):
cam = self.view.camera
dir = op.sub(cam.tgt, self.get_camera_pos())
if zeroZ:
dir = (dir[0], dir[1], 0)
if op.norm(dir) > 1e-6:
dir = op.mul(dir, 1 / op.norm(dir))
dir[1] *= -1
return dir
def random_rotation():
"""Returns a uniformly distributed rotation matrix."""
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return so3.from_moment(m)
def add_light(properties,pos,tgt=None,color=[1.,1.,1.], \
spotlight=False,radius=15.,falloff=20.,tightness=10., \
area=0.,sample=9,adaptive=True,jitter=True):
if 'lights' not in properties:
properties['lights'] = []
if isinstance(pos, list) and isinstance(pos[0], list):
if isinstance(tgt, list) and isinstance(tgt[0], list):
for p, t in zip(pos, tgt):
add_light(properties,pos=p,tgt=t,color=color, \
spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \
area=area,sample=sample,adaptive=adaptive,jitter=jitter)
else:
for p in pos:
add_light(properties,pos=p,tgt=tgt,color=color, \
spotlight=spotlight,radius=radius,falloff=falloff,tightness=tightness, \
area=area,sample=sample,adaptive=adaptive,jitter=jitter)
else:
light_params = [list(pos), 'color', list(color)]
if spotlight:
light_params += [
'spotlight', 'radius', radius, 'falloff', falloff, 'tightness',
tightness, 'point_at', tgt
]
if area > 0.:
d = op.sub(tgt, pos)
d = op.mul(d, 1 / op.norm(d))
minId = None
for i in range(3):
if abs(d[i]) <= abs(d[(i + 1) % 3]) and abs(d[i]) <= abs(
d[(i + 2) % 3]):
minId = i
t0 = [0, 0, 0]
t0[minId] = 1.
t1 = op.cross(d, t0)
t1 = op.mul(t1, 1 / op.norm(t1))
t0 = op.cross(d, t1)
light_params += [
'area_light',
list(op.mul(t0, area)),
list(op.mul(t1, area)), sample, sample, 'adaptive', 1, 'jitter'
]
properties['lights'].append(vp.LightSource(*light_params))
def add_multiple_lights(properties,object,dist,numLight,gravity=[0,0,-9.81],tgt=None,color=[1.,1.,1.], \
spotlight=False,radius=15.,falloff=20.,tightness=10., \
area=0.,sample=9,adaptive=True,jitter=True):
"""This is a convenient function that calls add_light multiple times"""
#normalize gravity
g = op.mul(gravity, -1 / op.norm(gravity))
#compute frame
gabs = [abs(gi) for gi in g]
id = gabs.index(min(gabs))
t0 = [1. if i == id else 0. for i in range(3)]
t1 = op.cross(t0, g)
t1 = op.mul(t1, 1 / op.norm(t1))
t0 = op.cross(t1, g)
#find highest direction
bb = compute_bb(object)
ctr = op.mul(op.add(bb[0], bb[1]), 0.5)
distg = sum([abs((bb[1][i] - bb[0][i]) / 2 * g[i]) for i in range(3)])
#add each light
for i in range(numLight):
angle = math.pi * 2 * i / numLight
d0 = op.mul(g, distg)
d1 = op.mul(t0, math.sin(angle) * dist)
d2 = op.mul(t1, math.cos(angle) * dist)
add_light(properties, op.add(d0, op.add(d1,
d2)), ctr, color, spotlight,
radius, falloff, tightness, area, sample, adaptive, jitter)
def apply_tendon_forces(self,i,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(),self.tendon0_local)
p1w = se3.apply(b1.getTransform(),self.tendon1_local)
p2w = se3.apply(b2.getTransform(),self.tendon2_local)
d = vectorops.distance(p1w,p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w,p1w))
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f,100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w,p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
cosangle = vectorops.dot(straight,tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
self.forces[i][2] = vectorops.mul(direction,f)
else:
self.forces[i] = [None,None,None]
return
def init(self, scene, object, hints):
"""Needs object to contain a structured PointCloud."""
if not isinstance(object, (RigidObjectModel, Geometry3D, PointCloud)):
print(
"Need to pass an object as a RigidObjectModel, Geometry3D, or PointCloud"
)
return False
if isinstance(object, RigidObjectModel):
return self.init(scene, object.geometry(), hints)
pc = None
xform = None
if isinstance(object, Geometry3D):
pc = object.getPointCloud()
xform = object.getCurrentTransform()
else:
pc = object
xform = se3.identity()
self.pc = pc
self.pc_xform = xform
#now look through PC and find flat parts
#do a spatial hash
from collections import defaultdict
estimation_knn = 6
pts = numpy_convert.to_numpy(pc)
N = pts.shape[0]
positions = pts[:, :3]
normals = np.zeros((N, 3))
indices = (positions * (1.0 / self._gripper.opening_span)).astype(int)
pt_hash = defaultdict(list)
for i, (ind, p) in enumerate(zip(indices, positions)):
pt_hash[ind].append((i, p))
options = []
for (ind, iplist) in pt_hash.items():
if len(iplist) < estimation_knn:
pass
else:
pindices = [ip[0] for ip in iplist]
pts = [ip[1] for ip in iplist]
c, n = fit_plane_centroid(pts)
if n[2] < 0:
n = vectorops.mul(n, -1)
verticality = self.vertical_penalty(math.acos(n[2]))
var = sum(
vectorops.dot(vectorops.sub(p, c), n)**2 for p in pts)
roughness = self.roughness_penalty(var)
options.append((cn, n, verticality + roughness))
if len(options) == 0:
return False
self.options = options.sorted(key=lambda x: -x[2])
self.index = 0
return True
def getStackedVelocity(self, q, dq, dt, priority): """Formulates dx to calculate dqdes """ Vlist = [] for taski in self.taskList: if taski.weight == 0 or taski.level != priority: continue #scale by weight Vtemp = vectorops.mul(taski.getCommandVelocity(q, dq, dt),taski.weight) Vlist.append(Vtemp) if len(Vlist)==0: return None V = np.hstack(Vlist) return V
def getStackedVelocity(self, q, dq, dt, priority): """Formulates dx to calculate dqdes """ Vlist = [] for taski in self.taskList: if taski.weight == 0 or taski.level != priority: continue #scale by weight Vtemp = vectorops.mul(taski.getCommandVelocity(q, dq, dt),taski.weight) Vlist.append(Vtemp) if len(Vlist)==0: return None V = np.hstack(Vlist) return V
def advance(self, **inputs):
try:
dt = inputs["dt"]
v = inputs[self.name]
except KeyError:
raise ValueError("Input needs to have value %s and timestep 'dt'" %
(self.name, ))
if len(v) == 0: return None
if self.integral is None:
self.integral = vectorops.mul(v, dt)
else:
self.integral = vectorops.madd(self.integral, v, dt)
result = vectorops.madd(self.integral, v, -0.5 * dt)
return {'I' + self.name: result}
def setConfig(self, x, y, z, sc, tht):
self.currConfig = [x, y, z, sc, tht]
cosTht = math.cos(tht)
sinTht = math.sin(tht)
vis.lock()
pt = [x, y, z]
rotMat = mathUtils.euler_zyx_mat([tht, 0, 0])
vis.add(self.robotSystemName, [rotMat, pt])
for iR in range(self.n):
q = self.robots[iR].getConfig()
scSj = vectorops.mul(self.sj[iR], sc)
q[0] = self.currConfig[0] + cosTht * scSj[0] - sinTht * scSj[1]
q[1] = self.currConfig[1] + sinTht * scSj[0] + cosTht * scSj[1]
q[2] = self.currConfig[2] + scSj[2]
self.robots[iR].setConfig(q)
vis.unlock()
self.checkCollision()
def next(self):
"""Returns the next Grasp from the database."""
if self.options is None:
return None
if self.index >= len(self.options):
self.options = None
return None
c, n, score = self.options(self.index)
self.index += 1
cworld = se3.apply(self.pc_xform, c)
nworld = so3.apply(self.pc_xform[0], n)
objective = IKObjective()
objective.setLinks(self.gripper.link)
objective.setFixedPoint(self.gripper.center, cworld)
objective.setAxialRotConstraint(self.gripper.primary_axis,
vectorops.mul(nworld, -1))
return Grasp(objective, score=score)
def getDesiredCartesianPose(self,limb,device):
"""Returns a pair (R,t) of the desired EE pose if the limb should have
a cartesian pose message, or None if it should not.
- limb: either 'left' or 'right'
- device: an index of the haptic device
Implementation-wise, this reads from self.startTransforms and the deviceState
to determine the correct desired end effector transform. The delta
from devices[device]['deviceCurrentTransform'] to devices[device]['deviceInitialTransform']
is scaled, then offset by self.startTransforms[device]. (self.startTransforms is
the end effector transform when deviceInitialTransform is set)
"""
if limb=='left':
T = self.serviceThread.eeGetter_left.get()
else:
T = self.serviceThread.eeGetter_right.get()
if T is None:
T = se3.identity()
R,t=T
deviceState = self.haptic_client.deviceState
if deviceState == None: return T
dstate = deviceState[device]
if dstate.widgetInitialTransform[0] == None or self.startTransforms[device] == None: return T
Tcur = dstate.widgetCurrentTransform
T0 = dstate.widgetInitialTransform[0]
if T0 == None:
T0 = Tcur
#print "Button is down and mode is",dstate['mode']
#print dstate
assert T0 != None,"T0 is null"
assert Tcur != None,"Tcur is null"
relRot = so3.mul(Tcur[0],so3.inv(T0[0]))
relTrans = vectorops.sub(Tcur[1],T0[1])
#print "Rotation moment",so3.moment(relRot)
desRot = so3.mul(relRot,self.startTransforms[device][0])
desPos = vectorops.add(vectorops.mul(relTrans,hapticArmTranslationScaling),self.startTransforms[device][1])
#TEST: just use start rotation
#desRot = self.startTransforms[i][0]
return (desRot,desPos)
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world):
global lower_corner,upper_corner
resolution = (15,15,15)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
for i in range(resolution[0]):
for j in range(resolution[1]):
for k in range(resolution[2]):
orig_config = robot.getConfig()
point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner)
world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world)
local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local)
point_goal = ik.objective(end_effector, local=point_local, world=point)
if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10):
reachable[i,j,k] = 1.0
robot.setConfig(orig_config)
return reachable
def control_loop(self):
sim = self.sim
world = self.world
controller = sim.controller(0)
robotModel = world.robot(0)
t = self.ttotal
#Problem 1: tune the values in this line
if problem == "1a" or problem == "1b":
kP = 1
kI = 1
kD = 1
controller.setPIDGains([kP], [kI], [kD])
#Problem 2: use setTorque to implement a control loop with feedforward
#torques
if problem == "2a" or problem == "2b":
qcur = controller.getSensedConfig()
dqcur = controller.getSensedVelocity()
qdes = [0]
dqdes = [0]
robotModel.setConfig(qcur)
robotModel.setVelocity(dqcur)
while qcur[0] - qdes[0] > math.pi:
qcur[0] -= 2 * math.pi
while qcur[0] - qdes[0] < -math.pi:
qcur[0] += 2 * math.pi
ddq = vectorops.mul(vectorops.sub(qdes, qcur), 100.0)
ddq = vectorops.madd(ddq, vectorops.sub(dqdes, dqcur), 20.0)
#TODO: solve the dynamics equation to fill this in
T = [3]
controller.setTorque(T)
#Problem 3: drive the robot so its end effector goes
#smoothly toward the target point
if problem == "3":
target = [1.0, 0.0, 0.0]
qdes = [0.7, -2.3]
dqdes = [0, 0]
controller.setPIDCommand(qdes, dqdes)
def advance(self, **inputs):
val = inputs[self.argname]
if hasattr(val, '__iter__'):
res = vectorops.mul(val, self.b[0])
assert len(self.history) + 1 <= len(self.b)
for i, v in enumerate(self.history):
res = vectorops.madd(res, v, self.b[i + 1])
if len(self.history) + 1 < len(self.b):
res = vectorops.madd(res, self.history[-1],
sum(self.b[len(self.history) + 1:]))
else:
res = val * self.b[0]
assert len(self.history) + 1 <= len(self.b)
for i, v in enumerate(self.history):
res += v * self.b[i + 1]
if len(self.history) + 1 < len(self.b):
res += self.history[-1] * sum(self.b[len(self.history) + 1:])
#advance history
self.history.appendleft(val)
while len(self.history) >= len(self.b):
self.history.pop()
return res
def build_default_keymap(world):
"""builds a default keymape: 1234567890 increases values of DOFs 1-10
of robot 0. qwertyuiop decreases values."""
if world.numRobots() == 0:
return {}
robot = world.robot(0)
up = '1234567890'
down = 'qwertyuiop'
res = {}
for i in range(min(robot.numDrivers(), 10)):
#up velocity
vel = [0] * robot.numLinks()
if robot.driver(i).getType() == 'normal':
vel[robot.driver(i).getAffectedLink()] = 1
else:
#skip it
#links = robot.driver(i).getAffectedLinks();
continue
res[up[i]] = (0, vel)
#down velocity
vel = vectorops.mul(vel, -1)
res[down[i]] = (0, vel)
return res
def solve(self, q,dq,dt): """Takes sensed q,dq, timestep dt and returns qdes and dqdes in joint space. """ for task in self.taskList: task.updateState(q,dq,dt) # priority 1 if not hasattr(self,'timingStats'): self.timingStats = defaultdict(int) self.timingStats['count'] += 1 t1 = time.time() J1 = self.getStackedJacobian(q,dq,1) v1 = self.getStackedVelocity(q,dq,dt,1) (A,b) = self.getMotionModel(q,dq,dt) if self.activeDofs != None: A = select_cols(A,self.activeDofs) if sparse.isspmatrix_coo(A) or sparse.isspmatrix_dia(A): A = A.tocsr() t2 = time.time() self.timingStats['get jac/vel p1'] += t2-t1 J2 = self.getStackedJacobian(q,dq,2) if J2 is not None: V2 = self.getStackedVelocity(q,dq,dt,2) t3 = time.time() self.timingStats['get jac/vel p2'] += t3-t2 #compute velocity limits vmax = self.robot.getVelocityLimits() vmin = vectorops.mul(vmax,-1.0) amax = self.robot.getAccelerationLimits() vref = dq if self.ulast == None else self.ulast for i,(v,vm,am) in enumerate(zip(vref,vmin,amax)): if v-dt*am > vm: vmin[i] = v-dt*am elif v < vm: #accelerate! vmin[i] = min(vm,v+dt*am) for i,(v,vm,am) in enumerate(zip(vref,vmax,amax)): if v-dt*am < vm: vmax[i] = v+dt*am elif v > vm: #decelerate! vmax[i] = max(vm,v-dt*am) for i,(l,u) in enumerate(zip(vmin,vmax)): assert l <= u if l > 0 or u < 0: print "Moving link:",self.robot.link(i).getName(),"speed",vref[i] #print zip(vmin,vmax) Aumin = np.array(vmin) - b Aumax = np.array(vmax) - b #print zip(Aumin.tolist(),Aumax.tolist()) J1A = J1.dot(A) J1b = J1.dot(b) if J2 == None: #just solve constrained least squares #J1*(A*u+b) = v1 #vmin < A*u + b < vmax u1 = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax)[0] u2 = [0.0]*len(u1) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 else: #solve equality constrained least squares #dq = A*u + b #J1*dq = v1 #J1*A*u + J1*b = v1 #least squares solve for u1: #J1*A*u1 = v1 - J1*b #vmin < A*u1 + b < vmax #need u to satisfy #Aact*u = bact #we know that u1 satisfies Aact*u = bact #let u = u1+u2 #=> u2 = (I - Aact^+ Aact) z = N*z #least squares solve for z: #J2*A*(u1+u2+b) = v2 #J2*A*N z = v2 - J2*(A*u1+b) (u1, active, activeRhs) = constrained_lsqr(J1A,v1-J1b,A,Aumin,Aumax) Aact = sparse.vstack([J1A]+[A[crow,:] for crow in active]).todense() #bact = np.hstack((v1-J1b,activeRhs)) J1Ainv = np.linalg.pinv(Aact) dq1 = A.dot(u1)+b if len(active)>0: print "Priority 1 active constraints:" for a in active: print self.robot.link(a).getName(),vmin[a],dq1[a],vmax[a] r1 = J1.dot(dq1)-v1 print "Op space controller solve" print " Residual 1",np.linalg.norm(r1) # priority 2 N = np.eye(len(dq)) - np.dot(J1Ainv, Aact) t4 = time.time() self.timingStats['pinv jac p1'] += t4-t3 u2 = [0.0]*len(u1) #print " Initial priority 2 task error",np.linalg.norm(V2-J2.dot(dq1)) J2A = J2.dot(A) J2AN = J2A.dot(N) AN = sparse.csr_matrix(np.dot(A.todense(),N)) #Note: N destroys sparsity V2_m_resid = np.ravel(V2 - J2.dot(dq1)) (z,active,activeRhs) = constrained_lsqr(J2AN,V2_m_resid,AN,vmin-dq1,vmax-dq1) t5 = time.time() self.timingStats['ls jac p2'] += t5-t4 u2 = np.ravel(np.dot(N, z)) #debug, should be close to zero #print " Nullspace projection error:",np.linalg.norm(J1A.dot(u2)) #this is the error in the nullspace of the first priority tasks dq2 = A.dot(u2) + dq1 #debug, should be equal to residual 2 printout above print " Residual 2",np.linalg.norm(J2.dot(dq2)-V2) #debug should be close to zero #print " Residual 2 in priority 1 frame",np.linalg.norm(J1.dot(dq2)-v1) if len(active)>0: print "Priority 2 active constraints:" for a in active: print self.robot.link(a).getName(),vmin[a],dq2[a],vmax[a] #compose the velocities together u = np.ravel((u1 + u2)) dqpred = A.dot(u)+b print " Residual 1 final",np.linalg.norm(np.ravel(J1.dot(dqpred))-v1) if J2 != None: print " Residual 2 final",np.linalg.norm(np.ravel(J2.dot(dqpred))-V2) u = u.tolist() #if self.activeDofs != None: # print "dqdes:",[self.dqdes[v] for v in self.activeDofs] self.qdes = vectorops.madd(q, u, dt) self.ulast = u t6 = time.time() self.timingStats['total']+=t6-t1 if self.timingStats['count']%10==0: n=self.timingStats['count'] print "OpSpace times (ms): vel/jac 1 %.2f inv 1 %.2f vel/jac 2 %.2f inv 2 %.2f total %.2f"%(self.timingStats['get jac/vel p1']/n*1000,self.timingStats['pinv jac p1']/n*1000,self.timingStats['get jac/vel p2']/n*1000,self.timingStats['ls jac p2']/n*1000,self.timingStats['total']/n*1000) return (self.qdes,u)
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,
visualize=False,verbose=0):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Side effect: the positions of objects in world are modified
Returns:
(tuple): (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:",objects_bound)
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = _get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in range(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
if verbose:
print("Sampled position of",object.getName(),object.getTransform()[1])
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
if verbose:
print("Failed to place object",object.getName())
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
if verbose:
print("Beginning to simulate")
#start letting everything fall
for firstfall in range(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)