Verified Commit fa57d90f authored by Justin Carpentier's avatar Justin Carpentier
Browse files

python: rework a bit the skew exposure and add skewSquare

parent bf4aaf02
// //
// Copyright (c) 2015-2019 CNRS INRIA // Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // Copyright (c) 2020 Wandercraft
// //
#include "pinocchio/bindings/python/fwd.hpp"
#include <boost/python.hpp> #include <boost/python.hpp>
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/skew.hpp" #include "pinocchio/spatial/skew.hpp"
namespace pinocchio namespace pinocchio
...@@ -12,30 +14,41 @@ namespace pinocchio ...@@ -12,30 +14,41 @@ namespace pinocchio
namespace python namespace python
{ {
namespace bp = boost::python; namespace bp = boost::python;
Eigen::Matrix3d skew_proxy(const Eigen::Vector3d & v) // We need to resort to another call, because it seems that Boost.Python is not aligning the Eigen::MatrixBase. TODO: fix it!
{ template<typename Matrix3>
return pinocchio::skew(v); Eigen::Matrix<typename Matrix3::Scalar,3,1,Matrix3::Options> unSkew(const Matrix3 & mat)
}
Eigen::Vector3d unSkew_proxy(const Eigen::Matrix3d & M)
{ {
return pinocchio::unSkew(M); return pinocchio::unSkew(mat);
} }
void exposeSkew() void exposeSkew()
{ {
typedef SE3::Matrix3 Matrix3;
bp::def("skew",&skew_proxy, typedef SE3::Vector3 Vector3;
bp::arg("Angular velocity (vector of size 3)"),
"Computes the skew representation of a given 3D vector, " bp::def("skew",&skew<Vector3>,
"i.e. the antisymmetric matrix representation of the cross product operator."); bp::arg("u"),
"Computes the skew representation of a given 3d vector, "
bp::def("unSkew",&unSkew_proxy, "i.e. the antisymmetric matrix representation of the cross product operator, aka U = [u]x.\n"
bp::arg("M: a 3x3 matrix."), "Parameters:\n"
"Inverse of skew operator. From a given skew-symmetric matrix M " "\tu: the input vector of dimension 3");
"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)."); bp::def("skewSquare",&skewSquare<Vector3,Vector3>,
bp::args("u","v"),
"Computes the skew square representation of two given 3d vectors, "
"i.e. the antisymmetric matrix representation of the chained cross product operator, u x (v x w), where w is another 3d vector.\n"
"Parameters:\n"
"\tu: the first input vector of dimension 3\n"
"\tv: the second input vector of dimension 3");
bp::def("unSkew",&unSkew<Matrix3>,
bp::arg("U"),
"Inverse of skew operator. From a given skew symmetric matrix U (i.e U = -U.T)"
"of dimension 3x3, it extracts the supporting vector, i.e. the entries of U.\n"
"Mathematically speacking, it computes v such that U.dot(x) = cross(u, x).\n"
"Parameters:\n"
"\tU: the input skew symmetric matrix of dimension 3x3.");
} }
} // namespace python } // namespace python
......
...@@ -185,7 +185,7 @@ namespace pinocchio ...@@ -185,7 +185,7 @@ namespace pinocchio
/// \param[in] u A 3 dimensional vector. /// \param[in] u A 3 dimensional vector.
/// \param[in] v A 3 dimensional vector. /// \param[in] v A 3 dimensional vector.
/// ///
/// \return The square cross product matrix C. /// \return The square cross product matrix skew[u] * skew[v].
/// ///
template <typename V1, typename V2> template <typename V1, typename V2>
inline Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options> inline Eigen::Matrix<typename V1::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
......
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