Source code for klampt.plan.kinetrajopt.utils

import numpy as np

[docs]class CostInterface(object): """An abstract base class for a cost function. The subclass must implement `compute` to evaluate f(x), d/dx f(x), and/or d^2/dx^2 f(x). """
[docs] def compute(self, x, grad_level=0): """Evaluates the cost and possibly (determining on grad_level) the derivative and/or Hessian. Args: x (ndarray): The evaluation point grad_level (int, optional): Which order of derivatives are computed. Defaults to 0, which only computes cost. Returns: tuple: the cost and optional derivatives, structured as follows: - if grad_level == 0, it returns (cost,) - if grad_level == 1, it returns (cost,gradient) - if grad_level == 2, it returns (cost,gradient,hessian) """ raise NotImplementedError("Sub-class has to implement function compute")
[docs]class ConstrInterface(object): """An abstract base class for a constraint function. The subclass must implement `compute` to evaluate g(x) and/or d/dx g(x). The interpretation of whether this gives an equality or equality is not provided in this class. See :class:`ConstrContainer`. Currently sparsity is not supported. """
[docs] def compute(self, x, grad_level=0): """Evaluates the constraint function and possibly (determining on grad_level) the Jacobian. Args: x (ndarray): the point to be evaluated grad_level (int, optional): which level of gradient is computed. Defaults to 0. Returns: tuple: The constraint value and optional derivatives, structured as follows: - If grad_level == 0, it returns a 1-D ndarray of g(x) - If grad_level == 1, it also returns a 2-D ndarray giving the constraint Jacobian d/dx g(x). """ raise NotImplementedError("Sub-class has to implement function compute.")
[docs]class ConstrContainer(object): """This one maintains a grid for constraints. :param eqs: list of equality constraint in the format of (index, constr) :param ineqs: list of equality constraint in the format of (index, constr) """ def __init__(self, eqs=None, ineqs=None): self.eqs = eqs if eqs is not None else [] # a list of (pos, constr) tuple self.ineqs = ineqs if ineqs is not None else []
[docs] def add_eq(self, index, constr): """Add an equality constraint to the container. :param index: int, at which point of the trajectory is the constraint imposed. Can be negative indexed. If None, it applies to all points. :param constr: ConstrInterface, the constraint which has to be zero at the solution. """ assert isinstance(constr, ConstrInterface), "constraint has to be subclass of ConstrInterface" self.eqs.append((index, constr))
[docs] def add_ineq(self, index, constr): """Add an inequality constraint to the container. :param index: int, at which point of the trajectory is the constraint imposed. Can be negative indexed. If None, it applies to all points. :param constr: ConstrInterface, the constraint which has to be negative at the solution. """ assert isinstance(constr, ConstrInterface), "constraint has to be subclass of ConstrInterface" self.ineqs.append((index, constr))
[docs] def merge(self, container): """Merge current container with another container :param container: ConstrContainer, jsut another instance of constraint container """ self.eqs += container.eqs self.ineqs += container.ineqs
[docs]class JointLimitsConstr(ConstrInterface): """The class for joint limits""" def __init__(self, qmin, qmax): self.qmin = np.array(qmin) self.qmax = np.array(qmax)
[docs] def compute(self, x, grad_level=0): val = np.concatenate((self.qmin - x, x - self.qmax)) if grad_level == 1: dimq = len(self.qmin) jac = np.r_[-np.eye(dimq), np.eye(dimq)] return (val, jac) else: return (val,)
[docs]class MaskedRobot(object): """Creates a mask to robot so only certain links can be changed. Slightly faster than SubRobotModel since it uses numpy to do the indexing. Args: object (robot): klampt robot index (arr-like): list of active links """ def __init__(self, robot, index): self.robot = robot self.index = index self._config = np.array(robot.getConfig()) def __len__(self): return len(self.index)
[docs] def setConfig(self, config): self._config[self.index] = config self.robot.setConfig(self._config)
[docs] def positionJacobian(self, linkid, config=None, lcl_pos=[0, 0, 0]): if config is not None: self.setConfig(config) jac = np.array(self.robot.link(linkid).getPositionJacobian(lcl_pos)) return jac[:, self.index]
[docs] def orientationJacobian(self, linkid, config=None): if config is not None: self.setConfig(config) jac = np.array(self.robot.link(linkid).getOrientationJacobian()) return jac[:, self.index]
[docs]class MaskedTerrain(object): """This object manages the obstacles we have to consider """ def __init__(self, world, index): self.world = world self.index = index def __len__(self): return len(self.index)
[docs] def terrain(self, num): return self.world.terrain(int(self.index[num]))
[docs] def terrain_unmasked(self, num): return self.world.terrain(num)