diff --git a/CMakeLists.txt b/CMakeLists.txt
index 085b1c3576affffad41c2ee67ddd7db4d16e1449..fbbd4479708d6edd2b2e1aef6d6459610657d332 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-base.hpp
multibody/joint/joint-revolute.hpp
multibody/joint/joint-spherical.hpp
+ multibody/joint/joint-spherical-ZYX.hpp
multibody/joint/joint-prismatic.hpp
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
diff --git a/src/multibody/joint.hpp b/src/multibody/joint.hpp
index c59e17f8b664e888f0a6ae78e798b64e2a126cab..d04431d5652ac12532bd88c44c9bb828a3d2a4f8 100644
--- a/src/multibody/joint.hpp
+++ b/src/multibody/joint.hpp
@@ -4,6 +4,7 @@
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#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-free-flyer.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..13d69600d673439a9586ffa9fdf186d6cd296fb9
--- /dev/null
+++ b/src/multibody/joint/joint-spherical-ZYX.hpp
@@ -0,0 +1,258 @@
+#ifndef __se3_joint_spherical_ZYX_hpp__
+#define __se3_joint_spherical_ZYX_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/math/sincos.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/skew.hpp"
+
+namespace se3
+{
+
+ struct JointDataSphericalZYX;
+ struct JointModelSphericalZYX;
+
+ struct JointSphericalZYX
+ {
+ struct BiasSpherical
+ {
+ Motion::Vector3 c_J;
+
+ BiasSpherical () {c_J.fill (NAN);}
+ //BiasSpherical (const Motion::Vector3 & c_J) c_J (c_J) {}
+
+ operator Motion () const { return Motion (Motion::Vector3::Zero (), c_J); }
+
+ Motion::Vector3 & operator() () { return c_J; }
+ const Motion::Vector3 & operator() () const { return c_J; }
+
+
+ }; // struct BiasSpherical
+
+ friend const Motion operator+ (const Motion & v, const BiasSpherical & c) { return Motion (v.linear (), v.angular () + c ()); }
+ friend const Motion operator+ (const BiasSpherical & c, const Motion & v) { return Motion (v.linear (), v.angular () + c ()); }
+
+ struct MotionSpherical
+ {
+ MotionSpherical () {w.fill(NAN);}
+ MotionSpherical (const Motion::Vector3 & w) : w (w) {}
+ Motion::Vector3 w;
+
+ Motion::Vector3 & operator() () { return w; }
+ const Motion::Vector3 & operator() () const { return w; }
+
+ operator Motion() const
+ {
+ return Motion (Motion::Vector3::Zero (), w);
+ }
+ }; // struct MotionSpherical
+
+ friend const MotionSpherical operator+ (const MotionSpherical & m, const BiasSpherical & c)
+ { return MotionSpherical (m.w + c.c_J); }
+
+ friend Motion operator+ (const MotionSpherical & m1, const Motion & m2)
+ {
+ return Motion( m2.linear(),m2.angular() + m1.w);
+ }
+
+ struct ConstraintRotationalSubspace
+ {
+ public:
+ typedef Motion::Scalar Scalar;
+ typedef Eigen::Matrix <Motion::Scalar,3,3> Matrix3;
+
+ public:
+ Matrix3 S_minimal;
+
+ Motion operator* (const MotionSpherical & vj) const
+ { return Motion (Motion::Vector3::Zero (), S_minimal * vj ()); }
+
+ ConstraintRotationalSubspace () : S_minimal () { S_minimal.fill (NAN); }
+ ConstraintRotationalSubspace (const Matrix3 & subspace) : S_minimal (subspace) {}
+
+ Matrix3 & operator() () { return S_minimal; }
+ const Matrix3 & operator() () const { return S_minimal; }
+
+ Matrix3 & matrix () { return S_minimal; }
+ const Matrix3 & matrix () const { return S_minimal; }
+
+ struct ConstraintTranspose
+ {
+ const ConstraintRotationalSubspace & ref;
+ ConstraintTranspose(const ConstraintRotationalSubspace & ref) : ref(ref) {}
+
+ Force::Vector3 operator* (const Force & phi)
+ {
+ return ref.S_minimal.transpose () * phi.angular ();
+ }
+
+
+ /* [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 & constraint_transpose, const Eigen::MatrixBase<D> & F )
+ {
+ assert(F.rows()==6);
+ return constraint_transpose.ref.S_minimal.transpose () * F.template middleRows <3> (Inertia::ANGULAR);
+ }
+ }; // struct ConstraintTranspose
+
+ ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+
+ operator ConstraintXd () const
+ {
+ Eigen::Matrix<Scalar,6,3> S;
+ S.block <3,3> (Inertia::LINEAR, 0).setZero ();
+ S.block <3,3> (Inertia::ANGULAR, 0) = S_minimal;
+ return ConstraintXd(S);
+ }
+
+ Eigen::Matrix <Scalar,6,3> se3Action (const SE3 & m) const
+ {
+ Eigen::Matrix <Scalar,6,3> X_subspace;
+ X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
+ X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
+
+ return X_subspace * S_minimal;
+ }
+
+ }; // struct ConstraintRotationalSubspace
+
+ template<typename D>
+ friend Motion operator* (const ConstraintRotationalSubspace & S, const Eigen::MatrixBase<D> & v)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+ return Motion (Motion::Vector3::Zero (), S () * v);
+ }
+
+ }; // struct JointSphericalZYX
+
+ Motion operator^ (const Motion & m1, const JointSphericalZYX::MotionSpherical & m2)
+ {
+// const Motion::Matrix3 m2_cross (skew (Motion::Vector3 (-m2.w)));
+// return Motion(m2_cross * m1.linear (), m2_cross * m1.angular ());
+ return Motion(m1.linear ().cross (m2.w), m1.angular ().cross (m2.w));
+ }
+
+ /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+ Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointSphericalZYX::ConstraintRotationalSubspace & S)
+ {
+ Eigen::Matrix <Inertia::Scalar, 6, 3> M;
+// M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
+ M.block <3,3> (Inertia::LINEAR, 0) = alphaSkew ( -Y.mass (), Y.lever ());
+ M.block <3,3> (Inertia::ANGULAR, 0) = (Inertia::Matrix3)(Y.inertia () - typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ()));
+ return M * S.matrix ();
+ }
+
+ namespace internal
+ {
+ template<>
+ struct ActionReturn<JointSphericalZYX::ConstraintRotationalSubspace >
+ { typedef Eigen::Matrix<double,6,3> Type; };
+ }
+
+ template<>
+ struct traits<JointSphericalZYX>
+ {
+ typedef JointDataSphericalZYX JointData;
+ typedef JointModelSphericalZYX JointModel;
+ typedef JointSphericalZYX::ConstraintRotationalSubspace Constraint_t;
+ typedef SE3 Transformation_t;
+ typedef JointSphericalZYX::MotionSpherical Motion_t;
+ typedef JointSphericalZYX::BiasSpherical Bias_t;
+ typedef Eigen::Matrix<double,6,3> F_t;
+ enum {
+ NQ = 3,
+ NV = 3
+ };
+ };
+ template<> struct traits<JointDataSphericalZYX> { typedef JointSphericalZYX Joint; };
+ template<> struct traits<JointModelSphericalZYX> { typedef JointSphericalZYX Joint; };
+
+ struct JointDataSphericalZYX : public JointDataBase<JointDataSphericalZYX>
+ {
+ typedef JointSphericalZYX 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;
+
+ JointDataSphericalZYX () : M(1)
+ {
+ M.translation (Transformation_t::Vector3::Zero ());
+ }
+ };
+
+ struct JointModelSphericalZYX : public JointModelBase<JointModelSphericalZYX>
+ {
+ typedef JointSphericalZYX 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 c0,s0; SINCOS (q(0), &s0, &c0);
+ double c1,s1; SINCOS (q(1), &s1, &c1);
+ double c2,s2; SINCOS (q(2), &s2, &c2);
+
+ data.M.rotation () << c0 * c1,
+ c0 * s1 * s2 - s0 * c2,
+ c0 * s1 * c2 + s0 * s2,
+ s0 * c1,
+ s0 * s1 * s2 + c0 * c2,
+ s0 * s1 * c2 - c0 * s2,
+ -s1,
+ c1 * s2,
+ c1 * c2;
+
+ data.S.matrix () << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
+ }
+
+ 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 c0,s0; SINCOS (q(0), &s0, &c0);
+ double c1,s1; SINCOS (q(1), &s1, &c1);
+ double c2,s2; SINCOS (q(2), &s2, &c2);
+
+ data.M.rotation () << c0 * c1,
+ c0 * s1 * s2 - s0 * c2,
+ c0 * s1 * c2 + s0 * s2,
+ s0 * c1,
+ s0 * s1 * s2 + c0 * c2,
+ s0 * s1 * c2 - c0 * s2,
+ -s1,
+ c1 * s2,
+ c1 * c2;
+
+
+ data.S.matrix () << -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
+
+ data.v () = data.S.matrix () * q_dot;
+
+ data.c ()(0) = -c1 * q_dot (0) * q_dot (1);
+ data.c ()(1) = -s1 * s2 * q_dot (0) * q_dot (1) + c1 * c2 * q_dot (0) * q_dot (2) - s2 * q_dot (1) * q_dot (2);
+ data.c ()(2) = -s1 * c2 * q_dot (0) * q_dot (1) - c1 * s2 * q_dot (0) * q_dot (2) - c2 * q_dot (1) * q_dot (2);
+ }
+ };
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_spherical_ZYX_hpp__
\ No newline at end of file
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index f651ae45cc9557ea89dc6bf707c6e6c168d384b5..2ae00ebaa6096549edc02cfea8df5ff9cbe46fba 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -3,13 +3,14 @@
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#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-free-flyer.hpp"
namespace se3
{
- typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelSpherical, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
- typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ , JointDataSpherical, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
+ typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
+ typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ , JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVector;
typedef std::vector<JointDataVariant> JointDataVector;
diff --git a/unittest/joints.cpp b/unittest/joints.cpp
index e426ec550ef58322ba1f0936ca7dd94317863d17..bacb3cab54dcc2ec836b05c03696cb7846093caf 100644
--- a/unittest/joints.cpp
+++ b/unittest/joints.cpp
@@ -6,6 +6,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#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/model.hpp"
#include "pinocchio/algorithm/rnea.hpp"
@@ -58,226 +59,226 @@ BOOST_AUTO_TEST_SUITE ( JointSphericalZYX )
BOOST_AUTO_TEST_CASE ( test_kinematics )
{
- // using namespace se3;
+ using namespace se3;
- // typedef Motion::Vector3 Vector3;
- // typedef Motion::Vector6 Vector6;
+ typedef Motion::Vector3 Vector3;
+ typedef Motion::Vector6 Vector6;
- // Motion expected_v_J (Motion::Zero ());
- // Motion expected_c_J (Motion::Zero ());
+ Motion expected_v_J (Motion::Zero ());
+ Motion expected_c_J (Motion::Zero ());
- // SE3 expected_configuration (SE3::Identity ());
+ SE3 expected_configuration (SE3::Identity ());
- // JointDataSphericalZYX joint_data;
- // JointModelSphericalZYX joint_model;
+ JointDataSphericalZYX joint_data;
+ JointModelSphericalZYX joint_model;
- // joint_model.setIndexes (0, 0, 0);
+ joint_model.setIndexes (0, 0, 0);
- // Vector3 q (Vector3::Zero());
- // Vector3 q_dot (Vector3::Zero());
+ Vector3 q (Vector3::Zero());
+ Vector3 q_dot (Vector3::Zero());
- // // -------
- // q = Vector3 (0., 0., 0.);
- // q_dot = Vector3 (0., 0., 0.);
+ // -------
+ q = Vector3 (0., 0., 0.);
+ q_dot = Vector3 (0., 0., 0.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
+ is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
- // // -------
- // q = Vector3 (1., 0., 0.);
- // q_dot = Vector3 (1., 0., 0.);
+ // -------
+ q = Vector3 (1., 0., 0.);
+ q_dot = Vector3 (1., 0., 0.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // expected_configuration.rotation ().transpose () <<
- // 0.54030230586814, 0.8414709848079, -0,
- // -0.8414709848079, 0.54030230586814, 0,
- // 0, 0, 1;
+ expected_configuration.rotation ().transpose () <<
+ 0.54030230586814, 0.8414709848079, -0,
+ -0.8414709848079, 0.54030230586814, 0,
+ 0, 0, 1;
- // expected_v_J.angular () << 0., 0., 1.;
+ expected_v_J.angular () << 0., 0., 1.;
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
- // // -------
- // q = Vector3 (0., 1., 0.);
- // q_dot = Vector3 (0., 1., 0.);
+ // -------
+ q = Vector3 (0., 1., 0.);
+ q_dot = Vector3 (0., 1., 0.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // expected_configuration.rotation ().transpose () <<
- // 0.54030230586814, 0, -0.8414709848079,
- // 0, 1, 0,
- // 0.8414709848079, 0, 0.54030230586814;
+ expected_configuration.rotation ().transpose () <<
+ 0.54030230586814, 0, -0.8414709848079,
+ 0, 1, 0,
+ 0.8414709848079, 0, 0.54030230586814;
- // expected_v_J.angular () << 0., 1., 0.;
+ expected_v_J.angular () << 0., 1., 0.;
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
- // // -------
- // q = Vector3 (0., 0., 1.);
- // q_dot = Vector3 (0., 0., 1.);
+ // -------
+ q = Vector3 (0., 0., 1.);
+ q_dot = Vector3 (0., 0., 1.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // expected_configuration.rotation ().transpose () <<
- // 1, 0, -0,
- // 0, 0.54030230586814, 0.8414709848079,
- // 0, -0.8414709848079, 0.54030230586814;
+ expected_configuration.rotation ().transpose () <<
+ 1, 0, -0,
+ 0, 0.54030230586814, 0.8414709848079,
+ 0, -0.8414709848079, 0.54030230586814;
- // expected_v_J.angular () << 1., 0., 0.;
+ expected_v_J.angular () << 1., 0., 0.;
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+ is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
- // // -------
- // q = Vector3 (1., 1., 1.);
- // q_dot = Vector3 (1., 1., 1.);
+ // -------
+ q = Vector3 (1., 1., 1.);
+ q_dot = Vector3 (1., 1., 1.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // expected_configuration.rotation ().transpose () <<
- // 0.29192658172643, 0.45464871341284, -0.8414709848079,
- // -0.072075012795695, 0.88774981831738, 0.45464871341284,
- // 0.95372116649051, -0.072075012795695, 0.29192658172643;
+ expected_configuration.rotation ().transpose () <<
+ 0.29192658172643, 0.45464871341284, -0.8414709848079,
+ -0.072075012795695, 0.88774981831738, 0.45464871341284,
+ 0.95372116649051, -0.072075012795695, 0.29192658172643;
- // expected_v_J.angular () << 0.1585290151921, 0.99495101928098, -0.54954440308147;
- // expected_c_J.angular () << -0.54030230586814, -1.257617821355, -1.4495997326938;
+ expected_v_J.angular () << 0.1585290151921, 0.99495101928098, -0.54954440308147;
+ expected_c_J.angular () << -0.54030230586814, -1.257617821355, -1.4495997326938;
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+ is_matrix_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.);
+ // -------
+ q = Vector3 (1., 1.5, 1.9);
+ q_dot = Vector3 (2., 3., 1.);
- // joint_model.calc (joint_data, q, q_dot);
+ joint_model.calc (joint_data, q, q_dot);
- // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+ printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
- // expected_configuration.rotation ().transpose () <<
- // 0.03821947317172, 0.059523302749877, -0.99749498660405,
- // 0.78204612603915, 0.61961526601658, 0.06693862014091,
- // 0.62204752922718, -0.7826454488138, -0.022868599288288;
+ expected_configuration.rotation ().transpose () <<
+ 0.03821947317172, 0.059523302749877, -0.99749498660405,
+ 0.78204612603915, 0.61961526601658, 0.06693862014091,
+ 0.62204752922718, -0.7826454488138, -0.022868599288288;
- // expected_v_J.angular () << -0.99498997320811, -0.83599146030869, -2.8846374616388;
- // expected_c_J.angular () << -0.42442321000622, -8.5482150213859, 2.7708697933151;
+ expected_v_J.angular () << -0.99498997320811, -0.83599146030869, -2.8846374616388;
+ expected_c_J.angular () << -0.42442321000622, -8.5482150213859, 2.7708697933151;
- // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
- // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
- // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
- // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+ is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+ is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+ is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+ is_matrix_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;
+ 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 model;
+ Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- // model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+ model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
- // Data data (model);
+ 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);
+ 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 (0., -4.905, 0.);
+ rnea (model, data, q, v, a);
+ Vector3 tau_expected (0., -4.905, 0.);
- // is_matrix_closed (tau_expected, data.tau, 1e-14);
+ is_matrix_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);
+ 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 << -0.53611600195085, -0.74621832606188, -0.38177329067604;
+ rnea (model, data, q, v, a);
+ tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
- // is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_closed (tau_expected, data.tau, 1e-12);
- // q << 3, 2, 1;
- // v = Eigen::VectorXd::Ones (model.nv);
- // a = Eigen::VectorXd::Ones (model.nv);
+ q << 3, 2, 1;
+ v = Eigen::VectorXd::Ones (model.nv);
+ a = Eigen::VectorXd::Ones (model.nv);
- // rnea (model, data, q, v, a);
- // tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
+ rnea (model, data, q, v, a);
+ tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
- // is_matrix_closed (tau_expected, data.tau, 1e-12);
+ is_matrix_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;
+ 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 model;
+ Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
- // model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+ model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
- // Data data (model);
+ Data data (model);
- // Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
- // Eigen::MatrixXd M_expected (model.nv,model.nv);
+ Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
+ Eigen::MatrixXd M_expected (model.nv,model.nv);
- // crba (model, data, q);
- // M_expected <<
- // 1.25, 0, 0,
- // 0, 1.25, 0,
- // 0, 0, 1;
+ crba (model, data, q);
+ M_expected <<
+ 1.25, 0, 0,
+ 0, 1.25, 0,
+ 0, 0, 1;
- // is_matrix_closed (M_expected, data.M, 1e-14);
+ is_matrix_closed (M_expected, data.M, 1e-14);
- // q = Eigen::VectorXd::Ones (model.nq);
+ q = Eigen::VectorXd::Ones (model.nq);
- // crba (model, data, q);
- // M_expected <<
- // 1.0729816454316, -5.5511151231258e-17, -0.8414709848079,
- // -5.5511151231258e-17, 1.25, 0,
- // -0.8414709848079, 0, 1;
+ crba (model, data, q);
+ M_expected <<
+ 1.0729816454316, -5.5511151231258e-17, -0.8414709848079,
+ -5.5511151231258e-17, 1.25, 0,
+ -0.8414709848079, 0, 1;
- // is_matrix_closed (M_expected, data.M, 1e-12);
+ is_matrix_closed (M_expected, data.M, 1e-12);
- // q << 3, 2, 1;
+ q << 3, 2, 1;
- // crba (model, data, q);
- // M_expected <<
- // 1.043294547392, 2.7755575615629e-17, -0.90929742682568,
- // 0, 1.25, 0,
- // -0.90929742682568, 0, 1;
+ crba (model, data, q);
+ M_expected <<
+ 1.043294547392, 2.7755575615629e-17, -0.90929742682568,
+ 0, 1.25, 0,
+ -0.90929742682568, 0, 1;
- // is_matrix_closed (M_expected, data.M, 1e-10);
+ is_matrix_closed (M_expected, data.M, 1e-10);
}
BOOST_AUTO_TEST_SUITE_END ()