Source code for klampt.model.ik

"""Inverse kinematics methods.  These use the underlying C++ solver in
Klamp't.  Compared to the SWIG bindings in robotik.h, these helpers
are more convenient (and Pythonic). 

The IK solver is a Newton-Raphson solver, which acts like a local
optimization.  It starts from the robot model's initial configuration
and takes a number of iterations to seek the objective.  The solution
is placed in the robot model's configuration.  If successful, the
solution is also guaranteed to satisfy the robot's joint limits.

For basic IK, the calling sequence is::

    link = robot.link("name")
    goal = ik.objective(link,local=[p1,p2,p3],world=[r1,r2,r3])
    robot.setConfig(startConfig)  #(optional, set the initial configuration)
    if ik.solve(goal):
        print("Hooray, IK solved")
        print("Resulting config:",robot.getConfig())
    else:
        print("IK failed")
        print("Final config:",robot.getConfig())

Here, the points p1,...,p3 on the link (specified in local coordinates)
will be matched to the points r1,...,r3 in the world.  You can also
directly constrain the translation/rotation of the link via the form::
    
    ik.objective(link,R=link.getTransform()[0],t=[x,y,z])
    
which will keep the orientation of the link constant, while setting its
position.

A convenience function is ik.fixed_objective(link) which fixes the position
of the link in its current position/orientation in world coordinates.
Individual points on the link can be constrained to their coordinates using
the local argument, as follows::

    ik.fixed_objective(link,local=[p1,p2,p3])

or if you prefer to give world coordinates, like so::

    ik.fixed_objective(link,world=[q1,q2,q3])

More advanced usage can constrain a link to another link using the ref
argument to ik.objective().  You may also pass multiple goals to ik.solve().

For more control over the solution process, you may use a ik.solver() object.
This object will let you set the active degrees of freedom, get the residual
and Jacobian, and sample random initial solutions.  (see
klampt.robotsim.IKSolver)

    1. By default, the active degrees of freedom are all ancestors of
       the constrained links.  For more control, set them using a solver
       object.
    2. As with any local IK method, Newton-Raphson has a tendency to get
       stuck in local minima.  As a result you may wish to choose random
       initial configurations if the first solve fails.

.. note::
    IK solving with constraints between robot links and rigid objects
    (GeneralizedIKObjective and GeneralizedIKSolver)
    is not yet implemented and will result in an exception.
    This functionality may be supported in the future but is not on the
    current development path.

"""

from ..robotsim import *
from ..math import so3,se3
from .subrobot import SubRobotModel
from typing import Union,Optional,List,Sequence,Callable
from .typing import IntArray,Vector,Vector3,Rotation,RigidTransform

[docs]def objective( body: Union[RobotModelLink,RigidObjectModel], ref: Union[None,RobotModelLink,RigidObjectModel] = None, local: Union[None,Vector3,List[Vector3]] = None, world: Union[None,Vector3,List[Vector3]] = None, R: Optional[Rotation] = None, t: Optional[Vector3] = None ) -> IKObjective: """Returns an IKObjective for a given body. There are two modes in which this can be used: 1. If `local` and `world` are provided, then this objective asks to match the local point (s) on the body to the world point(s). 2. If `R` and `t` are provided, then the objective is set to specify the transform of the body. If `ref` is given, then this is the target frame; otherwise it's the world frame. In mode 1, the world points are bound to this frame; in mode 2, the transformation from body to ref is specified as (R,t). Args: body (RobotModelLink or RigidObjectModel): the link that should be constrained. ref (RobotModelLink or RigidObjectModel, optional): the link that `body` should be constrained to, or None for the world frame. local (3-vector, or a list of 3-vectors, optional): the local coordinates on `body` should be constrained to the corresponding points in `world` world (3-vector, or a list of 3-vectors, optional): the coordinates to which the points in `local` should be constrained. These are given in world coordinates (if `ref`=None) or in `ref`'s frame. R (so3 element; list of 9 floats, optional): the rotation that the link should take on. t (3-vector, optional): the translation that the link's origin should take on. Returns: An IK objective describing the constraint. If body and ref are not on the same robot, then a GeneralizedIKObjective may be returned. TODO: not implemented yet. """ generalized = False if not hasattr(body,'robot'): generalized = True else: if ref and (not hasattr(ref,'robot') or ref.robot() != body.robot()): generalized=True if generalized: obj = None if ref: obj = GeneralizedIKObjective(body,ref) else: obj = GeneralizedIKObjective(body) if (local is not None) and (world is not None): assert(len(local)==len(world)) if hasattr(local[0],'__iter__'): #sanity check for pt in local: assert(len(pt)==3) for pt in world: assert(len(pt)==3) obj.setPoints(local,world) else: obj.setPoint(local,world) elif R and t: obj.setTransform(R,t) else: raise RuntimeError("Need to specify either local and world or R and t") return obj else: obj = IKObjective() obj.robot = body.robot() if (local is not None) and (world is not None): assert(len(local)==len(world)) if hasattr(local[0],'__iter__'): #sanity check for pt in local: assert(len(pt)==3) for pt in world: assert(len(pt)==3) if ref: obj.setRelativePoints(body.index,ref.index,local,world) else: obj.setFixedPoints(body.index,local,world) else: if ref: obj.setRelativePoint(body.index,ref.index,local,world) else: obj.setFixedPoint(body.index,local,world) elif (R is not None) and (t is not None): if ref: obj.setRelativeTransform(body.index,ref.index,R,t) else: obj.setFixedTransform(body.index,R,t) else: raise RuntimeError("Need to specify either local and world or R and t") return obj
[docs]def fixed_objective( link: RobotModelLink, ref: Union[None,RobotModelLink,RigidObjectModel] = None, local: Union[None,Vector3,List[Vector3]] = None, world: Union[None,Vector3,List[Vector3]] = None ) -> IKObjective: """Convenience function for fixing the given link at the current position in space. The arguments are interpreted as follows: - If local and world are not provided, the entire link is constrained. - If only local is provided, these points are fixed to their current positions in space. - If only world is provided, the points on the link with the given world position are constrained in place. Args: link (RobotModelLink): the link that should be constrained. ref (RobotModelLink or RigidObjectModel, optional): the link that `link` should be constrained to, or None for the world frame. local (3-vector, or a list of 3-vectors, optional): the local coordinates on `body` should be constrained in place world (3-vector, or a list of 3-vectors, optional): the world coordinates on `body` should be constrained in place """ refcoords = ref.getTransform() if ref is not None else se3.identity() Tw = link.getTransform() Trel = se3.mul(se3.inv(refcoords),Tw) if local is not None and not hasattr(local[0],'__iter__'): #just a single point, make it a list of points local = [local] if world is not None and not hasattr(world[0],'__iter__'): #just a single point, make it a list of points world = [world] if local is None and world is None: #fixed rotation/position objective return objective(link,ref,R=Trel[0],t=Trel[1]) elif local is None: #fixed point, given by world coordinates Trelinv = se3.inv(Trel) local = [se3.apply(Trelinv,p) for p in world] return objective(link,ref,local=local,world=world) elif world is None: #fixed point, given by local coordinates world = [se3.apply(Trel,p) for p in local] return objective(link,ref,local=local,world=world) else: raise ValueError("ik.fixed_objective does not accept both local and world keyword arguments")
[docs]def fixed_rotation_objective( link: RobotModelLink, ref: Union[None,RobotModelLink,RigidObjectModel] = None, local_axis: Optional[Vector3] = None, world_axis: Optional[Vector3] = None ) -> IKObjective: """Convenience function for fixing the given link at its current orientation in space. The arguments are interpreted as follows: - If `local_axis` and `world_axis` are not provided, the entire link's orientation is constrained. - If only `local_axis` is provided, the link is constrained to rotate about this local axis. - If only `world_axis` is provided, the link is constrained to rotate about this world-space axis. Args: link (RobotModelLink): the link that should be constrained. ref (RobotModelLink or RigidObjectModel, optional): the link that `link` should be constrained to, or None for the world frame. local_axis (3-vector, optional): the local direction on on `body` that should be constrained in place. world_axis (3-vector, optional): the world coordinates of the direction that should be constrained. """ refcoords = ref.getTransform()[0] if ref is not None else so3.identity() Rw = link.getTransform() Rrel = so3.mul(so3.inv(refcoords),Rw[0]) obj = IKObjective() obj.robot = link.robot() if ref: assert link.robot()==ref.robot(),"Can't do generalized fixed rotation objectives yet" obj.setLinks(link.index,(-1 if ref is None else ref.index)) if local_axis is None and world_axis is None: #fixed rotation objective obj.setFixedRotConstraint(Rrel) elif local_axis is None: #fixed axis, given by world coordinates Rrelinv = so3.inv(Rrel) local_axis = so3.apply(Rrelinv,world_axis) obj.setAxialRotConstraint(local_axis,world_axis) elif world_axis is None: #fixed point, given by local coordinates world_axis = so3.apply(Rrel,local_axis) obj.setAxialRotConstraint(local_axis,world_axis) else: raise ValueError("ik.fixed_rotation_objective does not accept both local_axis and world_axis keyword arguments") return obj
[docs]def objects(objectives): """Returns a list of all objects touched by the given objective(s). Not currently implemented.""" raise NotImplementedError() pass
[docs]class SubRobotIKSolver(IKSolver): """A version of IKSolver for subrobots. Allows addressing of configurations and links according to the indices of the SubRobotModel provided upon initialization. """ def __init__(self, r: SubRobotModel): assert(isinstance(r, SubRobotModel)) super().__init__(r._robot) super().setActiveDofs(r._links) self.subrobot = r
[docs] def copy(self) -> "SubRobotIKSolver": return SubRobotIKSolver(self.subrobot)
[docs] def setActiveDofs(self, active: IntArray) -> None: r""" Sets the active degrees of freedom. Respects subrobotness. Args: active (:obj:`list of int`) """ return super().setActiveDofs([self.subrobot._links[x] for x in active])
[docs] def getActiveDofs(self) -> List[int]: r""" Returns the active degrees of freedom. Respects subrobotness. """ return [self.subrobot._inv_links[i] for i in super().getActiveDofs()]
[docs] def setJointLimits(self, qmin: Vector, qmax: Vector) -> None: r""" Sets limits on the robot's configuration. If empty, this turns off joint limits. Respects subrobotness. Args: qmin (:obj:`list of floats`) qmax (:obj:`list of floats`) """ qmin_old, qmax_old = super().getJointLimits() for n, lower, upper in zip(self.subrobot._links, qmin, qmax): qmin_old[n] = lower qmax_old[n] = upper return super().setJointLimits(qmin_old, qmax_old)
[docs] def getJointLimits(self): r""" Returns the limits on the robot's configuration (by default this is the robot's joint limits. Respects subrobotness. Return: ------ (qmin, qmax): Pair of lists of joint limits. """ qmin_full, qmax_full = super().getJointLimits() qmin = [qmin_full[i] for i in self.subrobot._links] qmax = [qmax_full[i] for i in self.subrobot._links] return qmin, qmax
[docs] def setBiasConfig(self, bias_config: Vector) -> None: r""" Biases the solver to approach a given configuration. Setting an empty vector clears the bias term. Respects subrobotness. Args: bias_config (:obj:`list of floats`) """ old_config = super().getBiasConfig() if len(old_config) == 0: old_config = [0.0]*self.robot.numLinks() for n, x in zip(self.subrobot._links, bias_config): old_config[n] = x return super().setBiasConfig(old_config)
[docs] def getBiasConfig(self) ->Vector: r""" Returns the solvers' bias configuration. Respects subrobotness. """ config = super().getBiasConfig() return [config[i] for i in self.subrobot._links]
[docs] def getJacobian(self): r""" Computes the matrix describing the instantaneous derivative of the objective with respect to the active Dofs. Respects subrobotness. Return: ------ Matrix (6 x N) J; such that J @ qdot = EE velocity. """ return super().getJacobian()[:, self.subrobot._links]
[docs]def solver( objectives: Union[IKObjective,Sequence[IKObjective]], iters: Optional[int] = None, tol: Optional[float] = None, secondary : Optional[Union[IKObjective,Sequence[IKObjective],Tuple[Callable,Callable]]] = None, ) -> IKSolver: """Returns a solver for the given objective(s). Args: objectives (IKObjective or list of IKObjectives): the objective(s) that should be solved. Note that these should be result(s) from the :func:`objective` function. OR, if you are making your own goals by calling the ``IKObjective`` constructors from the ``robotsim`` module, you must set the ``.robot`` member of each objective to the :class:`RobotModel` being solved. iters (int, optional): if given, the max # of iterations for the solver is set to this value. Otherwise, the default value (100) is used tol (float, optional): if given, the constraint solving tolerance for the solver is set to this value. Otherwise, the default value (1e-3) is used. secondary (IKObjective or list of IKObjective, optional): if given, tries to optimize a secondary objective. Make sure to use ``solver.minimize()`` rather than ``solver.solve()`` .. note:: In rare cases, this may return a list of IKSolver's if you give it objectives on different robots. They should be solved independently for efficiency. """ result = None if hasattr(objectives,'__iter__'): generalized = [] robs = dict() for obj in objectives: if isinstance(obj,IKObjective): if not hasattr(obj,'robot'): raise ValueError("IKObjective objects must have 'robot' member for use in ik.solver. Either set this manually or use the ik.objective function") robot = obj.robot key = (robot.getName(),robot.getID()) robs.setdefault(key,[robot,[]])[1].append(obj) elif isinstance(obj,GeneralizedIKObjective): generalized.append(obj) else: raise TypeError("Objective is of wrong type") if len(generalized) != 0: #need a generalized solver world = None if generalized[0].isObj1: world = WorldModel(generalized[0].obj1.world) else: world = WorldModel(generalized[0].link1.world) result = GeneralizedIKSolver(world) else: result = [] for key,(r,objs) in robs.items(): if isinstance(r,SubRobotModel): s = SubRobotIKSolver(r) else: s = IKSolver(r) for obj in objs: s.add(obj) result.append(s) else: if isinstance(objectives,IKObjective): if not hasattr(objectives,'robot'): raise ValueError("IKObjective object must have 'robot' member for use in ik.solver. Either set this manually or use the ik.objective function") if isinstance(objectives.robot,SubRobotModel): r = objectives.robot result = SubRobotIKSolver(r) else: result = IKSolver(objectives.robot) result.add(objectives) elif isinstance(objectives,GeneralizedIKObjective): world = None if objectives.isObj1: world = WorldModel(objectives.obj1.world) else: world = WorldModel(objectives.link1.world) result = GeneralizedIKSolver(world) result.add(objectives) else: raise TypeError("Objective is of wrong type") if not isinstance(result,list): result = [result] if secondary is not None: #add secondary objectives if not hasattr(secondary,'__iter__'): secondary = [secondary] for s in result: for o in secondary: s.addSecondary(o) for s in result: if iters is not None: s.setMaxIters(iters) if tol is not None: s.setTolerance(tol) if len(result)==1: return result[0] return result
[docs]def solve( objectives: Union[IKObjective,Sequence[IKObjective]], iters: int = 1000, tol: float = 1e-3, activeDofs: Optional[List[int]] = None, secondary: Optional[Union[IKObjective,Sequence[IKObjective],Tuple[Callable,Callable]]]=None ) -> bool: """Attempts to solve the given objective(s). Either a single objective or a list of simultaneous objectives can be provided. Args: objectives: either a single IKObjective or a list of IKObjectives. iters (int, optional): a maximum number of iterations. tol (float, optional): a maximum error tolerance on satisfying the objectives activeDofs (list, optional): a list of link indices or names to use for IK solving. By default, will determine these automatically. secondary (IKObjective or list of IKObjective or tuple (func,gradient), optional): if given, tries to optimize a secondary objective. If a tuple, it provides both a function and its gradient. .. note:: You cannot use sub-robots and activeDofs at the same time. Undefined behavior will result. Returns: True if a solution is successfully found to the given tolerance, within the provided number of iterations. The robot(s) are then set to the solution configuration. If a solution is not found, False is returned and the robot(s) are set to the best found configuration. This function will be smart about multiple objects / robots. If the objectives are on disjoint robots then disjoint IK problems will be solved individually, which is fater. (The objectives should be a result from the :func:`objective` function. Beware that if you are making your own goals by calling the IKObjective constructors from the robotsim module, the .robot member of these goals must be set). """ minimize = False minimize_args = () if secondary is not None: if isinstance(secondary,Tuple) and len(secondary)==2 and callable(secondary[0]) and callable(secondary[1]): minimize_args = secondary secondary = None s = solver(objectives,iters,tol,secondary) if activeDofs is not None: links = activeDofs[:] for i,l in enumerate(links): if isinstance(l,str): links[i] = s.robot.link(l).getIndex() s.setActiveDofs(links) if hasattr(s,'__iter__'): if minimize: res = [si.minimize(*minimize_args)[0] for si in s] return all(res) res = [si.solve()[0] for si in s] return all(res) else: if minimize: return s.minimize(*minimize_args) return s.solve()
[docs]def residual(objectives: Union[IKObjective,Sequence[IKObjective]]) -> Vector: """Returns the residual of the given objectives.""" return solver(objectives).getResidual()
[docs]def jacobian(objectives: Union[IKObjective,Sequence[IKObjective]]): """Returns the jacobian of the given objectives.""" return solver(objectives).getJacobian()
[docs]def solve_global( objectives: Union[IKObjective,Sequence[IKObjective]], iters: int = 1000, tol: float = 1e-3, activeDofs: Optional[List[int]] = None, secondary: Optional[Union[IKObjective,Sequence[IKObjective],Tuple[Callable,Callable]]]=None, numRestarts: int=100, feasibilityCheck: Optional[Callable[[],bool]] = None, startRandom: bool=False ) -> bool: """Attempts to solve the given objective(s) but avoids local minima to some extent using a random-restart technique. Args: objectives, iters, tol, activeDofs, secondary: same as :func:`solve` numRestarts (int, optional): the number of random restarts. The larger this is, the more likely a solution will be found, but if no solution exists, the routine will take more time. feasibilityCheck (function, optional): if provided, it will be a function feasible() that returns True if the robot/world in the current configuration is feasible, and False otherwise. startRandom (bool, optional): True if you wish to forgo the robot's current configuration and start from random initial configurations. Returns: True if a feasible solution is successfully found to the given tolerance, within the provided number of iterations. """ if feasibilityCheck is None: feasibilityCheck=lambda : True minimize = False minimize_args = () if secondary is not None: if isinstance(secondary,Tuple) and len(secondary)==2 and callable(secondary[0]) and callable(secondary[1]): minimize_args = secondary secondary = None s = solver(objectives,iters,tol,secondary) if activeDofs is not None: links = activeDofs[:] for i,l in enumerate(links): if isinstance(l,str): links[i] = s.robot.link(l).getIndex() s.setActiveDofs(links) if not hasattr(s,'__iter__'): s = [s] if startRandom: s[0].sampleInitial() if minimize: res = [si.minimize(*minimize_args) for si in s] else: res = [si.solve() for si in s] if all(res): if feasibilityCheck(): return True for i in range(numRestarts): for si in s: si.sampleInitial() if minimize: res = [si.minimize(*minimize_args) for si in s] else: res = [si.solve() for si in s] if all(res): if feasibilityCheck(): return True return False
[docs]def solve_nearby( objectives: Union[IKObjective,Sequence[IKObjective]], maxDeviation: float, iters: int = 1000, tol: float = 1e-3, activeDofs: Optional[List[int]] = None, secondary: Optional[Union[IKObjective,Sequence[IKObjective],Tuple[Callable,Callable]]]=None, numRestarts: int = 0, feasibilityCheck: Optional[Callable[[],bool]] = None ) -> bool: """Solves for an IK solution that does not deviate too far from the initial configuration. .. note:: Currently only supports single-robot objectives! Args: objectives, iters, tol, activeDofs, feasibilityCheck: same as :func:`solve_global` except for... maxDeviation (float): the maximum absolute amount in configuration space that each configuration element is allowed to change. numRestarts (int): same as in :func:`solve_global` , but is by default set to zero. """ if feasibilityCheck is None: feasibilityCheck=lambda : True minimize = False minimize_args = () if secondary is not None: if isinstance(secondary,Tuple) and len(secondary)==2 and callable(secondary[0]) and callable(secondary[1]): minimize_args = secondary secondary = None s = solver(objectives,iters,tol) if not isinstance(s,IKSolver): raise NotImplementedError("solve_nearby: currently only supports single-robot objectives") if hasattr(objectives,'__iter__'): robot = objectives[0].robot else: robot = objectives.robot if activeDofs is not None: links = activeDofs[:] for i,l in enumerate(links): if isinstance(l,str): links[i] = s.robot.link(l).getIndex() s.setActiveDofs(links) #set up the joint limits dofs = s.getActiveDofs() q = robot.getConfig() qmin,qmax = robot.getJointLimits() for d in dofs: qmin[d] = max(qmin[d],q[d]-maxDeviation) qmax[d] = min(qmax[d],q[d]+maxDeviation) s.setJointLimits(qmin,qmax) s.setBiasConfig(q) #start solving if minimize: solved = s.minimize(*minimize_args) else: solved = s.solve() if solved and feasibilityCheck(): return True for i in range(numRestarts): s.sampleInitial() if minimize: solved = s.minimize(*minimize_args) else: solved = s.solve() if solved and feasibilityCheck(): return True return False