From be3c965c4efc7e50e13be975a8a85546de75b291 Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Mon, 27 Apr 2015 15:59:25 +0200
Subject: [PATCH] [Incremental][C++] Add complete translational joint
---
CMakeLists.txt | 1 +
src/multibody/joint/joint-translation.hpp | 199 +++++++++++++++++++++
src/multibody/joint/joint-variant.hpp | 5 +-
unittest/joints.cpp | 200 ++++++++++++++++++++++
4 files changed, 403 insertions(+), 2 deletions(-)
create mode 100644 src/multibody/joint/joint-translation.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2b506f68f..605620891 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -77,6 +77,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-spherical-ZYX.hpp
multibody/joint/joint-prismatic.hpp
multibody/joint/joint-planar.hpp
+ multibody/joint/joint-translation.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp
new file mode 100644
index 000000000..27dd44f2d
--- /dev/null
+++ b/src/multibody/joint/joint-translation.hpp
@@ -0,0 +1,199 @@
+#ifndef __se3_joint_translation_hpp__
+#define __se3_joint_translation_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/skew.hpp"
+
+namespace se3
+{
+
+ struct JointDataTranslation;
+ struct JointModelTranslation;
+
+ struct JointTranslation
+ {
+ 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 MotionTranslation
+ {
+ MotionTranslation () : v (Motion::Vector3 (NAN, NAN, NAN)) {}
+ MotionTranslation (const Motion::Vector3 & v) : v (v) {}
+ MotionTranslation (const MotionTranslation & other) : v (other.v) {}
+ Motion::Vector3 v;
+
+ Motion::Vector3 & operator() () { return v; }
+ const Motion::Vector3 & operator() () const { return v; }
+
+ operator Motion() const
+ {
+ return Motion (v, Motion::Vector3::Zero ());
+ }
+
+ MotionTranslation & operator= (const MotionTranslation & other)
+ {
+ v = other.v;
+ return *this;
+ }
+ }; // struct MotionTranslation
+
+ friend const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
+ { return m; }
+
+ friend Motion operator+ (const MotionTranslation & m1, const Motion & m2)
+ {
+ return Motion (m2.linear () + m1.v, m2.angular ());
+ }
+
+ struct ConstraintTranslationSubspace
+ {
+ public:
+
+ Motion operator* (const MotionTranslation & vj) const
+ { return Motion (vj (), Motion::Vector3::Zero ()); }
+
+ ConstraintTranslationSubspace () {}
+
+ struct ConstraintTranspose
+ {
+ const ConstraintTranslationSubspace & ref;
+ ConstraintTranspose(const ConstraintTranslationSubspace & ref) : ref(ref) {}
+
+ Force::Vector3 operator* (const Force & phi)
+ {
+ return phi.linear ();
+ }
+
+
+ /* [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 )
+ {
+ assert(F.rows()==6);
+ return F.template middleRows <3> (Inertia::LINEAR);
+ }
+ }; // struct ConstraintTranspose
+
+ ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+
+ operator ConstraintXd () const
+ {
+ Eigen::Matrix<double,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setIdentity ();
+ S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
+ return ConstraintXd(S);
+ }
+
+ Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
+ {
+ Eigen::Matrix <double,6,3> M;
+ M.block <3,3> (Motion::LINEAR, 0) = m.rotation ();
+ M.block <3,3> (Motion::ANGULAR, 0).setZero ();
+
+ return M;
+ }
+
+ }; // struct ConstraintTranslationSubspace
+
+ template<typename D>
+ friend Motion operator* (const ConstraintTranslationSubspace & S, const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ return Motion (v, Motion::Vector3::Zero ());
+ }
+
+ }; // struct JointTranslation
+
+ Motion operator^ (const Motion & m1, const JointTranslation::MotionTranslation & m2)
+ {
+ return Motion (m1.angular ().cross (m2.v), Motion::Vector3::Zero ());
+ }
+
+ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+ Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const JointTranslation::ConstraintTranslationSubspace & S)
+ {
+ Eigen::Matrix <double, 6, 3> M;
+ M.block <3,3> (Inertia::ANGULAR, 0) = alphaSkew(Y.mass (), Y.lever ());
+ // M.block <3,3> (Inertia::LINEAR, 0) = Y.mass () * Inertia::Matrix3::Identity ();
+ M.block <3,3> (Inertia::LINEAR, 0).setZero ();
+ M.block <3,3> (Inertia::LINEAR, 0).diagonal ().fill (Y.mass ());
+
+ return M;
+ }
+
+ namespace internal
+ {
+ template<>
+ struct ActionReturn<JointTranslation::ConstraintTranslationSubspace >
+ { typedef Eigen::Matrix<double,6,3> Type; };
+ }
+
+
+ template<>
+ struct traits<JointTranslation>
+ {
+ typedef JointDataTranslation JointData;
+ typedef JointModelTranslation JointModel;
+ typedef JointTranslation::ConstraintTranslationSubspace Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef JointTranslation::MotionTranslation Motion_t;
+ typedef JointTranslation::BiasZero Bias_t;
+ typedef Eigen::Matrix<double,6,3> F_t;
+ enum {
+ NQ = 3,
+ NV = 3
+ };
+ };
+
+ template<> struct traits<JointDataTranslation> { typedef JointTranslation Joint; };
+ template<> struct traits<JointModelTranslation> { typedef JointTranslation Joint; };
+
+ struct JointDataTranslation : public JointDataBase<JointDataTranslation>
+ {
+ typedef JointTranslation Joint;
+ SE3_JOINT_TYPEDEF;
+
+ typedef Eigen::Matrix<double,6,6> Matrix6;
+ typedef Eigen::Matrix<double,3,3> Matrix3;
+ typedef Eigen::Matrix<double,3,1> Vector3;
+
+ Constraint_t S;
+ Transformation_t M;
+ Motion_t v;
+ Bias_t c;
+
+ JointDataTranslation () : M(1) {}
+ };
+
+ struct JointModelTranslation : public JointModelBase<JointModelTranslation>
+ {
+ typedef JointTranslation Joint;
+ SE3_JOINT_TYPEDEF;
+
+ JointData createData() const { return JointData(); }
+
+ void calc (JointData & data,
+ const Eigen::VectorXd & qs) const
+ {
+ data.M.translation (qs.segment<NQ>(idx_q ()));
+ }
+ void calc (JointData & data,
+ const Eigen::VectorXd & qs,
+ const Eigen::VectorXd & vs ) const
+ {
+ data.M.translation (qs.segment<NQ> (idx_q ()));
+ data.v () = vs.segment<NQ> (idx_v ());
+ }
+ };
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_translation_hpp__
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 50bfd28d9..951bbd122 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -7,12 +7,13 @@
#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-translation.hpp"
#include "pinocchio/multibody/joint/joint-free-flyer.hpp"
namespace se3
{
- 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 boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation> JointModelVariant;
+ typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation> JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index 25f84f77c..cae6b6e6e 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -9,6 +9,7 @@
#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-translation.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
@@ -406,6 +407,205 @@ BOOST_AUTO_TEST_CASE ( test_crba )
BOOST_AUTO_TEST_SUITE_END ()
+BOOST_AUTO_TEST_SUITE ( JointTranslation )
+
+BOOST_AUTO_TEST_CASE ( test_kinematics )
+{
+ using namespace se3;
+
+ typedef Motion::Vector3 Vector3;
+ typedef Motion::Vector6 Vector6;
+
+ Motion expected_v_J (Motion::Zero ());
+ Motion expected_c_J (Motion::Zero ());
+
+ SE3 expected_configuration (SE3::Identity ());
+
+ JointDataTranslation joint_data;
+ JointModelTranslation joint_model;
+
+ joint_model.setIndexes (0, 0, 0);
+
+ Vector3 q (Vector3::Zero());
+ Vector3 q_dot (Vector3::Zero());
+
+ // -------
+ q = Vector3 (0., 0., 0.);
+ q_dot = Vector3 (0., 0., 0.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+
+ // -------
+ q = Vector3 (1., 0., 0.);
+ q_dot = Vector3 (1., 0., 0.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ expected_configuration.translation () << 1, 0, 0;
+
+ expected_v_J.linear () << 1., 0., 0.;
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+ // -------
+ q = Vector3 (0., 1., 0.);
+ q_dot = Vector3 (0., 1., 0.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ expected_configuration.translation () << 0, 1., 0;
+
+ expected_v_J.linear () << 0., 1., 0.;
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+ // -------
+ q = Vector3 (0., 0., 1.);
+ q_dot = Vector3 (0., 0., 1.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ expected_configuration.translation () << 0, 0, 1;
+
+ expected_v_J.linear () << 0., 0., 1.;
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+ // -------
+ q = Vector3 (1., 1., 1.);
+ q_dot = Vector3 (1., 1., 1.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ expected_configuration.translation () << 1., 1., 1.;
+ expected_v_J.linear () << 1., 1., 1.;
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+
+ // -------
+ q = Vector3 (1., 1.5, 1.9);
+ q_dot = Vector3 (2., 3., 1.);
+
+ joint_model.calc (joint_data, q, q_dot);
+
+ printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+ expected_configuration.translation () = q;
+ expected_v_J.linear () = q_dot;
+
+ is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+ is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+ is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+ is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+}
+
+BOOST_AUTO_TEST_CASE ( test_rnea )
+{
+ using namespace se3;
+ typedef Eigen::Matrix <double, 3, 1> Vector3;
+ typedef Eigen::Matrix <double, 3, 3> Matrix3;
+
+ Model model;
+ Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+
+ model.addBody (model.getBodyId("universe"), JointModelTranslation (), SE3::Identity (), inertia, "root");
+
+ Data data (model);
+
+ Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
+ Eigen::VectorXd v (Eigen::VectorXd::Zero (model.nv));
+ Eigen::VectorXd a (Eigen::VectorXd::Zero (model.nv));
+
+ rnea (model, data, q, v, a);
+ Vector3 tau_expected (Vector3::Zero ());
+
+ tau_expected << 0, 0, 9.81;
+
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-14);
+
+ q = Eigen::VectorXd::Ones (model.nq);
+ v = Eigen::VectorXd::Ones (model.nv);
+ a = Eigen::VectorXd::Ones (model.nv);
+
+ rnea (model, data, q, v, a);
+ tau_expected << 1, 1, 10.81;
+
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+
+ q << 3, 2, 1;
+ v = Eigen::VectorXd::Ones (model.nv);
+ a = Eigen::VectorXd::Ones (model.nv);
+
+ rnea (model, data, q, v, a);
+ tau_expected << 1, 1, 10.81;
+
+ is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+}
+
+BOOST_AUTO_TEST_CASE ( test_crba )
+{
+ using namespace se3;
+ using namespace std;
+ typedef Eigen::Matrix <double, 3, 1> Vector3;
+ typedef Eigen::Matrix <double, 3, 3> Matrix3;
+
+ Model model;
+ Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+
+ model.addBody (model.getBodyId("universe"), JointModelTranslation (), SE3::Identity (), inertia, "root");
+
+ Data data (model);
+
+ Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
+ Eigen::MatrixXd M_expected (model.nv,model.nv);
+
+ crba (model, data, q);
+ M_expected = Matrix3::Identity ();
+
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+
+ q = Eigen::VectorXd::Ones (model.nq);
+
+ crba (model, data, q);
+
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+
+ q << 3, 2, 1;
+
+ crba (model, data, q);
+
+ is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
BOOST_AUTO_TEST_SUITE ( JointSpherical )
--
GitLab