From 92d9019373471b4328915a7a605c686bf5bd0daf Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Mon, 27 Apr 2015 14:19:33 +0200
Subject: [PATCH] [Incremental][C++] Add planar joint useful for mobile robots
---
CMakeLists.txt | 1 +
src/multibody/joint/joint-planar.hpp | 253 ++++++++++++++++++++++++++
src/multibody/joint/joint-variant.hpp | 5 +-
3 files changed, 257 insertions(+), 2 deletions(-)
create mode 100644 src/multibody/joint/joint-planar.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 621f88fbe..2b506f68f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,6 +76,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-spherical.hpp
multibody/joint/joint-spherical-ZYX.hpp
multibody/joint/joint-prismatic.hpp
+ multibody/joint/joint-planar.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp
new file mode 100644
index 000000000..d5ae56eea
--- /dev/null
+++ b/src/multibody/joint/joint-planar.hpp
@@ -0,0 +1,253 @@
+#ifndef __se3_joint_planar_hpp__
+#define __se3_joint_planar_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/math/sincos.hpp"
+#include "pinocchio/spatial/motion.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+
+namespace se3
+{
+
+ struct JointDataPlanar;
+ struct JointModelPlanar;
+
+ struct JointPlanar
+ {
+ struct BiasZero
+ {
+ operator Motion () const { return Motion::Zero(); }
+ }; // struct BiasZero
+
+ friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
+ friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+
+ struct MotionPlanar
+ {
+ typedef double Scalar_t;
+ typedef MotionTpl<Scalar_t> Motion;
+
+ Scalar_t x_dot_, y_dot_, theta_dot_;
+
+ MotionPlanar () : x_dot_(NAN), y_dot_(NAN), theta_dot_(NAN) {}
+ MotionPlanar (Scalar_t x_dot, Scalar_t y_dot, Scalar_t theta_dot) : x_dot_(x_dot), y_dot_(y_dot), theta_dot_(theta_dot) {}
+
+ operator Motion() const
+ {
+ return Motion (Motion::Vector3(x_dot_, y_dot_, 0.), Motion::Vector3(0., 0., theta_dot_));
+ }
+ }; // struct MotionPlanar
+
+ friend const MotionPlanar operator+ (const MotionPlanar & m, const BiasZero &)
+ { return m; }
+
+ friend Motion operator+ (const MotionPlanar & m1, const Motion & m2)
+ {
+ Motion result (m2);
+ result.linear ()[0] += m1.x_dot_;
+ result.linear ()[1] += m1.y_dot_;
+
+ result.angular ()[2] += m1.theta_dot_;
+
+ return result;
+ }
+
+ struct ConstraintPlanar
+ {
+ public:
+ typedef double Scalar_t;
+ typedef MotionTpl<Scalar_t> Motion;
+ typedef ForceTpl<Scalar_t> Force;
+ typedef Motion::Matrix3 Matrix3;
+ typedef Motion::Vector3 Vector3;
+
+ public:
+
+ Motion operator* (const MotionPlanar & vj) const
+ { return vj; }
+
+
+ struct ConstraintTranspose
+ {
+ const ConstraintPlanar & ref;
+ ConstraintTranspose(const ConstraintPlanar & ref) : ref(ref) {}
+
+ Force::Vector3 operator* (const Force & phi)
+ {
+ return Force::Vector3 (phi.linear()[0], phi.linear()[1], phi.angular()[2]);
+ }
+
+ /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+ template<typename D>
+ friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
+ operator*( const ConstraintTranspose &, const Eigen::MatrixBase<D> & F )
+ {
+ typedef Eigen::Matrix<typename Eigen::MatrixBase<D>::Scalar, 3, -1> MatrixReturnType;
+ assert(F.rows()==6);
+
+ MatrixReturnType result (3, F.cols ());
+ result.template topRows <2> () = F.template topRows <2> ();
+ result.template bottomRows <1> () = F.template bottomRows <1> ();
+ return result;
+ }
+ }; // struct ConstraintTranspose
+
+ ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+
+ operator ConstraintXd () const
+ {
+ Eigen::Matrix<Scalar_t,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setZero ();
+ S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
+ S(Inertia::LINEAR + 0,0) = 1.;
+ S(Inertia::LINEAR + 1,1) = 1.;
+ S(Inertia::ANGULAR + 2,2) = 1.;
+ return ConstraintXd(S);
+ }
+
+ Eigen::Matrix <Scalar_t,6,3> se3Action (const SE3 & m) const
+ {
+ Eigen::Matrix <double,6,3> X_subspace;
+ X_subspace.block <3,2> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ().leftCols <2> ();
+ X_subspace.block <3,1> (Motion::LINEAR, 2) = m.rotation ().rightCols <1> ();
+
+ X_subspace.block <3,2> (Motion::ANGULAR, 0) = m.rotation ().leftCols <2> ();
+ X_subspace.block <3,1> (Motion::ANGULAR, 2).setZero ();
+
+ return X_subspace;
+ }
+
+ }; // struct ConstraintPlanar
+
+ template<typename D>
+ friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ Motion result (Motion::Zero ());
+ result.linear ().template head<2> () = v.template topRows<2> ();
+ result.angular ().template tail<1> () = v.template bottomRows<1> ();
+ return result;
+ }
+
+ }; // struct JointPlanar
+
+ Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2)
+ {
+ Motion result;
+
+ const Motion::Vector3 & m1_t = m1.linear();
+ const Motion::Vector3 & m1_w = m1.angular();
+
+ result.angular () << m1_w(1) * m2.theta_dot_, - m1_w(0) * m2.theta_dot_, 0.;
+ result.linear () << m1_t(1) * m2.theta_dot_ - m1_w(2) * m2.y_dot_, - m1_t(0) * m2.theta_dot_ + m1_w(2) * m2.x_dot_, m1_w(0) * m2.y_dot_ - m1_w(1) * m2.x_dot_;
+
+ return result;
+ }
+
+ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+ Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
+ {
+ Eigen::Matrix <Inertia::Scalar, 6, 3> M;
+ const double mass = Y.mass ();
+ const Inertia::Vector3 & com = Y.lever ();
+ const Symmetric3 & inertia = Y.inertia ();
+
+ M.topLeftCorner <3,3> ().setZero ();
+ M.topLeftCorner <2,2> ().diagonal ().fill (mass);
+
+ Inertia::Vector3 mc (mass * com);
+ M.rightCols <1> ().head <2> () << mc(1), - mc(0);
+
+ M.bottomLeftCorner <3,2> () << 0., mc(2), - mc(1), 0., mc(1), -mc(0);
+ M.rightCols <1> ().tail <3> () = inertia.data ().tail <3> ();
+ M.rightCols <1> ().head <2> () = mc.head <2> () * com(2);
+ M.rightCols <1> ().tail <1> () = -mc.head <2> ().transpose () * com.head <2> ();
+
+ return M;
+ }
+
+ namespace internal
+ {
+ template<>
+ struct ActionReturn<JointPlanar::ConstraintPlanar >
+ { typedef Eigen::Matrix<double,6,3> Type; };
+ }
+
+ template<>
+ struct traits<JointPlanar>
+ {
+ typedef JointDataPlanar JointData;
+ typedef JointModelPlanar JointModel;
+ typedef JointPlanar::ConstraintPlanar Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef JointPlanar::MotionPlanar Motion_t;
+ typedef JointPlanar::BiasZero Bias_t;
+ typedef Eigen::Matrix<double,6,3> F_t;
+ enum {
+ NQ = 3,
+ NV = 3
+ };
+ };
+ template<> struct traits<JointDataPlanar> { typedef JointPlanar Joint; };
+ template<> struct traits<JointModelPlanar> { typedef JointPlanar Joint; };
+
+ struct JointDataPlanar : public JointDataBase<JointDataPlanar>
+ {
+ typedef JointPlanar Joint;
+ SE3_JOINT_TYPEDEF;
+
+ typedef Motion::Scalar Scalar;
+
+ typedef Eigen::Matrix<Scalar,6,6> Matrix6;
+ typedef Eigen::Matrix<Scalar,3,3> Matrix3;
+ typedef Eigen::Matrix<Scalar,3,1> Vector3;
+
+ Constraint_t S;
+ Transformation_t M;
+ Motion_t v;
+ Bias_t c;
+
+ JointDataPlanar () : M(1) {}
+ };
+
+ struct JointModelPlanar : public JointModelBase<JointModelPlanar>
+ {
+ typedef JointPlanar Joint;
+ SE3_JOINT_TYPEDEF;
+
+ JointData createData() const { return JointData(); }
+
+ void calc (JointData & data,
+ const Eigen::VectorXd & qs) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
+
+ double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
+
+ data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
+ data.M.translation ().head <2> () = q.head<2> ();
+
+ }
+
+ void calc (JointData & data,
+ const Eigen::VectorXd & qs,
+ const Eigen::VectorXd & vs ) const
+ {
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
+ Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NQ> (idx_v ());
+
+ double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
+
+ data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
+ data.M.translation ().head <2> () = q.head<2> ();
+
+ data.v.x_dot_ = q_dot(0);
+ data.v.y_dot_ = q_dot(1);
+ data.v.theta_dot_ = q_dot(2);
+ }
+ };
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_planar_hpp__
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index ab1337ac4..50bfd28d9 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -6,12 +6,13 @@
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
+#include "pinocchio/multibody/joint/joint-planar.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
namespace se3
{
- typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
- typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ , JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
+ typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar> JointModelVariant;
+ typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar > JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
--
GitLab