def __init__(self, robot, combined='b-', color=None, linewidth=3,
linestyle=None, buffer_size=1000):
body = PointMass(
robot.com, robot.mass, name='RobotCOMState', visible=False)
super(COMTrajectoryDrawer, self).__init__(
body, combined, color, linewidth, linestyle)
self.robot = robot
def get_com_point_mass(self):
"""
Get the center of mass as a PointMass instance.
Returns
-------
com : PointMass
Center of mass of the robot.
"""
return PointMass(pos=self.com, mass=self.mass)
def show_com(self):
if self.__com_box is None:
self.__com_box = PointMass(self.com, self.mass)
self.__com_box.show()
class Humanoid(Robot):
"""
Humanoid robots add a free base and centroidal computations.
"""
__free_flyer_xml = """
<environment>
<robot>
<kinbody>
<body name="FLYER_TX_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_TY_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_TZ_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_ROLL_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_PITCH_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<kinbody>
<body name="FLYER_YAW_LINK">
<mass type="mimicgeom">
<total>0</total>
</mass>
</body>
</kinbody>
<robot file="%s" name="%s">
<kinbody>
<joint name="FLYER_TX" type="slider">
<body>FLYER_TX_LINK</body>
<body>FLYER_TY_LINK</body>
<axis>1 0 0</axis>
</joint>
<joint name="FLYER_TY" type="slider">
<body>FLYER_TY_LINK</body>
<body>FLYER_TZ_LINK</body>
<axis>0 1 0</axis>
</joint>
<joint name="FLYER_TZ" type="slider">
<body>FLYER_TZ_LINK</body>
<body>FLYER_YAW_LINK</body>
<axis>0 0 1</axis>
</joint>
<joint name="FLYER_YAW" type="hinge" circular="true">
<body>FLYER_YAW_LINK</body>
<body>FLYER_PITCH_LINK</body>
<axis>0 0 1</axis>
</joint>
<joint name="FLYER_PITCH" type="hinge" circular="true">
<body>FLYER_PITCH_LINK</body>
<body>FLYER_ROLL_LINK</body>
<axis>0 1 0</axis>
</joint>
<joint name="FLYER_ROLL" type="hinge" circular="true">
<body>FLYER_ROLL_LINK</body>
<body>%s</body>
<axis>1 0 0</axis>
</joint>
</kinbody>
</robot>
</robot>
</environment>
"""
def __init__(self, path, root_body, qd_lim=None):
"""
Create a new humanoid robot model.
INPUT:
- ``path`` -- path to the COLLADA model of the robot
- ``root_body`` -- name of the root (first) body in the model
- ``qd_lim`` -- maximum angular joint velocity (in [rad] / [s])
"""
name = basename(splitext(path)[0])
xml = Humanoid.__free_flyer_xml % (path, name, root_body)
super(Humanoid, self).__init__(path, xml=xml, qd_lim=qd_lim)
self.has_free_flyer = True
self.__cam = None
self.__com = None
self.__com_box = None
self.__comd = None
"""
Kinematics
==========
"""
def set_ff_pos(self, pos):
"""
Update the position of the free-flying frame.
INPUT:
- ``pos`` -- position in world frame
"""
self.set_dof_values(pos, [self.TRANS_X, self.TRANS_Y, self.TRANS_Z])
def set_ff_rpy(self, rpy):
"""
Update the orientation of the free-flying frame.
INPUT:
- ``rpy`` -- Euler angles (Euler sequence (1, 2, 3))
"""
self.set_dof_values(rpy, [self.ROT_R, self.ROT_P, self.ROT_Y])
def set_ff_quat(self, quat):
"""
Update the orientation of the free-flying frame.
INPUT:
- ``quat`` -- quaternion vector (w, x, y, z)
"""
return self.set_ff_rpy(rpy_from_quat(quat))
def set_ff_pose(self, pose):
"""
Update the pose of the free-flying frame.
INPUT:
- ``pose`` -- frame pose in OpenRAVE format (qw, qx, qy, qz, x, y, z)
"""
self.set_ff_quat(pose[:4])
self.set_ff_pos(pose[4:])
def set_dof_values(self, q, dof_indices=None, clamp=False):
"""
Set the joint values of the robot.
INPUT:
- ``q`` -- vector of joint angle values (ordered by DOF indices)
- ``dof_indices`` -- (optional) list of DOF indices to update
- ``clamp`` -- correct ``q`` if it exceeds joint limits
"""
self.__cam = None
self.__com = None
self.__comd = None
super(Humanoid, self).set_dof_values(
q, dof_indices=dof_indices, clamp=clamp)
def set_dof_velocities(self, qd, dof_indices=None):
self.__cam = None
self.__comd = None
super(Humanoid, self).set_dof_velocities(qd, dof_indices=dof_indices)
def set_active_dof_values(self, q_active):
self.__cam = None
self.__com = None
self.__comd = None
super(Humanoid, self).set_active_dof_values(q_active)
def set_active_dof_velocities(self, qd_active):
self.__cam = None
super(Humanoid, self).set_active_dof_velocities(qd_active)
"""
Center Of Mass
==============
"""
@property
def com(self):
if self.__com is None:
self.__com = self.compute_com()
if self.__com_box is not None:
self.__com_box.set_pos(self.__com)
return self.__com
@property
def comd(self):
if self.__comd is None:
self.__comd = self.compute_com_velocity()
return self.__comd
def compute_com(self):
total = zeros(3)
for link in self.rave.GetLinks():
m = link.GetMass()
c = link.GetGlobalCOM()
total += m * c
return total / self.mass
def compute_com_velocity(self):
total = zeros(3)
for link in self.rave.GetLinks():
m = link.GetMass()
R = link.GetTransform()[0:3, 0:3]
c_local = link.GetLocalCOM()
v = link.GetVelocity()
rd, omega = v[:3], v[3:]
cd = rd + cross(omega, dot(R, c_local))
total += m * cd
return total / self.mass
def compute_com_jacobian(self):
"""
Compute the jacobian matrix J(q) of the position of the center of mass G
of the robot, i.e. the velocity of the G is given by:
pd_G(q, qd) = J(q) * qd
q -- vector of joint angles
qd -- vector of joint velocities
pd_G -- velocity of the center of mass G
"""
J_com = zeros((3, self.nb_dofs))
for link in self.rave.GetLinks():
m = link.GetMass()
if m < 1e-4:
continue
index = link.GetIndex()
c = link.GetGlobalCOM()
J_com += m * self.rave.ComputeJacobianTranslation(index, c)
J_com /= self.mass
return J_com
def compute_com_acceleration(self, qdd):
qd = self.qd
J = self.compute_com_jacobian()
H = self.compute_com_hessian()
return dot(J, qdd) + dot(qd, dot(H, qdd))
def compute_com_hessian(self):
H_com = zeros((self.nb_dofs, 3, self.nb_dofs))
for link in self.rave.GetLinks():
m = link.GetMass()
if m < 1e-4:
continue
index = link.GetIndex()
c = link.GetGlobalCOM()
H_com += m * self.rave.ComputeHessianTranslation(index, c)
H_com /= self.mass
return H_com
def show_com(self):
if self.__com_box is None:
self.__com_box = PointMass(self.com, self.mass)
self.__com_box.show()
def hide_com(self):
self.__com_box.hide()
"""
Angular Momentum
================
"""
def compute_angular_momentum(self, p):
"""
Compute the angular momentum with respect to point p.
INPUT:
- ``p`` -- application point in world coordinates
"""
am = zeros(3)
for link in self.rave.GetLinks():
# TODO: replace by newer link.GetGlobalInertia()
T = link.GetTransform()
m = link.GetMass()
v = link.GetVelocity()
c = link.GetGlobalCOM()
R, r = T[0:3, 0:3], T[0:3, 3]
I = dot(R, dot(link.GetLocalInertia(), R.T))
rd, omega = v[:3], v[3:]
cd = rd + cross(r - c, omega)
am += cross(c - p, m * cd) + dot(I, omega)
return am
def compute_angular_momentum_jacobian(self, p):
"""
Compute the jacobian matrix J(q) such that the angular momentum of the
robot at p is given by:
L_p(q, qd) = J(q) * qd
INPUT:
- ``p`` -- application point in world coordinates
"""
J_am = zeros((3, self.nb_dofs))
for link in self.rave.GetLinks():
m = link.GetMass()
i = link.GetIndex()
c = link.GetGlobalCOM()
R = link.GetTransform()[0:3, 0:3]
I = dot(R, dot(link.GetLocalInertia(), R.T))
J_trans = self.rave.ComputeJacobianTranslation(i, c)
J_rot = self.rave.ComputeJacobianAxisAngle(i)
J_am += dot(crossmat(c - p), m * J_trans) + dot(I, J_rot)
return J_am
def compute_angular_momentum_hessian(self, p):
"""
Returns a matrix H(q) such that the rate of change of the angular
momentum with respect to point p is
Ld_p(q, qd) = dot(J(q), qdd) + dot(qd.T, dot(H(q), qd)),
where J(q) is the angular-momentum jacobian.
p -- application point in world coordinates
"""
def crosstens(M):
assert M.shape[0] == 3
Z = zeros(M.shape[1])
T = array([[Z, -M[2, :], M[1, :]],
[M[2, :], Z, -M[0, :]],
[-M[1, :], M[0, :], Z]])
return T.transpose([2, 0, 1]) # T.shape == (M.shape[1], 3, 3)
def middot(M, T):
"""
Dot product of a matrix with the mid-coordinate of a 3D tensor.
M -- matrix with shape (n, m)
T -- tensor with shape (a, m, b)
Outputs a tensor of shape (a, n, b).
"""
return tensordot(M, T, axes=(1, 1)).transpose([1, 0, 2])
H = zeros((self.nb_dofs, 3, self.nb_dofs))
for link in self.rave.GetLinks():
m = link.GetMass()
i = link.GetIndex()
c = link.GetGlobalCOM()
R = link.GetTransform()[0:3, 0:3]
# J_trans = self.rave.ComputeJacobianTranslation(i, c)
J_rot = self.rave.ComputeJacobianAxisAngle(i)
H_trans = self.rave.ComputeHessianTranslation(i, c)
H_rot = self.rave.ComputeHessianAxisAngle(i)
I = dot(R, dot(link.GetLocalInertia(), R.T))
H += middot(crossmat(c - p), m * H_trans) \
+ middot(I, H_rot) \
- dot(crosstens(dot(I, J_rot)), J_rot)
return H
"""
Centroidal Angular Momentum (CAM)
=================================
It is simply the angular momentum taken at the center of mass.
"""
@property
def cam(self):
if self.__cam is None:
self.__cam = self.compute_cam()
return self.__cam
def compute_cam(self):
"""Compute the centroidal angular momentum."""
return self.compute_angular_momentum(self.com)
def compute_cam_jacobian(self):
"""
Compute the jacobian matrix J(q) such that the CAM is given by:
L_G(q, qd) = J(q) * qd
"""
return self.compute_angular_momentum_jacobian(self.com)
def compute_cam_rate(self, qdd):
"""Compute the time-derivative of the CAM. """
qd = self.qd
J = self.compute_cam_jacobian()
H = self.compute_cam_hessian()
return dot(J, qdd) + dot(qd, dot(H, qd))
def compute_cam_hessian(self, q):
"""
Compute the matrix H(q) such that the rate of change of the CAM is
Ld_G(q, qd) = dot(J(q), qdd) + dot(qd.T, dot(H(q), qd))
"""
return self.compute_angular_momentum_hessian(self.com)
"""
Whole-body wrench
=================
"""
def compute_gravito_inertial_wrench(self, qdd, p):
"""
Compute the gravito-inertial wrench:
w(p) = [ f ] = [ m (g - pdd_G) ]
[ tau(p) ] [ (p_G - p) x m (g - pdd_G) - Ld_G ]
with m the robot mass, g the gravity vector, G the COM, pdd_G the
acceleration of the COM, and Ld_GG the rate of change of the angular
momentum (taken at the COM).
INPUT:
- ``qdd`` -- array of DOF accelerations
- ``p`` -- reference point at which the wrench is taken
"""
g = array([0, 0, -9.81])
f_gi = self.mass * g
tau_gi = zeros(3)
link_velocities = self.rave.GetLinkVelocities()
link_accelerations = self.rave.GetLinkAccelerations(qdd)
for link in self.rave.GetLinks():
mi = link.GetMass()
ci = link.GetGlobalCOM()
I_ci = link.GetLocalInertia()
Ri = link.GetTransform()[0:3, 0:3]
ri = dot(Ri, link.GetLocalCOM())
angvel = link_velocities[link.GetIndex()][3:]
linacc = link_accelerations[link.GetIndex()][:3]
angacc = link_accelerations[link.GetIndex()][3:]
ci_ddot = linacc \
+ cross(angvel, cross(angvel, ri)) \
+ cross(angacc, ri)
angmmt = dot(I_ci, angacc) - cross(dot(I_ci, angvel), angvel)
f_gi -= mi * ci_ddot[2]
tau_gi += mi * cross(ci, g - ci_ddot) - dot(Ri, angmmt)
return f_gi, tau_gi
"""
Zero-tilting Moment Point
=========================
"""
def compute_zmp(self, qdd):
"""
Compute the Zero-tilting Moment Point (ZMP).
INPUT:
``qdd`` -- vector of joint accelerations
.. NOTE::
For an excellent introduction to the concepts of ZMP and center of
pressure, see “Forces acting on a biped robot. center of
pressure-zero moment point” by P. Sardain and G. Bessonnet
<http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.138.8014&rep=rep1&type=pdf>.
"""
O, n = zeros(3), array([0, 0, 1])
f_gi, tau_gi = self.compute_gravito_inertial_wrench(qdd, O)
return cross(n, tau_gi) * 1. / dot(n, f_gi)
"""
Posture generation
==================
"""
def generate_posture(self, stance, **kwargs):
"""
Generate robot posture (joint-angles + free-flyer) for a given Stance.
INPUT:
- ``stance`` -- Stance object
Extra keyword arguments are passed on to ``solve_ik()``.
"""
from tasks import COMTask, ContactTask, PostureTask
if stance.left_foot is not None:
self.ik.add_task(
ContactTask(
self, self.left_foot, stance.left_foot, weight=1.))
if stance.right_foot is not None:
self.ik.add_task(
ContactTask(
self, self.right_foot, stance.right_foot, weight=1.))
if stance.left_hand is not None:
self.ik.add_task(
ContactTask(
self, self.left_hand, stance.left_hand, weight=1.))
if stance.right_hand is not None:
self.ik.add_task(
ContactTask(
self, self.right_hand, stance.right_hand, weight=1.))
com_task = COMTask(self, stance.com, weight=1e-2)
posture_task = PostureTask(self, self.q_halfsit, weight=1e-4)
self.ik.add_task(com_task)
self.ik.add_task(posture_task)
self.solve_ik(**kwargs)
