conversions.cpp

Go to the documentation of this file.

//
// Copyright (c) 2019-2020 CNRS INRIA
//
 
#include <sstream>
 
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/spatial/se3.hpp"
 
namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;
    typedef SE3::Scalar Scalar;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1> VectorXd;
    typedef Eigen::Matrix<Scalar, 7,1> Vector7d;
    typedef Eigen::Map<      SE3::Quaternion> QuatMap;
    typedef Eigen::Map<const SE3::Quaternion> QuatConstMap;
 
    VectorXd SE3ToXYZQUAT(const SE3& M)
    {
      Vector7d res;
      res.head<3>() = M.translation();
      QuatMap (res.tail<4>().data()) = M.rotation();
      return res;
    }
 
    bp::tuple SE3ToXYZQUATtuple(const SE3& M)
    {
      SE3::Quaternion q (M.rotation());
      return bp::make_tuple (
          M.translation()(0), M.translation()(1), M.translation()(2),
          q.x(), q.y(), q.z(), q.w());
    }
 
    template <typename TupleOrList>
    SE3 XYZQUATToSE3_bp(const TupleOrList& v)
    {
      bp::ssize_t size = bp::len(v);
      if(size != 7)
      {
        throw std::invalid_argument(
            "Wrong size: v(" + std::to_string(size) + ") should have 7 elements");
      }
      SE3::Quaternion q (
          static_cast<Scalar>(bp::extract<Scalar>(v[6])),
          static_cast<Scalar>(bp::extract<Scalar>(v[3])),
          static_cast<Scalar>(bp::extract<Scalar>(v[4])),
          static_cast<Scalar>(bp::extract<Scalar>(v[5])));
      SE3::Vector3 t (
          static_cast<Scalar>(bp::extract<Scalar>(v[0])),
          static_cast<Scalar>(bp::extract<Scalar>(v[1])),
          static_cast<Scalar>(bp::extract<Scalar>(v[2])));
      return SE3 (q.matrix(), t);
    }
 
    template <typename Vector7Like>
    SE3 XYZQUATToSE3_ei(const Vector7Like& v)
    {
      if(v.rows() != 7 || v.cols() != 1)
      {
        std::ostringstream shape;
        shape << "(" << v.rows() << ", " << v.cols() << ")";
        throw std::invalid_argument("Wrong size: v" + shape.str() + " but should have the following shape (7, 1)");
      }
      QuatConstMap q (v.template tail<4>().data());
      return SE3 (q.matrix(), v.template head<3>());
    }
    
    void exposeConversions()
    {
      const char* doc1 = "Convert the input SE3 object to a numpy array.";
      bp::def("SE3ToXYZQUAT"     , SE3ToXYZQUAT     , "M", doc1);
      const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w].";
      bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple);
 
      const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element.";
      bp::def("XYZQUATToSE3",
              static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSE3_bp<bp::tuple>),
              bp::arg("tuple"),doc2);
      bp::def("XYZQUATToSE3",
              static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSE3_bp<bp::list >),
              bp::arg("list"),doc2);
      bp::def("XYZQUATToSE3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSE3_ei<VectorXd >),
              bp::arg("array"),doc2);
    }
    
  } // namespace python
} // namespace pinocchio