Commit 286c86a7 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[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 <boost/python.hpp>
#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!
Please register or to comment