def workspace_interpolate(a, b, u):
"""Interpolates either in R^n or SE(3) between points a and b. If len(a)<=3,
this will do cartesian interpolation. Otherwise, len(a)==6 is required and
this will do SE(3) interpolation."""
if len(a) <= 3:
return vectorops.interpolate(a, b, u)
assert len(a) == 6, "Can only interpolate in R2, R3, or SE(3)"
Ra, Rb = so3.from_moment(a[3:]), so3.from_moment(b[3:])
R = so3.interpolate(Ra, Rb, u)
return vectorops.interpolate(a[:3], b[:3], u) + so3.moment(R)
def workspace_distance(a, b):
"""Returns a distance in R^n or SE(3) between points a and b. If len(a)<=3,
this will do cartesian distance. Otherwise, len(a)==6 is required and
this will do SE(3) distance (with a max orientation distance of 1)."""
if len(a) <= 3:
return vectorops.distance(a, b)
assert len(a) == 6, "Can only interpolate in R2, R3, or SE(3)"
dt = vectorops.distance(a[:3], b[:3])
dR = so3.distance(so3.from_moment(a[3:]), so3.from_moment(b[3:]))
return dt + dR / math.pi
def display(self):
T1 = self.taskGen.getDesiredCartesianPose('left',0)
T2 = self.taskGen.getDesiredCartesianPose('right',1)
baseTransform = self.taskGen.world.robot(0).link(2).getTransform()
glEnable(GL_LIGHTING)
if T1 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T1),0.2,0.03,fancy=True,lighting=True)
if T2 is not None:
gldraw.xform_widget(se3.mul(baseTransform,T2),0.2,0.03,fancy=True,lighting=True)
dstate = self.taskGen.serviceThread.hapticupdater.deviceState
if T1 is not None and dstate[0] != None:
R = dstate[0].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T1[1]))
self.draw_wand(T)
if T2 is not None and dstate[1] != None:
R = dstate[1].widgetCurrentTransform[0]
T = se3.mul(baseTransform,(R,T2[1]))
self.draw_wand(T)
if debugHapticTransform:
for deviceState in dstate:
#debugging
#this is the mapping from the handle to the user frame
Traw = (so3.from_moment(deviceState.rotationMoment),deviceState.position)
Tuser = se3.mul(se3.inv(handleToUser),Traw)
T = se3.mul(userToWorld,se3.mul(Tuser,worldToUser))
T = (T[0],so3.apply([0,-1,0, 0,0,1, -1,0,0],T[1]))
T = deviceToViewTransform(Traw[0],Traw[1])
gldraw.xform_widget(se3.mul(se3translation([-1,0,0]),Traw),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),Tuser),0.5,0.05,lighting=True,fancy=True)
#gldraw.xform_widget(se3.mul(se3translation([-0.5,0,0]),se3.mul(Traw,worldToHandle)),0.5,0.05,lighting=True,fancy=True)
gldraw.xform_widget(T,0.5,0.05,lighting=True,fancy=True)
break
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 compute(self, x, grad_level=0):
self.robot.setConfig(x)
R, _ = self.link.getTransform()
moment = so3.error(R, self.target_R)
# for point x
val = np.array(moment)
if grad_level == 1:
rel_R = so3.from_moment(moment)
J1 = self.robot.orientationJacobian(self.linkid)
m_omega = np.array([
MomentDerivative(moment, rel_R, [1, 0, 0]),
MomentDerivative(moment, rel_R, [0, 1, 0]),
MomentDerivative(moment, rel_R, [0, 0, 1])
]).T
jac = m_omega.dot(J1)
return (val, jac)
return (val, )
def compute(self, x, grad_level=0):
self.robot.setConfig(x)
# p0 = self.link.getWorldPosition([0, 0, 0])
R, p0 = self.link.getTransform()
moment = so3.error(R, self.target_R)
# for point x
val = np.concatenate((np.array(p0) - self.p0, moment))
if grad_level == 1:
rel_R = so3.from_moment(moment)
J0 = self.robot.positionJacobian(self.linkid, lcl_pos=[0, 0, 0])
J1 = self.robot.orientationJacobian(self.linkid)
m_omega = np.array([
MomentDerivative(moment, rel_R, [1, 0, 0]),
MomentDerivative(moment, rel_R, [0, 1, 0]),
MomentDerivative(moment, rel_R, [0, 0, 1])
]).T
jac = np.r_[J0, m_omega.dot(J1)]
return (val, jac)
return (val, )
def display(self):
if self.configs == None:
self.drawConfig(None)
else:
for q in self.configs:
self.drawConfig(q)
if self.walk_workspace_path != None:
if self.temp_config:
glEnable(GL_BLEND)
#glDisable(GL_DEPTH_TEST)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(1, 0, 0, 0.5)
self.drawConfig(self.temp_config)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(0.5, 0.5, 0.5, 1)
#glEnable(GL_DEPTH_TEST)
glDisable(GL_BLEND)
glColor3f(1, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
for w in self.walk_workspace_path.milestones:
if len(w) == 2:
glVertex2f(w[0], w[1])
else:
glVertex3f(w[0], w[1], w[2])
glEnd()
glLineWidth(1.0)
#draw workspace graph
if self.drawWorkspaceRoadmap:
def drawWorkspaceRoadmap(orientation=None):
G = self.resolution.Gw
if orientation is not None:
Rclosest, G = self.resolution.extractOrientedSubgraph(
orientation)
print G.number_of_nodes()
if not self.resolution.hasResolution():
glDisable(GL_LIGHTING)
glPointSize(5.0)
glColor3f(1, 0, 0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
for (i, j) in G.edges_iter():
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
else:
#maxn = max(len(d['qlist']) for n,d in G.nodes_iter(data=True))
#maxe = max(len(d['qlist']) for i,j,d in G.edges_iter(data=True))
#maxn = max(maxn,1)
#maxe = max(maxe,1)
glDisable(GL_LIGHTING)
glPointSize(5.0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
if not self.resolution.isResolvedNode(n):
continue
#u = float(len(d['qlist']))/float(maxn)
#nsubset = sum(1 for iq in d['qlist'] if iq in self.rr.Qsubgraph)
#if nsubset > 1:
# glColor3f(1,0,1)
#else:
# glColor3f(u,nsubset*0.5,0)
glColor3f(1, 0.5, 0)
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glBegin(GL_LINES)
for (i, j, d) in G.edges_iter(data=True):
if not self.resolution.isResolvedNode(
i) or not self.resolution.isResolvedNode(j):
continue
#nsubset = sum(1 for (ia,ib) in d['qlist'] if (ia in self.Qsubgraph and ib in self.Qsubgraph))
#u = float(len(d['qlist']))/float(maxe)
r, g, b = 1, 1, 0
if not self.resolution.isResolvedEdge(i, j):
r, g, b = 1, 0, 1
else:
qd = self.robot.distance(G.node[i]['config'],
G.node[j]['config'])
wd = workspace_distance(G.node[i]['params'],
G.node[j]['params'])
u = 1.0 - math.exp(-max(0.0, 2.0 * qd / wd - 1.0))
if u > 0.8:
r, g, b = 1, 1.0 - u * 5.0, 0.0
else:
r, g, b = u / 0.8, 1, 0.0
#assert (nsubset >=1) == self.resolution.isResolvedEdge(i,j),"Mismatch between Qsubgraph and resolution?"
glColor3f(r, g, b)
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
"""
glEnable(GL_LIGHTING)
q0 = self.robot.getConfig()
for iw,d in self.rr.Gw.nodes_iter(data=True):
qs = [iq for iq in d['qlist'] if iq in self.rr.Qsubgraph]
if len(qs) > 1:
for iq in qs:
self.robot.setConfig(self.rr.Gq.node[iq]['config'])
self.robot.drawGL()
self.robot.setConfig(q0)
glDisable(GL_LIGHTING)
"""
if len(self.resolution.domain[0]) == 6:
if not hasattr(self, 'roadmapDisplayLists_by_orientation'):
self.roadmapDisplayLists_by_orientation = dict()
self.lastROrientation = None
self.lastRMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastROrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastRMbest = mbest
else:
mbest = self.lastRMbest
if mbest not in self.roadmapDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.roadmapDisplayLists_by_orientation[
mbest] = CachedGLObject()
orientation = so3.from_moment(mbest)
self.roadmapDisplayLists_by_orientation[mbest].draw(
drawWorkspaceRoadmap, args=(orientation, ))
else:
#there looks to be a bug in GL display lists for drawing lines...
#self.roadmapDisplayList.draw(drawWorkspaceRoadmap)
drawWorkspaceRoadmap()
else:
#render boundaries only
def drawDisconnections(orientation=None):
bmin, bmax = self.resolution.domain
active = [
i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a
]
if self.useboundary == None:
self.useboundary = (len(active) == 2)
verts, faces = self.resolution.computeDiscontinuities(
self.useboundary, orientation)
if len(active) == 3:
glEnable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glDisable(GL_CULL_FACE)
#if self.walk_workspace_path != None:
# gldraw.setcolor(1,0,1,0.25)
#else:
# gldraw.setcolor(1,0,1,0.5)
for tri in faces:
tverts = [verts[v] for v in tri]
centroid = vectorops.div(vectorops.add(*tverts),
len(tri))
params = [
(x - a) / (b - a)
for (x, a,
b) in zip(centroid, self.resolution.domain[0],
self.resolution.domain[1])
]
if self.walk_workspace_path != None:
gldraw.setcolor(params[0], params[1], params[2],
0.25)
else:
gldraw.setcolor(params[0], params[1], params[2],
1.0)
gldraw.triangle(*tverts)
glEnable(GL_CULL_FACE)
else:
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(1, 0, 1, 0.5)
glLineWidth(4.0)
glBegin(GL_LINES)
for s in faces:
spts = verts[s[0]], verts[s[1]]
glVertex3f(spts[0][0], 0, spts[0][1])
glVertex3f(spts[1][0], 0, spts[1][1])
glEnd()
glLineWidth(1.0)
#draw stabbing lines
"""
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
glBegin(GL_LINES)
for (i,j,d) in self.resolution.Gw.edges_iter(data=True):
if self.resolution.isResolvedEdge(i,j):
continue
if not self.resolution.isResolvedNode(i) or not self.resolution.isResolvedNode(j):
continue
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[i]['params']))
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[j]['params']))
glEnd()
"""
if len(self.resolution.domain[0]) == 6:
if not hasattr(self,
'disconnectionDisplayLists_by_orientation'):
self.disconnectionDisplayLists_by_orientation = dict()
self.lastOrientation = None
self.lastMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastOrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastMbest = mbest
else:
mbest = self.lastMbest
if mbest not in self.disconnectionDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.disconnectionDisplayLists_by_orientation[
mbest] = CachedGLObject()
orientation = so3.from_moment(mbest)
self.disconnectionDisplayLists_by_orientation[mbest].draw(
drawDisconnections, args=(orientation, ))
else:
self.disconnectionDisplayList.draw(drawDisconnections)
bmin, bmax = self.resolution.domain
#draw workspace bound
#glDisable(GL_LIGHTING)
#glColor3f(1,0.5,0)
#gldraw.box(bmin[:3],bmax[:3],lighting=False,filled=False)
GLBaseClass.display(self)
def expand_se3(x):
m = x[3:]
return x[:3] + vectorops.mul(so3.from_moment(m), 0.3)
def onMessage(self, msg):
#print "Getting haptic message"
#print msg
self.numMessages += 1
if msg['type'] != 'MultiHapticState':
print "Strange message type", msg['type']
return
if len(self.deviceState) == 0:
print "Adding", len(msg['devices']) - len(
self.deviceState), "haptic devices"
while len(self.deviceState) < len(msg['devices']):
self.deviceState.append(HapticDeviceState())
if len(msg['devices']) != len(self.deviceState):
print "Incorrect number of devices in message:", len(
msg['devices'])
return
#read button presses
for dstate in self.deviceState:
dstate.buttonPressed[0] = dstate.buttonPressed[1] = False
dstate.buttonReleased[0] = dstate.buttonReleased[1] = False
if 'events' in msg:
for e in msg['events']:
#print e['type']
if e['type'] == 'b1_down':
dstate = self.deviceState[e['device']]
dstate.buttonPressed[0] = True
elif e['type'] == 'b2_down':
dstate = self.deviceState[e['device']]
dstate.buttonPressed[1] = True
elif e['type'] == 'b1_up':
dstate = self.deviceState[e['device']]
dstate.buttonReleased[0] = True
elif e['type'] == 'b2_up':
dstate = self.deviceState[e['device']]
dstate.buttonReleased[1] = True
for i in range(len(self.deviceState)):
dmsg = msg['devices'][i]
dstate = self.deviceState[i]
if dstate.time == dmsg['time']:
#no update...
continue
dstate.newupdate = True
# ===== read from msg =====
dstate.position = dmsg['position']
dstate.rotationMoment = dmsg['rotationMoment']
dstate.velocity = dmsg['velocity']
dstate.angularVelocity = dmsg['angularVelocity']
dstate.jointAngles = dmsg['jointAngles']
#dstate.gimbalAngle = dmsg['gimbalAngle']
oldTime = dstate.time
dstate.time = dmsg['time']
#print "position",dmsg['position']
#print "rotation moment",dmsg['rotationMoment']
#print "angular velocity",dmsg['angularVelocity']
dstate.widgetCurrentTransform = deviceToWidgetTransform(
so3.from_moment(dmsg['rotationMoment']), dmsg['position'])
if dmsg['button1'] == 1:
oldButtonDown = dstate.buttonDown[0]
dstate.buttonDown[0] = True
if not oldButtonDown:
dstate.widgetInitialTransform[
0] = dstate.widgetPreviousTransform
continue
else:
dstate.buttonDown[0] = False
if dmsg['button2'] == 1:
oldButtonDown = dstate.buttonDown[1]
dstate.buttonDown[1] = True
if not oldButtonDown:
dstate.widgetInitialTransform[
1] = dstate.widgetPreviousTransform
continue
else:
dstate.buttonDown[1] = False
newTime = dstate.time
if newTime != oldTime and dstate.widgetPreviousTransform is not None:
# print "previous position = ", dstate.widgetPreviousTransform[1]
# print "current position = ", dstate.widgetCurrentTransform[1]
timeInterval = newTime - oldTime
#print "========================"
#print "time = ", timeInterval
delta_Pos = vectorops.sub(dstate.widgetCurrentTransform[1],
dstate.widgetPreviousTransform[1])
vel = vectorops.div(delta_Pos, timeInterval)
delta_Moment = so3.moment(
so3.mul(dstate.widgetCurrentTransform[0],
so3.inv(dstate.widgetPreviousTransform[0])))
angvel = vectorops.div(delta_Moment, timeInterval)
# print "vel = [%2.4f %2.4f %2.4f]" % (vel[0], vel[1], vel[2])
# print "angvel = [%2.4f %2.4f %2.4f]" % (angvel[0], angvel[1], angvel[2])
dstate.linearVel = list(vel)
dstate.angularVel = list(angvel)
#end of loop, store previous transform
dstate.widgetPreviousTransform = dstate.widgetCurrentTransform
def drive(self, qcur, angVel, vel, dt):
"""Drives the robot by an incremental time step to reach the desired
Cartesian (angular/linear) velocities of the links previously specified
in start().
Args:
qcur (list of floats): The robot's current configuration.
angVel (3-vector or list of 3-vectors): the angular velocity of
each driven link. Set angVel to None to turn off orientation
control of every constraint. angVel[i] to None to turn off
orientation control of constraint i.
vel (3-vector or list of 3-vectors): the linear velocity of each
driven link. Set vel to None to turn off position control of
every constraint. Set vel[i] to None to turn off position
control of constraint i.
dt (float): the time step.
Returns:
tuple: A pair ``(progress,qnext)``. ``progress`` gives a value
in the range [0,1] indicating indicating how far along the nominal
drive amount the solver was able to achieve. If the result is < 0,
this indicates that the solver failed to make further progress.
``qnext`` is the resulting configuration that should be sent to the
robot's controller.
For longer moves, you should pass qnext back to this function as qcur.
"""
if angVel is None:
#turn off orientation control
if vel is None:
#nothing specified
return (1.0, qcur)
angVel = [None] * len(self.links)
else:
assert len(angVel) > 0
if not hasattr(angVel[0], '__iter__'):
angVel = [angVel]
if vel is None:
#turn off position control
vel = [None] * len(self.links)
else:
assert len(vel) > 0
if not hasattr(vel[0], '__iter__'):
vel = [vel]
if len(qcur) != self.robot.numLinks():
raise ValueError(
"Invalid size of current configuration, %d != %d" %
(len(qcur), self.robot.numLinks()))
if len(angVel) != len(self.links):
raise ValueError(
"Invalid # of angular velocities specified, %d != %d" %
(len(angVel), len(self.links)))
if len(vel) != len(self.links):
raise ValueError(
"Invalid # of translational velocities specified, %d != %d" %
(len(vel), len(self.links)))
anyNonzero = False
zeroVec = [0, 0, 0]
for (w, v) in zip(angVel, vel):
if w is not None and list(w) != zeroVec:
anyNonzero = True
break
if v is not None and list(v) != zeroVec:
anyNonzero = True
break
if not anyNonzero:
return (1.0, qcur)
qout = [v for v in qcur]
#update drive transforms
self.robot.setConfig(qcur)
robotLinks = [self.robot.link(l) for l in self.links]
#limit the joint movement by joint limits and velocity
tempqmin, tempqmax = self.robot.getJointLimits()
if self.qmin is not None:
tempqmin = self.qmin
if self.qmax is not None:
tempqmax = self.qmax
vmax = self.robot.getVelocityLimits()
vmin = vectorops.mul(vmax, -1)
if self.vmin is not None:
vmin = self.vmin
if self.vmax is not None:
vmax = self.vmax
for i in range(self.robot.numLinks()):
tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i])
tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i])
#Setup the IK solver parameters
self.solver.setJointLimits(tempqmin, tempqmax)
tempGoals = [ik.IKObjective(g) for g in self.ikGoals]
for i in range(len(self.links)):
if math.isfinite(self.positionTolerance) and math.isfinite(
self.rotationTolerance):
tempGoals[i].rotationScale = self.positionTolerance / (
self.positionTolerance + self.rotationTolerance)
tempGoals[i].positionScale = self.rotationTolerance / (
self.positionTolerance + self.rotationTolerance)
elif not math.isfinite(
self.positionTolerance) and not math.isfinite(
self.rotationTolerance):
pass
else:
tempGoals[i].rotationScale = min(
self.positionTolerance,
self.rotationTolerance) / self.rotationTolerance
tempGoals[i].positionScale = min(
self.positionTolerance,
self.rotationTolerance) / self.positionTolerance
tolerance = min(
1e-6,
min(self.positionTolerance, self.rotationTolerance) /
(math.sqrt(3.0) * len(self.links)))
self.solver.setTolerance(tolerance)
#want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t)
#where TPath_i(t) = Advance(Tdrive_i,Screw_i*t)
if self.driveSpeedAdjustment == 0:
self.driveSpeedAdjustment = 0.1
anyFailures = False
while self.driveSpeedAdjustment > 0:
#advance the desired cartesian goals
#set up IK parameters: active dofs, IKGoal
amount = dt * self.driveSpeedAdjustment
desiredTransforms = [[None, None] for i in range(len(self.links))]
for i in range(len(self.links)):
if vel[i] is not None:
desiredTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], amount)
tempGoals[i].setFixedPosConstraint(
self.endEffectorOffsets[i], desiredTransforms[i][1])
else:
tempGoals[i].setFreePosition()
if angVel[i] is not None:
increment = so3.from_moment(
vectorops.mul(angVel[i], amount))
desiredTransforms[i][0] = so3.mul(
increment, self.driveTransforms[i][0])
tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0])
else:
tempGoals[i].setFreeRotConstraint()
self.solver.set(i, tempGoals[i])
err0 = self.solver.getResidual()
quality0 = vectorops.norm(err0)
res = self.solver.solve()
q = self.robot.getConfig()
activeDofs = self.solver.getActiveDofs()
for k in activeDofs:
if q[k] < tempqmin[k] or q[k] > tempqmax[k]:
#the IK solver normalizer doesn't care about absolute
#values for joints that wrap around 2pi
if tempqmin[k] <= q[k] + 2 * math.pi and q[
k] + 2 * math.pi <= tempqmax[k]:
q[k] += 2 * math.pi
elif tempqmin[k] <= q[k] - 2 * math.pi and q[
k] - 2 * math.pi <= tempqmax[k]:
q[k] -= 2 * math.pi
else:
print(
"CartesianDriveSolver: Warning, result from IK solve is out of bounds: index",
k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k])
q[k] = max(min(q[k], tempqmax[k]), tempqmin[k])
self.robot.setConfig(q)
#now evaluate quality of the solve
errAfter = self.solver.getResidual()
qualityAfter = vectorops.norm(errAfter)
if qualityAfter > quality0:
#print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter)
#print(" solver result was",res,"increment",amount)
res = False
elif qualityAfter < quality0 - 1e-8:
res = True
if res:
#success!
for k in activeDofs:
qout[k] = q[k]
assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k]
#now advance the driven transforms
#figure out how much to drive along screw
numerator = 0 # this will get sum of distance * screws
denominator = 0 # this will get sum of |screw|^2 for all screws
#result will be numerator / denominator
achievedTransforms = [
(l.getTransform()[0], l.getWorldPosition(ofs))
for l, ofs in zip(robotLinks, self.endEffectorOffsets)
]
#TODO: get transforms relative to baseLink
for i in range(len(self.links)):
if vel[i] is not None:
trel = vectorops.sub(achievedTransforms[i][1],
self.driveTransforms[i][1])
vellen = vectorops.norm(vel[i])
axis = vectorops.div(vel[i], max(vellen, 1e-8))
ut = vellen
tdistance = vectorops.dot(trel, axis)
tdistance = min(max(tdistance, 0.0), dt * vellen)
numerator += ut * tdistance
denominator += ut**2
if angVel[i] is not None:
Rrel = so3.mul(achievedTransforms[i][0],
so3.inv(self.driveTransforms[i][0]))
vellen = vectorops.norm(angVel[i])
angVelRel = so3.apply(
so3.inv(self.driveTransforms[i][0]), angVel[i])
rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8))
Rdistance = axis_rotation_magnitude(Rrel, rotaxis)
Rdistance = min(max(Rdistance, 0.0), dt * vellen)
uR = vellen
numerator += uR * Rdistance
denominator += uR**2
distance = numerator / max(denominator, 1e-8)
#computed error-minimizing distance along screw motion
for i in range(len(self.links)):
if vel[i] is not None:
self.driveTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], distance)
else:
self.driveTransforms[i][1] = achievedTransforms[i][1]
if angVel[i] is not None:
Rincrement = so3.from_moment(
vectorops.mul(angVel[i], distance))
self.driveTransforms[i][0] = normalize_rotation(
so3.mul(Rincrement, self.driveTransforms[i][0]))
else:
self.driveTransforms[i][0] = achievedTransforms[i][0]
if math.ceil(distance / dt * 10) < math.floor(
self.driveSpeedAdjustment * 10):
self.driveSpeedAdjustment -= 0.1
self.driveSpeedAdjustment = max(self.driveSpeedAdjustment,
0.0)
elif not anyFailures:
#increase drive velocity
if self.driveSpeedAdjustment < 1.0:
self.driveSpeedAdjustment += 0.1
self.driveSpeedAdjustment = min(
1.0, self.driveSpeedAdjustment)
return (distance / dt, qout)
else:
#IK failed: try again with a slower speed adjustment
anyFailures = True
self.driveSpeedAdjustment -= 0.1
if self.driveSpeedAdjustment <= 1e-8:
self.driveSpeedAdjustment = 0.0
break
#no progress
return (-1, qcur)
