joint-derived.hpp

//
// Copyright (c) 2015-2021 CNRS INRIA
//

#pragma once

#include <boost/python.hpp>
#include <eigenpy/exception.hpp>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/multibody/joint.hpp"

namespace pinocchio
{
 namespace python
 {
 namespace bp = boost::python;

 template<class JointModelDerived>
 struct JointModelBasePythonVisitor
 : public boost::python::def_visitor<JointModelBasePythonVisitor<JointModelDerived>>
 {
 public:
 typedef typename JointModelDerived::JointDataDerived JointDataDerived;

 template<class PyClass>
 void visit(PyClass & cl) const
 {
 cl.def(bp::init<>(bp::arg("self")))
 // All are add_properties cause ReadOnly
 .add_property("id", &get_id)
 .add_property("idx_q", &get_idx_q)
 .add_property("idx_v", &get_idx_v)
 .add_property("idx_vExtended", &get_idx_vExtended)
 .add_property("nq", &get_nq)
 .add_property("nv", &get_nv)
 .add_property("nvExtended", &get_nvExtended)
 .add_property(
 "hasConfigurationLimit", &JointModelDerived::hasConfigurationLimit,
 "Return vector of boolean if joint has configuration limits.")
 .add_property(
 "hasConfigurationLimitInTangent", &JointModelDerived::hasConfigurationLimitInTangent,
 "Return vector of boolean if joint has configuration limits in tangent space.")
 .def("setIndexes", &setIndexes0, bp::args("self", "joint_id", "idx_q", "idx_v"))
 .def(
 "setIndexes", &setIndexes1,
 bp::args("self", "joint_id", "idx_q", "idx_v", "idx_vExtended"))
 .def("classname", &JointModelDerived::classname)
 .staticmethod("classname")
 .def("calc", &calc0, bp::args("self", "jdata", "q"))
 .def("calc", &calc1, bp::args("self", "jdata", "q", "v"))
 .def(
 "createData", &JointModelDerived::createData, bp::arg("self"),
 "Create data associated to the joint model.")
 .def(
 "hasSameIndexes", &JointModelDerived::template hasSameIndexes<JointModelDerived>,
 bp::args("self", "other"), "Check if this has same indexes than other.")
 .def(
 "shortname", &JointModelDerived::shortname, bp::arg("self"),
 "Returns string indicating the joint type (class name):"
 "\n\t- JointModelR[*]: Revolute Joint, with rotation axis [*] ∈ [X,Y,Z]"
 "\n\t- JointModelRevoluteUnaligned: Revolute Joint, with rotation axis not aligned "
 "with X, Y, nor Z"
 "\n\t- JointModelRUB[*]: Unbounded revolute Joint (without position limits), with "
 "rotation axis [*] ∈ [X,Y,Z]"
 "\n\t- JointModelRevoluteUnboundedUnaligned: Unbounded revolute Joint, with rotation "
 "axis not aligned with X, Y, nor Z"
 "\n\t- JointModelP[*]: Prismatic Joint, with rotation axis [*] ∈ [X,Y,Z]"
 "\n\t- JointModelPlanar: Planar joint"
 "\n\t- JointModelPrismaticUnaligned: Prismatic joint, with translation axis not "
 "aligned with X, Y, nor Z"
 "\n\t- JointModelSphericalZYX: Spherical joint (3D rotation)"
 "\n\t- JointModelTranslation: Translation joint (3D translation)"
 "\n\t- JointModelFreeFlyer: Joint enabling 3D rotation and translations.")

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
 .def(bp::self == bp::self)
 .def(bp::self != bp::self)
#endif
 ;
 }

 static JointIndex get_id(const JointModelDerived & self)
 {
 return self.id();
 }
 static int get_idx_q(const JointModelDerived & self)
 {
 return self.idx_q();
 }
 static int get_idx_v(const JointModelDerived & self)
 {
 return self.idx_v();
 }
 static int get_idx_vExtended(const JointModelDerived & self)
 {
 return self.idx_vExtended();
 }
 static int get_nq(const JointModelDerived & self)
 {
 return self.nq();
 }
 static int get_nv(const JointModelDerived & self)
 {
 return self.nv();
 }
 static int get_nvExtended(const JointModelDerived & self)
 {
 return self.nvExtended();
 }

 static void
 calc0(const JointModelDerived & self, JointDataDerived & jdata, const context::VectorXs & q)
 {
 self.calc(jdata, q);
 }
 static void calc1(
 const JointModelDerived & self,
 JointDataDerived & jdata,
 const context::VectorXs & q,
 const context::VectorXs & v)
 {
 self.calc(jdata, q, v);
 }

 static void
 setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v)
 {
 self.setIndexes(id, idx_q, idx_v);
 }

 static void setIndexes1(
 JointModelDerived & self,
 const int & id,
 const int & idx_q,
 const int & idx_v,
 const int & idx_vExtended)
 {
 self.setIndexes(id, idx_q, idx_v, idx_vExtended);
 }
 };

 template<class JointDataDerived>
 struct JointDataBasePythonVisitor
 : public boost::python::def_visitor<JointDataBasePythonVisitor<JointDataDerived>>
 {
 public:
 template<class PyClass>
 void visit(PyClass & cl) const
 {
 cl
 // All are add_properties cause ReadOnly
 .add_property("joint_q", &get_joint_q)
 .add_property("joint_v", &get_joint_v)
 .add_property("S", &get_S)
 .add_property("M", &get_M)
 .add_property("v", &get_v)
 .add_property("c", &get_c)
 .add_property("U", &get_U)
 .add_property("Dinv", &get_Dinv)
 .add_property("UDinv", &get_UDinv)
 .def("shortname", &JointDataDerived::shortname, bp::arg("self"))

#ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
 .def(bp::self == bp::self)
 .def(bp::self != bp::self)
#endif
 ;
 }

 static typename JointDataDerived::ConfigVector_t get_joint_q(const JointDataDerived & self)
 {
 return self.joint_q_accessor();
 }
 static typename JointDataDerived::TangentVector_t get_joint_v(const JointDataDerived & self)
 {
 return self.joint_v_accessor();
 }
 // static typename JointDataDerived::Constraint_t get_S(const JointDataDerived & self)
 // { return self.S_accessor(); }
 static typename JointDataDerived::Constraint_t::DenseBase get_S(const JointDataDerived & self)
 {
 return self.S_accessor().matrix();
 }
 static typename JointDataDerived::Transformation_t get_M(const JointDataDerived & self)
 {
 return self.M_accessor();
 }
 static typename JointDataDerived::Motion_t get_v(const JointDataDerived & self)
 {
 return self.v_accessor();
 }
 static typename JointDataDerived::Bias_t get_c(const JointDataDerived & self)
 {
 return self.c_accessor();
 }
 static typename JointDataDerived::U_t get_U(const JointDataDerived & self)
 {
 return self.U_accessor();
 }
 static typename JointDataDerived::D_t get_Dinv(const JointDataDerived & self)
 {
 return self.Dinv_accessor();
 }
 static typename JointDataDerived::UD_t get_UDinv(const JointDataDerived & self)
 {
 return self.UDinv_accessor();
 }

 static void expose()
 {
 }
 };

 } // namespace python
} // namespace pinocchio