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)
