From 386d89c3351834732d5a5175e375fe160b042a2f Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Thu, 10 Oct 2019 14:35:06 +0200
Subject: [PATCH] [python][se3] if compiled with pinocchio, se3 return SE3 and
Motion objects instead of matrices
---
python/curves_python.cpp | 35 +++++++++++++++++++++++++++++++++++
1 file changed, 35 insertions(+)
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index fc90846..b36a85e 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -4,10 +4,17 @@
#include <boost/python.hpp>
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+#include <pinocchio/spatial/se3.hpp>
+#include <pinocchio/spatial/motion.hpp>
+#endif //CURVES_WITH_PINOCCHIO_SUPPORT
+
namespace curves
{
using namespace boost::python;
+
+
/* base wrap of curve_abc and others parent abstract class: must implement all pure virtual methods */
struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t>
{
@@ -364,7 +371,26 @@ namespace curves
return new SE3Curve_t(&translation_curve,&rotation_curve);
}
+ #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ typedef pinocchio::SE3Tpl<real,0> SE3_t;
+ typedef pinocchio::MotionTpl<real,0> Motion_t;
+
+ SE3Curve_t* wrapSE3CurveFromSE3Pinocchio(const SE3_t& init_pose, const SE3_t& end_pose, const real min, const real max)
+ {
+
+ return new SE3Curve_t(transform_t(init_pose.toHomogeneousMatrix()),transform_t(end_pose.toHomogeneousMatrix()), min, max);
+ }
+
+ SE3_t se3Return(const SE3Curve_t& curve, const real t)
+ {
+ return SE3_t(curve(t).matrix());
+ }
+ Motion_t se3ReturnDerivate(const SE3Curve_t& curve, const real t, const std::size_t order)
+ {
+ return Motion_t(curve.derivate(t,order));
+ }
+ #else
matrix4_t se3Return(const SE3Curve_t& curve, const real t)
{
return curve(t).matrix();
@@ -374,6 +400,8 @@ namespace curves
{
return curve.derivate(t,order);
}
+ #endif //CURVES_WITH_PINOCCHIO_SUPPORT
+
matrix3_t se3returnRotation(const SE3Curve_t& curve, const real t)
{
@@ -729,6 +757,13 @@ namespace curves
.def("min", &SE3Curve_t::min, "Get the LOWER bound on interval definition of the curve.")
.def("max", &SE3Curve_t::max,"Get the HIGHER bound on interval definition of the curve.")
.def("dim", &SE3Curve_t::dim,"Get the dimension of the curve.")
+ #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ .def("__init__",
+ make_constructor(&wrapSE3CurveFromSE3Pinocchio,default_call_policies(),
+ args("init_SE3","end_SE3","min","max")),
+ "Create a SE3 curve between two SE3 objects from Pinocchio, defined for t \in [min,max]."
+ " Using linear interpolation for translation and slerp for rotation between init and end.")
+ #endif //CURVES_WITH_PINOCCHIO_SUPPORT
// .def("saveAsText", &SE3Curve_t::saveAsText<SE3Curve_t>,bp::args("filename"),"Saves *this inside a text file.")
// .def("loadFromText",&SE3Curve_t::loadFromText<SE3Curve_t>,bp::args("filename"),"Loads *this from a text file.")
// .def("saveAsXML",&SE3Curve_t::saveAsXML<SE3Curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
--
GitLab