bindings_spatial.py
Go to the documentation of this file.
1 import unittest
2 
3 import numpy as np
4 import pinocchio as pin
5 from numpy.random import rand
6 from pinocchio import skew, skewSquare, unSkew
7 
8 try:
9  import casadi
10  from pinocchio import casadi as cpin
11 
12  WITH_CASADI = True
13 except ImportError:
14  WITH_CASADI = False
15 
16 from test_case import PinocchioTestCase
17 
18 
20  def test_skew(self):
21  v3 = rand(3)
22  self.assertApprox(v3, unSkew(skew(v3)))
23  self.assertLess(np.linalg.norm(skew(v3).dot(v3)), 1e-10)
24 
25  x, y, z = tuple(rand(3).tolist())
26  M = np.array([[0.0, x, y], [-x, 0.0, z], [-y, -z, 0.0]])
27  self.assertApprox(M, skew(unSkew(M)))
28 
29  rhs = rand(3)
30  self.assertApprox(np.cross(v3, rhs, axis=0), skew(v3).dot(rhs))
31  self.assertApprox(M.dot(rhs), np.cross(unSkew(M), rhs, axis=0))
32 
33  x, y, z = tuple(v3.tolist())
34  self.assertApprox(skew(v3), np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]))
35 
36  def skewSquare(self):
37  v3 = rand(3)
38 
39  Mss = skewSquare(v3)
40 
41  Ms = skew(v3)
42  Mss_ref = Ms.dot(Ms)
43 
44  self.assertApprox(Mss, Mss_ref)
45 
47  if WITH_CASADI:
48  Rtarget = pin.utils.rotate("x", 3.14 / 4) # Target
49  R0 = pin.Quaternion(0.707107, 0.707107, 0, 0).matrix()
50  nu0 = pin.log3(R0)
51 
52  # Casadi symbolic variables and functions functions
53  nu = casadi.SX.sym("v", 3, 1)
54  D = cpin.log3(
55  cpin.exp3(nu).T @ Rtarget
56  ) # This seems to be the probelematic function
57  dDi_dnu = [
58  casadi.Function("gradient" + str(i), [nu], [casadi.gradient(D[i], nu)])
59  for i in range(3)
60  ] # Compute the gradient of function D wrt nu
61 
62  d0 = dDi_dnu[0](nu0) # Evaluate the gradient at a problematic point nu0
63  self.assertFalse(
64  np.any(np.isnan(d0)), "NaN detected in the log3 function derivative"
65  )
66 
67  def test_Jlog6(self):
68  for _ in range(10):
69  M0: pin.SE3 = pin.SE3.Random()
70  M1 = M0.copy()
71  dM = M0.actInv(M1)
72  J = pin.Jlog6(dM)
73  R = dM.rotation
74  logR = pin.log3(R)
75  Jrot = pin.Jlog3(R)
76  print(R, f":\nlog: {logR}")
77  print(Jrot)
78 
79  self.assertApprox(dM, pin.SE3.Identity())
80  self.assertApprox(Jrot, np.eye(3))
81  self.assertApprox(J, np.eye(6))
82 
83 
84 if __name__ == "__main__":
85  unittest.main()
bindings_spatial.TestSpatial.test_Jlog6
def test_Jlog6(self)
Definition: bindings_spatial.py:67
bindings_spatial.TestSpatial
Definition: bindings_spatial.py:19
bindings_spatial.TestSpatial.test_skew
def test_skew(self)
Definition: bindings_spatial.py:20
bindings_spatial.TestSpatial.skewSquare
def skewSquare(self)
Definition: bindings_spatial.py:36
test_case.PinocchioTestCase.assertApprox
def assertApprox(self, a, b, eps=1e-6)
Definition: test_case.py:12
bindings_spatial.TestSpatial.test_NaN_log3_casadi
def test_NaN_log3_casadi(self)
Definition: bindings_spatial.py:46
pinocchio::python::skew
Eigen::Matrix< typename Vector3::Scalar, 3, 3, Vector3::Options > skew(const Vector3 &v)
Definition: expose-skew.cpp:19
pinocchio::python::unSkew
Eigen::Matrix< typename Matrix3::Scalar, 3, 1, Matrix3::Options > unSkew(const Matrix3 &mat)
Definition: expose-skew.cpp:36
test_case.PinocchioTestCase
Definition: test_case.py:11
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:41


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06