Commit 286c86a7 by Gabriele Buondonno

### [python] Expose skew and unSkew

parent 48483d6b
 ... ... @@ -18,6 +18,9 @@ namespace pinocchio void exposeMotion(); void exposeInertia(); void exposeExplog(); void exposeSkew(); // Expose math module void exposeRpy(); // Expose multibody classes ... ...
 ... ... @@ -53,7 +53,8 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap) exposeJoints(); exposeExplog(); exposeRpy(); exposeSkew(); bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame") .value("WORLD",::pinocchio::WORLD) .value("LOCAL",::pinocchio::LOCAL) ... ...
 // // Copyright (c) 2015-2019 CNRS INRIA // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // #include "pinocchio/bindings/python/fwd.hpp" #include #include "pinocchio/spatial/skew.hpp" namespace pinocchio { namespace python { namespace bp = boost::python; Eigen::Matrix3d skew_proxy(const Eigen::Vector3d & v) { return pinocchio::skew(v); } Eigen::Vector3d unSkew_proxy(const Eigen::Matrix3d & M) { return pinocchio::unSkew(M); } void exposeSkew() { bp::def("skew",&skew_proxy, bp::arg("Angular velocity (vector of size 3)"), "Computes the skew representation of a given 3D vector, " "i.e. the antisymmetric matrix representation of the cross product operator."); bp::def("unSkew",&unSkew_proxy, bp::arg("M: a 3x3 matrix."), "Inverse of skew operator. From a given skew-symmetric matrix M " "of dimension 3x3, it extracts the supporting vector, i.e. the entries of M." "Mathematically speacking, it computes v such that M x = cross(x, v)."); } } // namespace python } // namespace pinocchio
 ... ... @@ -13,6 +13,7 @@ SET(\${PROJECT_NAME}_PYTHON_TESTS bindings_force bindings_frame bindings_inertia bindings_spatial bindings_joint_composite bindings_motion bindings_SE3 ... ...
 import unittest import numpy as np from numpy.random import rand import pinocchio as pin pin.switchToNumpyMatrix() from pinocchio import skew, unSkew from test_case import PinocchioTestCase class TestSpatial(PinocchioTestCase): 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]])) if __name__ == '__main__': unittest.main()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!