Commit be3c965c authored by jcarpent's avatar jcarpent
Browse files

[Incremental][C++] Add complete translational joint

parent 92d90193
......@@ -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
......
#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__
......@@ -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;
......
......@@ -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 )
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment