def __init__(self, robot, gain=None, weight=None, exclude_dofs=None): """ Create task. INPUT: - ``robot`` -- a Robot object - ``gain`` -- (optional) residual gain between 0 and 1 - ``weight`` -- task weight used in IK cost function - ``exclude_dofs`` -- (optional) DOFs not used by task .. NOTE:: As the differential IK returns velocities, we approximate the task "minimize qdd" by "minimize (qd_next - qd)". """ E = eye(robot.nb_dofs) def vel_residual(dt): return robot.qd def jacobian(): return E Task.__init__( self, jacobian, vel_residual=vel_residual, gain=gain, weight=weight, exclude_dofs=exclude_dofs)
def __init__(self, robot, link, target, gain=None, weight=None, exclude_dofs=None): """ Create a new position task for a given link. INPUT: - ``robot`` -- a Robot object - ``link`` -- one of the Link objects in the robot - ``target`` -- numpy.array or pymanoid.Body for position coordinates - ``gain`` -- (optional) residual gain between 0 and 1 - ``weight`` -- task weight used in IK cost function - ``exclude_dofs`` -- (optional) DOFs not used by task """ if type(link) is str: link = robot.__dict__[link] if type(target) is list: target = array(target) if hasattr(target, 'p'): def pos_residual(): return target.p - link.p elif type(target) is ndarray: def pos_residual(): return target - link.p else: # this is an aesthetic comment raise Exception("Target %s has no 'p' attribute" % type(target)) def jacobian(): return robot.compute_link_pos_jacobian(link) self.link = link Task.__init__( self, jacobian, pos_residual=pos_residual, gain=gain, weight=weight, exclude_dofs=exclude_dofs)
def __init__(self, robot, **kwargs): """ Minimize the centroidal angular momentum. INPUT: - ``robot`` -- a CentroidalRobot object """ def vel_residual(dt): return zeros((3,)) jacobian = robot.compute_cam_jacobian Task.__init__(self, jacobian, vel_residual=vel_residual, **kwargs)
def __init__(self, robot, target, gain=None, weight=None, exclude_dofs=None): """ Add a COM tracking task. INPUT: - ``robot`` -- a CentroidalRobot object - ``target`` -- coordinates or any Body object with a 'pos' attribute """ self.robot = robot jacobian = self.robot.compute_com_jacobian pos_residual = self.compute_pos_residual(target) Task.__init__( self, jacobian, pos_residual=pos_residual, gain=gain, weight=weight, exclude_dofs=exclude_dofs)
def __init__(self, robot, gain=None, weight=None, exclude_dofs=None): """ Try to keep the centroidal angular momentum constant. INPUT: - ``robot`` -- a CentroidalRobot object .. NOTE:: The way this task is implemented may be surprising. Basically, keeping a constant CAM means d/dt(CAM) == 0, i.e., d/dt (J_cam * qd) == 0 J_cam * qdd + qd * H_cam * qd == 0 Because the IK works at the velocity level, we approximate qdd by finite difference from the previous velocity (``qd`` argument to the residual function): J_cam * (qd_next - qd) / dt + qd * H_cam * qd == 0 Finally, the task in qd_next (output velocity) is: J_cam * qd_next == J_cam * qd - dt * qd * H_cam * qd Hence, there are two occurrences of J_cam: one in the task residual, and the second in the task jacobian. """ def vel_residual(dt): qd = robot.qd J_cam = robot.compute_cam_jacobian() H_cam = robot.compute_cam_hessian() # computation intensive :( return dot(J_cam, qd) - dt * dot(qd, dot(H_cam, qd)) jacobian = robot.compute_cam_jacobian Task.__init__( self, jacobian, vel_residual=vel_residual, gain=gain, weight=weight, exclude_dofs=exclude_dofs)
def __init__(self, robot, gain=None, weight=None, exclude_dofs=None): """ Create task. INPUT: - ``robot`` -- a Robot object - ``gain`` -- (optional) residual gain between 0 and 1 - ``weight`` -- task weight used in IK cost function - ``exclude_dofs`` -- (optional) DOFs not used by task """ E = eye(robot.nb_dofs) def vel_residual(dt): return -robot.qd def jacobian(): return E Task.__init__( self, jacobian, vel_residual=vel_residual, gain=gain, weight=weight, exclude_dofs=exclude_dofs)