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)