def calcDiff(self, data, x, recalc=True):
if recalc:
self.calc(data, x)
dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives(
self.pinocchio, data.pinocchio, data.joint,
pinocchio.ReferenceFrame.LOCAL)
vw = data.v.angular
vv = data.v.linear
data.Aq[:, :] = (data.fXj * da_dq)[:3, :] + skew(vw) * (
data.fXj * dv_dq)[:3, :] - skew(vv) * (data.fXj * dv_dq)[3:, :]
data.Av[:, :] = (data.fXj *
da_dv)[:3, :] + skew(vw) * data.J - skew(vv) * data.Jw
R = data.pinocchio.oMf[self.frame].rotation
if self.gains[0] != 0.:
data.Aq[:, :] += self.gains[0] * R * pinocchio.getFrameJacobian(
self.pinocchio, data.pinocchio, self.frame,
pinocchio.ReferenceFrame.LOCAL)[:3, :]
if self.gains[1] != 0.:
data.Aq[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dq)
data.Av[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dvq)
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, 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 = se3.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)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack(
[np.hstack([R, 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 = se3.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)
p = np.vstack([p, 1])
self.assertApprox(m * p, m.homogeneous * p)
self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
p = rand(6)
self.assertApprox(m * p, m.action * p)
self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
p = rand(5)
with self.assertRaises(ValueError):
m * p
with self.assertRaises(ValueError):
m.actInv(p)
with self.assertRaises(ValueError):
m.actInv('42')
def test_se3(self):
R, p, m = self.R, self.p, self.m
X = np.vstack([np.hstack([R, 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 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 = 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_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 * skew(I.lever).transpose() * 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 test_action_matrix(self):
amb = se3.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 = 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, 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 = se3.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')
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. '
def calcw(q, vq, aq):
pinocchio.forwardKinematics(model, data, q, vq, aq)
return data.v[-1].angular
dv_dqn = df_dq(model, partial(calcv, vq=vq, aq=aq), q)
dw_dqn = df_dq(model, partial(calcw, vq=vq, aq=aq), q)
assertNumDiff(
dv_dq[:3, :], dv_dqn, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
assertNumDiff(
dv_dq[3:, :], dw_dqn, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
daa_dq = da_dq[:3, :] + skew(vw) * dv_dq[:3, :] - skew(vv) * dv_dq[3:, :]
assertNumDiff(
daa_dq, daa_dqn, NUMDIFF_MODIFIER *
h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
# ------------------------------------------------------------------------
# Check contact dynamics 3D contact
del vq, aq
q = pinocchio.randomConfiguration(model)
v = rand(model.nv) * 2 - 1
tau = rand(model.nv) * 2 - 1
for i, f in enumerate(fs):
fs[i] = f * 0
pinocchio.computeJointJacobians(model, data, q)
J6 = pinocchio.getJointJacobian(model, data, model.joints[-1].id,
#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})
