def wrench_span(self):
"""
Span matrix of the contact wrench cone in world frame.
This matrix is such that all valid contact wrenches can be written as:
.. math::
w_P = S \\lambda, \\quad \\lambda \\geq 0
where `S` is the friction span and :math:`\\lambda` is a vector with
positive coordinates.
Returns
-------
S : array, shape=(6, 16)
Span matrix of the contact wrench cone.
Notes
-----
Note that the contact wrench coordinates :math:`w_P` ("output" of `S`)
are taken at the contact point `P` (``self.p``) and in the world frame.
Meanwhile, the number of columns of `S` results from our choice of 4
contact points (one for each vertex of the rectangular area) with
4-sided friction pyramids at each.
"""
return hstack([
dot(vstack([eye(3), crossmat(v - self.p)]), self.force_span)
for v in self.vertices
])
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:
.. math::
L_P(q, \\dot{q}) = J(q) \\dot{q}
Parameters
----------
p : array, shape=(3,)
Application point `P` in world coordinates.
Returns
-------
J_am : array, shape=(3,)
Jacobian giving the angular momentum of the robot at `P`.
"""
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 apply_twist(self, v, omega, dt):
"""
Apply a twist [v, omega] defined in the local coordinate frame.
INPUT:
- ``v`` -- linear velocity in local frame
- ``omega`` -- angular velocity in local frame
- ``dt`` -- duration of twist application
"""
self.set_pos(self.p + v * dt)
self.set_rotation_matrix(self.R + dot(crossmat(omega), self.R) * dt)
def apply_twist(self, v, omega, dt):
"""
Apply a twist [v, omega] defined in the local coordinate frame.
Parameters
----------
v : array, shape=(3,)
Linear velocity in local frame.
omega : array, shape=(3,)
Angular velocity in local frame.
dt : scalar
Duration of twist application in [s].
"""
self.set_pos(self.p + v * dt)
self.set_rotation_matrix(self.R + dot(crossmat(omega), self.R) * dt)
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
def compute_angular_momentum_hessian(self, p):
"""
Returns the Hessian tensor :math:`H(q)` such that the rate of change of
the angular momentum with respect to point `P` is
.. math::
\\dot{L}_P(q, \\dot{q}) = J(q) \\ddot{q} + \\dot{q}^T H(q) \\dot{q},
where :math:`J(q)` is the angular-momentum jacobian.
Parameters
----------
p : array, shape=(3,)
Application point in world coordinates.
Returns
-------
H : array, shape=(3, 3, 3)
Hessian tensor of the angular momentum at the application point.
"""
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)
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_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
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 wrench_span(self):
"""
Span matrix of the contact wrench cone in world frame.
This matrix is such that all valid contact wrenches can be written as:
w = S * lambda, lambda >= 0
where S is the friction span and lambda is a vector with positive
coordinates. Note that the contact wrench w is taken at the contact
point (self.p) and in the world frame.
"""
if self.is_sliding:
raise NotImplementedError
else: # fixed contact mode
span_blocks = []
for (i, v) in enumerate(self.vertices):
x, y, z = v - self.p
Gi = vstack([eye(3), crossmat(v - self.p)])
span_blocks.append(dot(Gi, self.force_span))
S = hstack(span_blocks)
assert S.shape == (6, 16)
return S
def compute_zmp_support_area(self, com, plane):
"""
Compute the (pendular) ZMP support area for a given COM position.
INPUT:
- ``com`` -- COM position
- ``plane`` -- position of horizontal plane
OUTPUT:
List of vertices of the area.
ALGORITHM:
This method implements the double-description version of the algorithm
(with a vertical plane normal) <https://arxiv.org/pdf/1510.03232.pdf>
Two better alternatives are available:
1) the raycasting method (Bretl algorithm), available in
<https://github.com/stephane-caron/contact_stability>
2) the more recent convex-hull reduction, described in
<https://scaron.info/research/humanoids-2016.html>
"""
mass = 42. # [kg]
# mass has no effect on the output polygon, c.f. Section IV.C in
# <https://hal.archives-ouvertes.fr/hal-01349880>
n = [0, 0, 1]
z_in, z_out = com[2], plane[2]
G = self.compute_grasp_matrix([0, 0, 0])
F = -self.compute_stacked_wrench_cones()
b = zeros((F.shape[0], 1))
# the input [b, -F] to cdd.Matrix represents (b - F x >= 0)
# see ftp://ftp.ifor.math.ethz.ch/pub/fukuda/cdd/cddlibman/node3.html
M = cdd.Matrix(hstack([b, -F]), number_type='float')
M.rep_type = cdd.RepType.INEQUALITY
B = vstack([
hstack([z_in * eye(3), crossmat(n)]),
hstack([zeros(3), com])]) # hstack([-(cross(n, p_in)), n])])
C = 1. / (- mass * 9.81) * dot(B, G)
d = hstack([com, [0]])
# the input [d, -C] to cdd.Matrix.extend represents (d - C x == 0)
# see ftp://ftp.ifor.math.ethz.ch/pub/fukuda/cdd/cddlibman/node3.html
M.extend(hstack([d.reshape((4, 1)), -C]), linear=True)
# Convert from H- to V-representation
# M.canonicalize()
P = cdd.Polyhedron(M)
V = array(P.get_generators())
# Project output wrenches to 2D set
vertices, rays = [], []
for i in xrange(V.shape[0]):
f_gi = dot(G, V[i, 1:])[:3]
if V[i, 0] == 1: # 1 = vertex, 0 = ray
p_out = (z_out - z_in) * f_gi / (- mass * 9.81) + com
vertices.append(p_out)
else:
r_out = (z_out - z_in) * f_gi / (- mass * 9.81)
rays.append(r_out)
return vertices, rays
