Source code for klampt.control.cartesian_drive

from klampt.model import ik
from klampt.math import vectorops,so3,se3
import math

[docs]class CartesianDriveSolver: """Allows continuous, smooth Cartesian commands to be sent to a robot that is only joint-controlled. This is a highly configurable and reliable solver suitable for long-duration, precise cartesian motions. Specifically, it handles subtleties with out-of-range commands, singularities, IK solve tolerances, and oscillations. Most of the solver is stateless except for ``driveTransforms`` and ``driveSpeedAdjustment``. Tests on an Intel i7 show that this can be run for 1 6DOF IK goal at ~200Hz in Python with default settings. Reducing the max # of iterations or increasing the tolerance can help with running time. There is also a C++ version in KrisLibrary/robotics/CartesianDrive.h if you need to squeak out extra performance. It includes some perforamcne enhancements, like avoiding forward kinematics updates for non-active joints. Attributes: robot (RobotModel or SubRobotModel): the robot that will be used. positionTolerance (float): IK position tolerance, default 1e-4 rotationTolerance (float): IK orientation tolerance, default 1e-3 links (list of ints): set of end effectors in the IK constraints. baseLinks (list of ints): if given, IK constraints will be treated relative to these links endEffectorOffsets (list of 3-vectors): the tool coordinates (local positions) for each constraint. Must be of size len(links). driveTransforms (list of se3 elements): The current cartesian drive goals. Must be of size len(links). ikGoals (list of IKObjectives): If set, must be of size len(links). The solver updates the end effector DOFs selected for by these goals. If not set, uses fixed position/orientation goals. qmin (list of floats, optional): If set, overrides the robot's default joint limits. qmax (list of floats, optional): If set, overrides the robot's default joint limits. vmin (list of floats, optional): If set, overrides the robot's default velocity limits. vmax (list of floats, optional): If set, overrides the robot's default velocity limits. solver (IKSolver): the IK solver used. The user can configure parameters, e.g., the IK solve tolerance, active DOFs, joint limits, etc. driveSpeedAdjustment (float): internal speed adjustment in the range [0,1]. This helps keep the # of iterations consistent and low per time step. Usage:: qstart = controller.getCommandedConfig() solver = CartesianDriveSolver(robot) solver.start(qstart,5) #initializes with 1 link qcur = qstart while not done: wdes,vdes = [desired angular and translational velocities] progress,qcmd =,wdes,vdes,dt) if progress <= 0: print("Stopped") controller.setCommandedConfig(qcmd) qcur = qcmd """ def __init__(self,robot): self.robot = robot self.positionTolerance = 1e-4 self.rotationTolerance = 1e-3 self.ikSolveTolerance = 0 self.driveSpeedAdjustment = 1.0 self.links = None self.baseLinks = None self.endEffectorOffsets = None self.driveTransforms = None self.ikGoals = None self.qmin = None self.qmax = None self.vmin = None self.vmax = None self.solver = None
[docs] def start(self,q,links,baseLinks=None,endEffectorPositions=None): """Configures the IK solver with some set of links After start() is called, you can set up additional settings for the IK solver by calling :meth:`setTolerance`, :meth:`setMaxIters`, :meth:`setActiveDofs`. Args: q (list of floats): the robot's current configuration. links (int, str, list of int, or list of str): the link or links to drive. baseLinks (int, str, list of int, or list of str, optional): If given, uses relative positioning mode. Each drive command is treated as relative to the given base link. A value of -1 or an empty str indicates no base link. endEffectorPositions (3-vector or list of 3-vectors, optional): Local offsets for the end effector positions, relative to the specified link. If not given, the offsets will be treated as zero. """ if not hasattr(links,'__iter__'): links = [links] if baseLinks is not None: if not hasattr(baseLinks,'__iter__'): baseLinks = [baseLinks] if len(links) != len(baseLinks): raise ValueError("Links and baseLinks must have the same length") if endEffectorPositions is None: endEffectorPositions = [(0,0,0) for i in range(len(links))] else: assert len(endEffectorPositions) > 0 if not hasattr(endEffectorPositions[0],'__iter__'): endEffectorPositions = [endEffectorPositions] if len(endEffectorPositions) != len(links): raise ValueError("Links and endEffectorPositions must have the same length") self.robot.setConfig(q) self.links = links self.baseLinks = baseLinks self.endEffectorOffsets = endEffectorPositions robotLinks = [ for l in links] self.driveTransforms = [[l.getTransform()[0],l.getWorldPosition(ofs)] for l,ofs in zip(robotLinks,endEffectorPositions)] if baseLinks is None: baseRobotLinks = [None]*len(links) else: baseRobotLinks = [ if ((isinstance(b,str) and len(b) > 0) or b >= 0) else None for b in baseLinks] #compute relative drive transforms self.driveTransforms = [list(se3.mul(se3.inv(b.getTransform()),t)) for (b,t) in zip(baseRobotLinks,self.driveTransforms)] self.ikGoals = [ik.fixed_objective(l,b,local=ofs) for (l,b,ofs) in zip(robotLinks,baseRobotLinks,endEffectorPositions)] self.solver = ik.IKSolver(self.robot) self.solver.setMaxIters(20) for g in self.ikGoals: self.solver.add(g) self.driveSpeedAdjustment = 1.0
[docs] def setTolerance(self,positionTolerance,rotationTolerance=None): self.positionTolerance = positionTolerance if rotationTolerance is None: self.rotationTolerance = positionTolerance self.rotationTolerance = rotationTolerance
[docs] def setMaxIters(self,maxIters): self.solver.setMaxIters(maxIters)
[docs] def setActiveDofs(self,activeDofs): self.solver.setActiveDofs(activeDofs)
[docs] def setJointLimits(self,qmin,qmax=None): if qmin is None: assert qmax is None,"qmin and qmax are either both given or both None" self.qmin,self.qmax = qmin,qmax else: assert qmax is not None,"qmin and qmax are either both given or both None" self.qmin,self.qmax = qmin,qmax
[docs] def setVelocityLimits(self,vmin=None,vmax=None): if vmin is None: assert vmax is None,"vmin and vmax are either both given or both None" self.vmin,self.vmax = vmin,vmax else: assert vmax is not None,"vmin and vmax are either both given or both None" self.vmin,self.vmax = vmin,vmax
[docs] 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 = [ 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 =,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)
[docs] def getTrajectory(self,qcur,angVel,vel,dt,numSteps,reset=True): """Retrieves an entire trajectory produced by stepping ``numSteps`` steps at resolution ``dt``. If ``reset`` is true (on by default), resets the ``driveTransforms`` and ``driveSpeedAdjustment`` to the initial state after the trajectory is computed. """ startDriveTransforms = None startDriveSpeedAdjustment = self.driveSpeedAdjustment if reset: startDriveTransforms = [T for T in self.driveTransforms] qout = [qcur] for i in range(numSteps): frac,qnext =[-1],angVel,vel,dt) if frac <= 0: #done, just stop qout += [qout[-1]]*(numSteps+1-i) break qout.append(qnext) if reset: self.driveTransforms = self.startDriveTransforms self.driveSpeedAdjustment = startDriveSpeedAdjustment
[docs]def axis_rotation_magnitude(R,a): """Given a rotation matrix R and a unit axis a, determines how far R rotates about a. Specifically, if R = from_axis_angle((a,v)) this should return v (modulo pi). """ cterm = so3.trace(R) -,so3.apply(R,a)) mR = so3.matrix(R) sterm = -(a[0]*(mR[1][2]-mR[2][1]) + a[1]*(mR[2][0]-mR[0][2]) + a[2]*(mR[0][1]-mR[1][0])) return math.atan2(sterm,cterm)
[docs]def normalize_rotation(R): """Given a matrix close to a proper (orthogonal) rotation matrix, returns a true orthogonal matrix.""" q = so3.quaternion(R) #normalizes return so3.from_quaternion(q)