def test_computeRpyJacobianTimeDerivative(self):
# Check zero at zero velocity
r = random() * 2 * pi - pi
p = random() * pi - pi / 2
y = random() * 2 * pi - pi
rpy = np.array([r, p, y])
rpydot = np.zeros(3)
dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
self.assertTrue((dj0 == np.zeros((3, 3))).all())
djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
self.assertTrue((djL == np.zeros((3, 3))).all())
djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
self.assertTrue((djW == np.zeros((3, 3))).all())
djA = computeRpyJacobianTimeDerivative(rpy, rpydot,
pin.LOCAL_WORLD_ALIGNED)
self.assertTrue((djA == np.zeros((3, 3))).all())
# Check correct identities between different versions
rpydot = np.random.rand(3)
dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
djA = computeRpyJacobianTimeDerivative(rpy, rpydot,
pin.LOCAL_WORLD_ALIGNED)
self.assertTrue((dj0 == djL).all())
self.assertTrue((djW == djA).all())
R = rpyToMatrix(rpy)
jL = computeRpyJacobian(rpy, pin.LOCAL)
jW = computeRpyJacobian(rpy, pin.WORLD)
omegaL = jL.dot(rpydot)
omegaW = jW.dot(rpydot)
self.assertApprox(omegaW, R.dot(omegaL))
self.assertApprox(djW, pin.skew(omegaW).dot(R).dot(jL) + R.dot(djL))
self.assertApprox(djW, R.dot(pin.skew(omegaL)).dot(jL) + R.dot(djL))
def test_skew(self):
u = np.random.rand((3))
v = np.random.rand((3))
u_skew = pin.skew(u)
u_unskew = pin.unSkew(u_skew)
self.assertApprox(u, u_unskew)
v_skew = pin.skew(v)
u_v_square = pin.skewSquare(u, v)
self.assertApprox(u_v_square, u_skew.dot(v_skew))
def test_skew(self):
v3 = rand(3)
self.assertApprox(v3, unSkew(skew(v3)))
self.assertLess(np.linalg.norm(skew(v3).dot(v3)), 1e-10)
x, y, z = tuple(rand(3).tolist())
M = np.array([[0., x, y], [-x, 0., z], [-y, -z, 0.]])
self.assertApprox(M, skew(unSkew(M)))
rhs = rand(3)
self.assertApprox(np.cross(v3, rhs, axis=0), skew(v3).dot(rhs))
self.assertApprox(M.dot(rhs), np.cross(unSkew(M), rhs, axis=0))
x, y, z = tuple(v3.tolist())
self.assertApprox(skew(v3),
np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]))
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, pin.skew(p).dot(R)]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack([
np.hstack([R, np.expand_dims(p, 1)]),
np.array([[0., 0., 0., 1.]])
])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous,
m.homogeneous.dot(m2.homogeneous))
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation.dot(p) + m.translation)
self.assertApprox(
m.actInv(p),
m.rotation.T.dot(p) - m.rotation.T.dot(m.translation))
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(
Exception
): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
def skewSquare(self):
v3 = rand(3)
Mss = skewSquare(v3)
Ms = skew(v3)
Mss_ref = Ms.dot(Ms)
self.assertApprox(Mss,Mss_ref)
def test_computeRpyJacobian(self):
# Check identity at zero
rpy = np.zeros(3)
j0 = computeRpyJacobian(rpy)
self.assertTrue((j0 == np.eye(3)).all())
jL = computeRpyJacobian(rpy, pin.LOCAL)
self.assertTrue((jL == np.eye(3)).all())
jW = computeRpyJacobian(rpy, pin.WORLD)
self.assertTrue((jW == np.eye(3)).all())
jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
self.assertTrue((jA == np.eye(3)).all())
# Check correct identities between different versions
r = random() * 2 * pi - pi
p = random() * pi - pi / 2
y = random() * 2 * pi - pi
rpy = np.array([r, p, y])
R = rpyToMatrix(rpy)
j0 = computeRpyJacobian(rpy)
jL = computeRpyJacobian(rpy, pin.LOCAL)
jW = computeRpyJacobian(rpy, pin.WORLD)
jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
self.assertTrue((j0 == jL).all())
self.assertTrue((jW == jA).all())
self.assertApprox(jW, R.dot(jL))
# Check against finite differences
rpydot = np.random.rand(3)
eps = 1e-7
tol = 1e-4
dRdr = (rpyToMatrix(r + eps, p, y) - R) / eps
dRdp = (rpyToMatrix(r, p + eps, y) - R) / eps
dRdy = (rpyToMatrix(r, p, y + eps) - R) / eps
Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2]
omegaL = jL.dot(rpydot)
self.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol))
omegaW = jW.dot(rpydot)
self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
def forcePose(self, name, force):
""" computes unit vectors describing the force and populates the pose matrix """
unit_force = force / np.linalg.norm(force)
res = np.cross(self.x_axis, unit_force, axis=0)
res_norm = np.linalg.norm(res)
if res_norm <= 1.e-8:
return np.eye(3)
projection = np.dot(self.x_axis, unit_force)
res_skew = pin.skew(res)
rotation = np.eye(3) + res_skew + np.dot(res_skew, res_skew) * (1 - projection) / res_norm**2
pose = self.data.oMf[self.model.getFrameId(name)]
return pin.SE3(rotation, pose.translation)
def test_dynamic_parameters(self):
I = pin.Inertia.Random()
v = I.toDynamicParameters()
self.assertApprox(v[0], I.mass)
self.assertApprox(v[1:4], I.mass * I.lever)
I_o = I.inertia + I.mass * pin.skew(I.lever).transpose() * pin.skew(
I.lever)
I_ov = np.matrix([[float(v[4]),
float(v[5]), float(v[7])],
[float(v[5]), float(v[6]),
float(v[8])],
[float(v[7]), float(v[8]),
float(v[9])]])
self.assertApprox(I_o, I_ov)
I2 = pin.Inertia.FromDynamicParameters(v)
self.assertApprox(I2, I)
def rotationMatrixFromTwoVectors(a, b):
a_copy = a / np.linalg.norm(a)
b_copy = b / np.linalg.norm(b)
a_cross_b = np.cross(a_copy, b_copy, axis=0)
s = np.linalg.norm(a_cross_b)
if libcrocoddyl_pywrap.getNumpyType() == np.matrix:
warnings.warn(
"Numpy matrix supports will be removed in future release",
DeprecationWarning,
stacklevel=2)
if s == 0:
return np.matrix(np.eye(3))
c = np.asscalar(a_copy.T * b_copy)
ab_skew = pinocchio.skew(a_cross_b)
return np.matrix(
np.eye(3)) + ab_skew + ab_skew * ab_skew * (1 - c) / s**2
else:
if s == 0:
return np.eye(3)
c = np.dot(a_copy, b_copy)
ab_skew = pinocchio.skew(a_cross_b)
return np.eye(3) + ab_skew + np.dot(ab_skew, ab_skew) * (1 - c) / s**2
def test_action_matrix(self):
amb = pin.SE3.Random()
aXb = amb.action
self.assertTrue(np.allclose(
aXb[:3, :3], amb.rotation)) # top left 33 corner = rotation of amb
self.assertTrue(np.allclose(
aXb[3:,
3:], amb.rotation)) # bottom right 33 corner = rotation of amb
tblock = pin.skew(amb.translation) * amb.rotation
self.assertTrue(np.allclose(
aXb[:3,
3:], tblock)) # top right 33 corner = translation + rotation
self.assertTrue(np.allclose(aXb[3:, :3],
zero([3, 3]))) # bottom left 33 corner = 0
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, pin.skew(p) * R]),
np.hstack([zero([3, 3]), R])])
self.assertApprox(m.action, X)
M = np.vstack(
[np.hstack([R, p]),
np.matrix([0., 0., 0., 1.], np.double)])
self.assertApprox(m.homogeneous, M)
m2 = pin.SE3.Random()
self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
p = rand(3)
self.assertApprox(m * p, m.rotation * p + m.translation)
self.assertApprox(m.actInv(p),
m.rotation.T * p - m.rotation.T * m.translation)
## not supported
# p = np.vstack([p, 1])
# self.assertApprox(m * p, m.homogeneous * p)
# self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
## not supported
# p = rand(6)
# self.assertApprox(m * p, m.action * p)
# self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
# Currently, the different cases do not throw the same exception type.
# To have a more robust test, only Exception is checked.
# In the comments, the most specific actual exception class at the time of writing
p = rand(5)
with self.assertRaises(Exception): # RuntimeError
m * p
with self.assertRaises(Exception): # RuntimeError
m.actInv(p)
with self.assertRaises(
Exception
): # Boost.Python.ArgumentError (subclass of TypeError)
m.actInv('42')
def J_M_times_P(M, P):
return np.hstack((M.rotation, -np.dot(M.rotation, pinocchio.skew(P))))
def compute_force_qp(self, q, dq, cnt_array, w_com):
"""Computes the forces needed to generated a desired centroidal wrench.
Args:
q: Generalized robot position configuration.
q: Generalized robot velocity configuration.
cnt_array: Array with {0, 1} of #endeffector size indicating if
an endeffector is in contact with the ground or not. Forces are
only computed for active endeffectors.
w_com: Desired centroidal wrench to achieve given forces.
Returns:
Computed forces as a plain array of size 3 * num_endeffectors.
"""
robot = self.robot
# com = np.reshape(np.array(q[0:3]), (3,))
com = pin.centerOfMass(robot.pin_robot.model, robot.pin_robot.data, q,
dq)
r = [
robot.pin_robot.data.oMf[i].translation - com for i in self.eff_ids
]
# Use the contact activation from the plan to determine which of the forces
# should be active.
N = (int)(robot.nb_ee)
assert len(cnt_array) == robot.nb_ee
# Setup the QP problem.
Q = 2. * np.eye(3 * N + 6)
Q[-6:-3, -6:-3] = np.diag(self.qp_penalty_lin)
Q[-3:, -3:] = np.diag(self.qp_penalty_ang)
p = np.zeros(3 * N + 6)
A = np.zeros((6, 3 * N + 6))
b = w_com
G = np.zeros((5 * N, 3 * N + 6))
h = np.zeros((5 * N))
j = 0
for i in range(robot.nb_ee):
if cnt_array[i] == 0:
continue
A[:3, 3 * j:3 * (j + 1)] = np.eye(3)
A[3:, 3 * j:3 * (j + 1)] = pin.skew(r[i])
G[5 * j + 0, 3 * j + 0] = 1 # mu Fz - Fx >= 0
G[5 * j + 0, 3 * j + 2] = -self.mu
G[5 * j + 1, 3 * j + 0] = -1 # mu Fz + Fx >= 0
G[5 * j + 1, 3 * j + 2] = -self.mu
G[5 * j + 2, 3 * j + 1] = 1 # mu Fz - Fy >= 0
G[5 * j + 2, 3 * j + 2] = -self.mu
G[5 * j + 3, 3 * j + 1] = -1 # mu Fz + Fy >= 0
G[5 * j + 3, 3 * j + 2] = -self.mu
G[5 * j + 4, 3 * j + 2] = -1 # Fz >= 0
j += 1
A[:, -6:] = np.eye(6)
solx = quadprog_solve_qp(Q, p, G, h, A, b)
F = np.zeros(3 * len(cnt_array))
j = 0
for i in range(len(cnt_array)):
if cnt_array[i] == 0:
continue
F[3 * i:3 * (i + 1)] = solx[3 * j:3 * (j + 1)]
j += 1
return F
#ancestorsOf.__init__.__defaults__ = (robot,)
iv.__defaults__ = (robot,)
parent.__defaults__ = (robot,)
jFromIdx.__defaults__ = (robot,)
# --- SE3 operators
Mcross = lambda x,y: Motion(x).cross(Motion(y)).vector
Mcross.__doc__ = "Motion cross product"
Fcross = lambda x,y: Motion(x).cross(Force(y)).vector
Fcross.__doc__ = "Force cross product"
MCross = lambda V,v: np.bmat([ Mcross(V[:,i],v) for i in range(V.shape[1]) ])
FCross = lambda V,f: np.bmat([ Fcross(V[:,i],f) for i in range(V.shape[1]) ])
adj = lambda nu: np.bmat([[ skew(nu[3:]),skew(nu[:3])],[zero([3,3]),skew(nu[3:])]])
adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)"
adjdual = lambda nu: np.bmat([[ skew(nu[3:]),zero([3,3])],[skew(nu[:3]),skew(nu[3:])]])
adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "
td = np.tensordot
quad = lambda H,v: np.matrix(td(td(H,v,[2,0]),v,[1,0])).T
quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]'''
def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7):
mformat = lambda x,sarg = sarg,eps=eps: sarg.format(x) if abs(x)>eps else ' 0. '
np.set_printoptions(formatter={'float': mformat})
