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 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 insideGeometryBB(self, point, bb, bbMargin):
#t1 = time.time()
mins = vectorops.sub(bb[0], bbMargin)
maxes = vectorops.sub(bb[1], -bbMargin)
#print "bounding box checked first two lines in time: ",time.time()-t1
insideX = point[0] >= mins[0] and point[0] <= maxes[0]
insideY = point[1] >= mins[1] and point[1] <= maxes[1]
insideZ = point[2] >= mins[2] and point[2] <= maxes[2]
#print "bounding box checked in time: ",time.time()-t1
#print maxes[0], maxes[1]
#print "point:", point[0], point[1]
return insideX and insideY and insideZ
def point_fit_xform_3d(apts,bpts):
"""Finds a 3D rigid transform that maps the list of points apts to the
list of points bpts. Return value is a klampt.se3 element that
minimizes the sum of squared errors ||T*ai-bi||^2.
"""
assert len(apts)==len(bpts)
ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
arel = [vectorops.sub(a,ca) for a in apts]
brel = [vectorops.sub(b,cb) for b in bpts]
R = point_fit_rotation_3d(arel,brel)
#R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
t = so3.sub(cb,so3.apply(R,ca))
return (R,t)
def drawGL(self):
#draw tendons
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glLineWidth(4.0)
glColor3f(0, 1, 1)
glBegin(GL_LINES)
for i in range(3):
b1 = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
b2 = self.sim.body(
self.model.robot.link(self.model.distal_links[i]))
glVertex3f(*se3.apply(b1.getTransform(), self.tendon1_local))
glVertex3f(*se3.apply(b2.getTransform(), self.tendon2_local))
if self.ext_forces[i]:
glColor3f(0, 1, 0)
b = self.sim.body(
self.model.robot.link(self.model.proximal_links[i]))
print "Applying ext force", self.EXT_F, "to", i
glVertex3f(*se3.apply(b.getTransform(), self.EXT_F_DISP))
glVertex3f(
*se3.apply(b.getTransform(),
vectorops.sub(self.EXT_F_DISP, self.EXT_F)))
glEnd()
glLineWidth(1)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
def output_and_advance(self, **inputs):
try:
q = inputs['q']
dq = inputs['dq']
u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt'])
except KeyError:
print "Warning, cannot debug motion model, dq or dqcmd not in input"
return None
if self.dqpredlast != None:
if self.activeDofs != None:
dq = dq[:]
for i in [
i for i in range(len(q)) if i not in self.activeDofs
]:
dq[i] = self.dqpredlast[i]
#compare motion model to dq
print "Motion model error:", np.linalg.norm(self.dqpredlast -
np.array(dq))
(v, i) = max(
zip(
np.abs(self.dqpredlast - np.array(dq)).tolist(),
range(len(dq))))
print " Max error:", v, "at", i,
if self.robot != None: print self.robot.getLink(i).getName()
else: print
print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[
i], "Actual:", dq[i]
print " pred:", self.Alast[i, i], "*u +", self.blast[i]
def taskDifference(self, a, b):
if self.taskType == 'po':
return se3.error(a, b)
elif self.taskType == 'position':
return vectorops.sub(a, b)
elif self.taskType == 'orientation':
return so3.error(a, b)
else:
raise ValueError("Invalid taskType " + self.taskType)
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 sphere_collision_tests(x, radius, grid):
bmin = vectorops.sub(x, [radius] * len(x))
bmax = vectorops.add(x, [radius] * len(x))
imin = [max(0, int(v)) for v in bmin]
imax = [min(int(v), b - 1) for v, b in zip(bmax, workspace_size)]
for i in range(imin[0], imax[0]):
for j in range(imin[1], imax[1]):
for k in range(imin[2], imax[2]):
if vectorops.distanceSquared((i, j, k), x) < radius**2:
yield (i, j, k)
return
def control_loop(self):
sim = self.sim
world = self.world
controller = sim.getController(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 apply_tendon_forces(self,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 100000.0
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
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)
b1.applyForceAtPoint(vectorops.mul(direction,f),p1w)
b2.applyForceAtPoint(vectorops.mul(direction,-f),p2w)
else:
pass
def keyboardfunc(self, key, x, y):
h = 0.932
targets = {
'a': (0.35, 0.25, h),
'b': (0.35, 0.05, h),
'c': (0.35, -0.1, h),
'd': (0.35, -0.1, h)
}
if key in targets:
dest = targets[key]
shift = vectorops.sub(dest, self.Tstart[1])
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.hand = Hand('l')
#plan transit path to grasp object
self.transitPath = planTransit(self.world, 0, self.hand)
if self.transitPath:
#plan transfer path
self.robot.setConfig(self.transitPath[-1])
self.transferPath = planTransfer(self.world, 0, self.hand,
shift)
if self.transferPath:
self.Tgoal = graspedObjectTransform(
self.robot, self.hand, self.transferPath[0],
self.Tstart, self.transferPath[-1])
#plan free path to retract arm
self.robot.setConfig(self.transferPath[-1])
self.object.setTransform(*self.Tgoal)
self.retractPath = planFree(self.world, self.hand,
self.qstart)
#reset the animation
self.animationTime = self.ttotal
glutPostRedisplay()
if key == 'n':
print "Moving to next action"
if self.transitPath and self.transferPath and self.retractPath:
#update start state
self.qstart = self.retractPath[-1]
self.Tstart = self.Tgoal
self.robot.setConfig(self.qstart)
self.object.setTransform(*self.Tstart)
self.transitPath = None
self.transferPath = None
self.hand = None
glutPostRedisplay()
def advance(self, **inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q) == 0: return
print len(qcmd), len(q)
u = vectorops.sub(qcmd, q)
u = vectorops.div(u, dt)
print "u estimate:", u
print "u estimate rarm:", u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key", e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast, self.dqlast, u, q, dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
def nextState(self, x, u):
pos = [x[0], x[1]]
fwd = [math.cos(x[2]), math.sin(x[2])]
right = [-fwd[1], fwd[0]]
phi = u[1]
d = u[0]
if abs(phi) < 1e-8:
newPos = vectorops.madd(pos, fwd, d)
return newpos + [x[2]]
else:
#rotate about a center of rotation, with radius 1/phi
cor = vectorops.madd(pos, right, 1.0 / phi)
sign = cmp(d * phi, 0)
d = abs(d)
phi = abs(phi)
theta = 0
thetaMax = d * phi
newpos = vectorops.add(
so2.apply(sign * thetaMax, vectorops.sub(pos, cor)), cor)
return newpos + [so2.normalize(x[2] + sign * thetaMax)]
def diff(x, y):
return vectorops.sub(x, y)
def difference(self,a,b):
"""For Lie groups, returns a difference vector that, when integrated
would get to a from b. In Cartesian spaces it is a-b."""
return vectorops.sub(a,b)
def getVector(p1, p2):
'''
Get vector formed by two points
'''
return vectorops.sub(p1, p2)
def interpolate_linear(a, b, u):
"""Interpolates linearly in cartesian space between a and b."""
return vectorops.madd(a, vectorops.sub(b, a), u)
def onMessage(self, msg):
global deviceState, defaultDeviceState
#print "Getting haptic message"
#print msg
if msg['type'] != 'MultiHapticState':
print "Strange message type", msg['type']
return
if len(deviceState) == 0:
print "Adding", len(
msg['devices']) - len(deviceState), "haptic devices"
while len(deviceState) < len(msg['devices']):
deviceState.append(defaultDeviceState.copy())
if len(msg['devices']) != len(deviceState):
print "Incorrect number of devices in message:", len(
msg['devices'])
return
#change overall state depending on button 2
if 'events' in msg:
for e in msg['events']:
#print e['type']
if e['type'] == 'b2_down':
dstate = deviceState[e['device']]
toggleMode(dstate)
print 'Changing device', e['device'], 'to state', dstate[
'mode']
for i in range(len(deviceState)):
dmsg = msg['devices'][i]
dstate = deviceState[i]
if dmsg['button1'] == 1:
#drag widget if button 1 is down
oldButtonDown = dstate['buttonDown']
dstate['buttonDown'] = True
#moving
if dstate['mode'] == 'normal':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
if self.widgetGetters == None:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
else:
Twidget = self.widgetGetters[i]()
if Twidget != None:
dstate['moveStartWidgetTransform'] = Twidget
else:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
#================================================================================================
# #perform transformation of widget
# deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position'])
# #compute relative motion
# Tdev = se3.mul(deviceToWidgetTransform,deviceTransform)
# TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform'])
# relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1]))
# #scale relative motion
# Rrel,trel = relDeviceTransform
# wrel = so3.moment(Rrel)
# wrel = vectorops.mul(wrel,dstate['rotationScale'])
# trel = vectorops.mul(trel,dstate['positionScale'])
# #print "Moving",trel,"rotating",wrel
# relDeviceTransform = (so3.from_moment(wrel),trel)
# #perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
#================================================================================================
#- perform transformation of widget
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
# delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1])
# print "haptic moves: ", delta_device
delta_device_R_matrix = so3.mul(
deviceTransform[0],
so3.inv(dstate['moveStartDeviceTransform'][0]))
# delta_device_R = so3.moment(delta_device_R_matrix)
# norm_delta_R = vectorops.norm(delta_device_R)
# if norm_delta_R != 0:
# vec_delta_R = vectorops.div(delta_device_R, norm_delta_R)
# else:
# vec_delta_R = [0,0,0]
#
# print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R
#- compute relative motion
Tdev = se3.mul(deviceToBaxterBaseTransform,
deviceTransform)
TdevStart = se3.mul(deviceToBaxterBaseTransform,
dstate['moveStartDeviceTransform'])
# delta_widget = vectorops.sub(Tdev[1],TdevStart[1])
# print "Baxter moves: ", delta_widget
# delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0]))
# delta_widget_R = so3.moment(delta_widget_R_matrix)
# norm_widget_R = vectorops.norm(delta_widget_R)
# if norm_widget_R != 0:
# vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R)
# else:
# vec_widget_R = [0,0,0]
# print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R
relDeviceTransform = (so3.mul(Tdev[0],
so3.inv(TdevStart[0])),
vectorops.sub(Tdev[1], TdevStart[1]))
#- scale relative motion
Rrel, trel = relDeviceTransform
wrel = so3.moment(Rrel)
wrel = vectorops.mul(wrel, dstate['rotationScale'])
trel = vectorops.mul(trel, dstate['positionScale'])
#- print "Moving",trel,"rotating",wrel
relDeviceTransform = (so3.from_moment(wrel), trel)
#- perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
dstate['widgetTransform'] = (
so3.mul(dstate['moveStartWidgetTransform'][0],
relDeviceTransform[0]),
vectorops.add(dstate['moveStartWidgetTransform'][1],
relDeviceTransform[1]))
#================================================================================================
elif dstate['mode'] == 'scaling':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
#perform scaling
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
oldDeviceTransform = dstate['moveStartDeviceTransform']
znew = deviceTransform[1][1]
zold = oldDeviceTransform[1][1]
posscalerange = [1e-2, 20]
rotscalerange = [0.5, 2]
newposscale = min(
max(
dstate['positionScale'] * math.exp(
(znew - zold) * 10), posscalerange[0]),
posscalerange[1])
newrotscale = min(
max(
dstate['rotationScale'] * math.exp(
(znew - zold) * 4), rotscalerange[0]),
rotscalerange[1])
if any(newposscale == v for v in posscalerange) or any(
newrotscale == v for v in rotscalerange):
pass #dont change the scale
else:
dstate['positionScale'] = newposscale
dstate['rotationScale'] = newrotscale
print "Setting position scale to", dstate[
'positionScale'], "rotation scale to", dstate[
'rotationScale']
#update start device transform
dstate['moveStartDeviceTransform'] = deviceTransform
elif dstate['mode'] == 'gripper':
print "Gripper mode not available"
dstate['mode'] = 'normal'
dstate['buttonDown'] = False
else:
dstate['buttonDown'] = False
if self.viewer: self.viewer.update(deviceState)
else: print "No viewer..."
def interpolate(a, b, u):
"""Interpolates linearly between a and b"""
return vectorops.madd(a, vectorops.sub(b, a), u)
def taskDifference(self, a, b):
"""Default: assumes a Cartesian space"""
return vectorops.sub(a, b)
