diff --git a/src/algorithm/crba.hxx b/src/algorithm/crba.hxx
index 4c7b0e451bdb4d6edf3f545bb29c6e4826c6fea5..74d0eddaa845a86363dd914252aa535acb62b18d 100644
--- a/src/algorithm/crba.hxx
+++ b/src/algorithm/crba.hxx
@@ -161,7 +161,7 @@ namespace se3
const se3::Model & model,
se3::Data & data)
{
- typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
+ typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
@@ -169,19 +169,33 @@ namespace se3
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
- if (JointModel::NV == -1)
- {
- ColsBlock jF
- = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
- }
- else
- {
- ColsBlock jF
+
+ ColsBlock jF
= data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
- }
+
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
+ static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
+ se3::JointDataBase<JointDataComposite> & jdata,
+ const se3::Model & model,
+ se3::Data & data)
+ {
+ typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
+
+ const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
+ const Model::Index & parent = model.parents[i];
+
+ data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
+
+ jdata.U() = data.Ycrb[i] * jdata.S();
+
+ ColsBlock jF
+ = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
+
+ forceSet::se3Action(data.oMi[i],jdata.U(),jF);
+ }
+
}; // struct CcrbaBackwardStep
inline const Data::Matrix6x &
diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp
index 9aa612990297fa4b1dc0716b4d73823922386e28..bcaf830478692801d41ae4b37941a6221ce5ab8f 100644
--- a/src/multibody/joint/joint-basic-visitors.hpp
+++ b/src/multibody/joint/joint-basic-visitors.hpp
@@ -19,9 +19,9 @@
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
-#include <boost/variant.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
+
namespace se3
{
@@ -108,6 +108,16 @@ namespace se3
const double u);
+ /**
+ * @brief Visit a JointModelVariant through JointRandomVisitor to
+ * generate a random configuration vector
+ *
+ * @param[in] jmodel The JointModelVariant
+ *
+ * @return The joint randomconfiguration
+ */
+ inline Eigen::VectorXd random(const JointModelVariant & jmodel);
+
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
@@ -343,7 +353,8 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
-#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
+// Included later
+// #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#endif // ifndef __se3_joint_basic_visitors_hpp__
diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx
index ecb549b4ea5f511dc97d3660356501e682d1acca..a155bb2d21d3c8ef9df270381c24e3abaf5a7316 100644
--- a/src/multibody/joint/joint-basic-visitors.hxx
+++ b/src/multibody/joint/joint-basic-visitors.hxx
@@ -19,7 +19,8 @@
#define __se3_joint_basic_visitors_hxx__
#include "pinocchio/assert.hpp"
-#include "pinocchio/multibody/joint/joint-variant.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/visitor.hpp"
namespace se3
@@ -192,6 +193,25 @@ namespace se3
return JointInterpolateVisitor::run(jmodel, q0, q1, u);
}
+ /**
+ * @brief JointRandomVisitor visitor
+ */
+ class JointRandomVisitor: public boost::static_visitor<Eigen::VectorXd>
+ {
+ public:
+
+ template<typename D>
+ Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.random(); }
+
+ static Eigen::VectorXd run(const JointModelVariant & jmodel)
+ { return boost::apply_visitor( JointRandomVisitor(), jmodel ); }
+ };
+ inline Eigen::VectorXd random(const JointModelVariant & jmodel)
+ {
+ return JointRandomVisitor::run(jmodel);
+ }
+
/**
* @brief JointRandomConfigurationVisitor visitor
*/
@@ -348,8 +368,8 @@ namespace se3
{
public:
template<typename D>
- int operator()(const JointModelBase<D> & ) const
- { return D::NV; }
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.nv(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNvVisitor(), jmodel ); }
@@ -364,8 +384,8 @@ namespace se3
{
public:
template<typename D>
- int operator()(const JointModelBase<D> & ) const
- { return D::NQ; }
+ int operator()(const JointModelBase<D> & jmodel) const
+ { return jmodel.nq(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNqVisitor(), jmodel ); }
diff --git a/src/multibody/joint/joint-composite.hpp b/src/multibody/joint/joint-composite.hpp
index 8a5dfac510e51d502d2ab60d3efe955f4fe5aa57..5db56f822c5d8c9f69022c068ba478fd43d50418 100644
--- a/src/multibody/joint/joint-composite.hpp
+++ b/src/multibody/joint/joint-composite.hpp
@@ -19,7 +19,8 @@
#define __se3_joint_composite_hpp__
#include "pinocchio/assert.hpp"
-#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+// #include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+#include "pinocchio/multibody/joint/joint.hpp"
namespace se3
{
@@ -36,8 +37,9 @@ namespace se3
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
- typedef JointDataComposite JointData;
- typedef JointModelComposite JointModel;
+ typedef double Scalar;
+ typedef JointDataComposite JointDataDerived;
+ typedef JointModelComposite JointModelDerived;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
@@ -52,8 +54,8 @@ namespace se3
typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
};
- template<> struct traits<JointDataComposite> { typedef JointComposite Joint; };
- template<> struct traits<JointModelComposite> { typedef JointComposite Joint; };
+ template<> struct traits<JointDataComposite> { typedef JointComposite JointDerived; };
+ template<> struct traits<JointModelComposite> { typedef JointComposite JointDerived; };
struct JointDataComposite : public JointDataBase<JointDataComposite>
{
@@ -121,7 +123,7 @@ namespace se3
, nq_composite(jmodel1.nq())
, nv_composite(jmodel1.nv())
{
- joints.push_back(jmodel1.derived());
+ joints.push_back(JointModel(jmodel1.derived()));
}
template <typename D1, typename D2 >
@@ -131,8 +133,8 @@ namespace se3
, nq_composite(jmodel1.nq() + jmodel2.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv())
{
- joints.push_back(jmodel1.derived());
- joints.push_back(jmodel2.derived());
+ joints.push_back(JointModel(jmodel1.derived()));
+ joints.push_back(JointModel(jmodel2.derived()));
}
template <typename D1, typename D2, typename D3 >
@@ -144,9 +146,9 @@ namespace se3
, nq_composite(jmodel1.nq() + jmodel2.nq() + jmodel3.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv() + jmodel3.nv())
{
- joints.push_back(jmodel1.derived());
- joints.push_back(jmodel2.derived());
- joints.push_back(jmodel3.derived());
+ joints.push_back(JointModel(jmodel1.derived()));
+ joints.push_back(JointModel(jmodel2.derived()));
+ joints.push_back(JointModel(jmodel3.derived()));
}
// JointModelComposite( const JointModelVector & models ) : max_joints(models.size()) , joints(models) {}
@@ -157,7 +159,7 @@ namespace se3
std::size_t nb_joints = joints.size();
if (!isFull())
{
- joints.push_back(jmodel.derived());
+ joints.push_back(JointModel(jmodel.derived()));
nq_composite += jmodel.nq();
nv_composite += jmodel.nv();
nb_joints = joints.size();
@@ -174,18 +176,18 @@ namespace se3
{
return joints.size() <= 0 ;
}
- JointData createData() const
+ JointDataDerived createData() const
{
JointDataVector res;
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
res.push_back(::se3::createData(*i));
}
- return JointData(res, nq_composite, nv_composite);
+ return JointDataDerived(res, nq_composite, nv_composite);
}
- void calc (JointData & data,
+ void calc (JointDataDerived & data,
const Eigen::VectorXd & qs) const
{
data.M.setIdentity();
@@ -197,24 +199,34 @@ namespace se3
}
}
- void calc (JointData & data,
+ void calc (JointDataDerived & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
data.M.setIdentity();
data.v.setZero();
+ data.c.setZero();
+ data.S.matrix().setZero();
+ std::cout << "Size of joint stack: " << data.joints.size() << std::endl;
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
+ std::cout << "i = " << index << std::endl;
+ std::cout << "Index in joint composite : \t" << index << std::endl;
+ std::cout << "nq of contained joint : \t" << ::se3::nq(joints[(std::size_t)index]) << std::endl;
calc_first_order(joints[(std::size_t)index], *i, qs, vs);
data.M = data.M * joint_transform(*i);
}
- SE3 iMcomposite(SE3::Identity());
data.v = motion(data.joints[joints.size()-1]);
+ data.c = bias(data.joints[joints.size()-1]);
+ // data.S = constraint_xd(data.joints[joints.size()-1])
int start_col = nv_composite;
int sub_constraint_dimension = (int)constraint_xd(data.joints[joints.size()-1]).matrix().cols();
+ start_col -= sub_constraint_dimension;
+ data.S.matrix().middleCols(start_col,sub_constraint_dimension) = constraint_xd(data.joints[joints.size()-1]).matrix();
+ SE3 iMcomposite(SE3::Identity());
for (JointDataVector::reverse_iterator i = data.joints.rbegin()+1; i != data.joints.rend(); ++i)
{
sub_constraint_dimension = (int)constraint_xd(*i).matrix().cols();
@@ -224,13 +236,13 @@ namespace se3
data.v += iMcomposite.actInv(motion(*i));
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*i));
- // Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
- // data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
+ Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
+ data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
}
}
- void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
+ void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
// calc has been called previously in abaforwardstep1 so data.M, data.v are up to date
data.U.setZero();
@@ -239,7 +251,7 @@ namespace se3
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
- ::se3::calc_aba(joints[(std::size_t)index], *i, I, false);
+ ::se3::calc_aba(joints[(std::size_t)index], *i, I, false); // HERE MEMORY ACCES VIOLATION WHEN VISITING A JOINT(JOINTMODELCOMPOSITE). SEE UNITTEST JOINT
}
data.U = I * data.S;
Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
@@ -270,6 +282,16 @@ namespace se3
return result;
}
+ ConfigVector_t random_impl() const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::random(*i);
+ }
+ return result;
+ }
+
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
{
ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
@@ -299,6 +321,34 @@ namespace se3
return difference_impl(q0,q1).norm();
}
+ void normalize_impl(Eigen::VectorXd & q) const
+ {
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ ::se3::normalize(*i,q);
+ }
+ }
+
+ ConfigVector_t neutralConfiguration_impl() const
+ {
+ ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::neutralConfiguration(*i);
+ }
+ return result;
+ }
+
+ bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
+ {
+ for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
+ {
+ if ( !::se3::isSameConfiguration(*i, q1, q2) )
+ return false;
+ }
+ return true;
+ }
+
int nv_impl() const { return nv_composite; }
int nq_impl() const { return nq_composite; }
@@ -320,10 +370,8 @@ namespace se3
}
}
- static const std::string shortname()
- {
- return std::string("JointModelComposite");
- }
+ static std::string classname() { return std::string("JointModelComposite"); }
+ std::string shortname() const { return classname(); }
template <class D>
bool operator == (const JointModelBase<D> &) const
@@ -338,8 +386,22 @@ namespace se3
&& jmodel.idx_v() == idx_v();
}
+ // see http://en.cppreference.com/w/cpp/language/copy_assignment#Copy-and-swap_idiom
+ void swap(JointModelComposite & other) {
+ max_joints = other.max_joints;
+ nq_composite = other.nq_composite;
+ nv_composite = other.nv_composite;
+
+ joints.swap(other.joints);
+ }
+
+ JointModelComposite& operator=(JointModelComposite other)
+ {
+ swap(other);
+ return *this;
+ }
-template<typename D>
+ template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
@@ -360,25 +422,25 @@ template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols(Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite); }
-template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
- jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
- template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
- jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
- template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
- jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
- template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
- jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
-
- template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
- jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
- template<typename D>
- typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
- jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+ jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+ jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
+ jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
+ jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
+
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
+ jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
+ template<typename D>
+ typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
+ jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
};
diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp
index 10a806addf1085e313137f8611a58e2a669bfab1..fc6d001d82387774b74a88434ef2573c2c37a96e 100644
--- a/src/multibody/joint/joint-prismatic.hpp
+++ b/src/multibody/joint/joint-prismatic.hpp
@@ -482,7 +482,7 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
- ConfigVector_t result;
+ TangentVector_t result;
result << (q_1 - q_0);
return result;
}
diff --git a/src/multibody/joint/joint-variant.hpp b/src/multibody/joint/joint-variant.hpp
index 28f78e57898a9c94e543b81942663d2ba0c6ae5c..64fdacff960090717316251109dbc34576314bfa 100644
--- a/src/multibody/joint/joint-variant.hpp
+++ b/src/multibody/joint/joint-variant.hpp
@@ -31,7 +31,6 @@
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
-#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index 84f2637d437996b16e871c3b46baad7acdfb6a2a..34b8f2074ece0f9719556c9bec3234c805e89a94 100644
--- a/src/multibody/joint/joint.hpp
+++ b/src/multibody/joint/joint.hpp
@@ -20,7 +20,7 @@
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
-#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
+#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
namespace se3
{
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 96505bf3e7594a7fb0d117422ebaa6dd9a88672a..d13ebf78bc9c8d7b8d5d093f182ebca6bce0b3ab 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -26,7 +26,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
-#include "pinocchio/multibody/joint/joint.hpp"
+// #include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/tools/string-generator.hpp"
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 3b443d996be39855387fe2d5bffcdd8df7f3109d..1a3d3256c00d7805b8f0581f41cd5ae4452e82b8 100644
--- a/src/multibody/model.hxx
+++ b/src/multibody/model.hxx
@@ -85,12 +85,12 @@ namespace se3
nq += joint_model.nq();
nv += joint_model.nv();
- if (D::NV == -1)
+ if (D::NV == Eigen::Dynamic)
{
- effortLimit.conservativeResize(nv);effortLimit.bottomRows(j.nv()) = max_effort;
- velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(j.nv()) = max_velocity;
- lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(j.nq()) = min_config;
- upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(j.nq()) = max_config;
+ effortLimit.conservativeResize(nv);effortLimit.bottomRows(joint_model.nv()) = max_effort;
+ velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(joint_model.nv()) = max_velocity;
+ lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(joint_model.nq()) = min_config;
+ upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(joint_model.nq()) = max_config;
}
else
{
diff --git a/src/multibody/visitor.hpp b/src/multibody/visitor.hpp
index d0cce5bb57d86d4d30e86f65602b7f0cb5245ad6..e430fd5add59deb5a68f728b6ba08fd794694107 100644
--- a/src/multibody/visitor.hpp
+++ b/src/multibody/visitor.hpp
@@ -61,6 +61,16 @@ namespace se3
static_cast<const Visitor*>(this)->args));
}
+ // void operator() (const JointModelBase<JointModelComposite> & jmodel) const
+ // {
+ // JointDataVariant& jdataSpec = static_cast<const Visitor*>(this)->jdata;
+
+ // bf::invoke(&Visitor::template algo<JointModelComposite>,
+ // bf::append2(jmodel,
+ // boost::ref(boost::get<JointDataComposite>(jdataSpec)),
+ // static_cast<const Visitor*>(this)->args));
+ // }
+
template<typename ArgsTmp>
static void run(const JointModelVariant & jmodel,
JointDataVariant & jdata,
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index 86e5d286e69f53ea507d1b5e5a893c583221e5c0..9b72c807d488027f3236adf64090bd85529c49df 100644
--- a/src/parsers/urdf/model.cpp
+++ b/src/parsers/urdf/model.cpp
@@ -135,6 +135,21 @@ namespace se3
appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
}
+ ///
+ /// \brief Handle the case of JointModelComposite which is dynamic.
+ ///
+ void addJointAndBody(Model & model, const JointModelBase< JointModelComposite > & jmodel, const Model::JointIndex parent_id,
+ const SE3 & joint_placement, const std::string & joint_name,
+ const Inertia & Y, const std::string & body_name)
+ {
+ Model::JointIndex idx;
+
+ idx = model.addJoint(parent_id,jmodel,
+ joint_placement,joint_name);
+
+ model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
+ }
+
///
/// \brief Recursive procedure for reading the URDF tree.
/// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp
index a454c2d2f8063b848b8b2ff879d222423685408a..bf8f2d00440384992fea08d2c6aacd90a9ff027a 100644
--- a/unittest/joint-composite.cpp
+++ b/unittest/joint-composite.cpp
@@ -17,7 +17,7 @@
#include "pinocchio/tools/timer.hpp"
-#include "pinocchio/multibody/joint/joint-accessor.hpp"
+// #include "pinocchio/multibody/joint/joint-accessor.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/aba.hpp"
@@ -31,55 +31,160 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
+
+template <typename T>
+void test_joint_methods (T & jmodel)
+{
+ using namespace se3;
+
+ typename T::JointDataDerived jdata = jmodel.createData();
+
+ // JointModelComposite jmodel_composite((T()));
+ JointModelComposite jmodel_composite(jmodel);
+ jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v());
+ jmodel_composite.updateComponentsIndexes();
+
+ JointDataComposite jdata_composite = jmodel_composite.createData();
+
+ Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
+ Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
+ Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));
+ double u = 0.3;
+ se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
+ bool update_I = false;
+
+ jmodel.calc(jdata, q1, q1_dot);
+ jmodel_composite.calc(jdata_composite, q1, q1_dot);
+
+
+ std::string error_prefix("Joint Model Composite on " + jmodel.shortname());
+
+ BOOST_CHECK_MESSAGE(jmodel.nq() == jmodel_composite.nq() ,std::string(error_prefix + " - nq "));
+ BOOST_CHECK_MESSAGE(jmodel.nv() == jmodel_composite.nv() ,std::string(error_prefix + " - nv "));
+
+ BOOST_CHECK_MESSAGE(jmodel.idx_q() == jmodel_composite.idx_q() ,std::string(error_prefix + " - Idx_q "));
+ BOOST_CHECK_MESSAGE(jmodel.idx_v() == jmodel_composite.idx_v() ,std::string(error_prefix + " - Idx_v "));
+ BOOST_CHECK_MESSAGE(jmodel.id() == jmodel_composite.id() ,std::string(error_prefix + " - JointId "));
+
+ BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(jmodel_composite.integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
+ BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(jmodel_composite.interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
+ BOOST_CHECK_MESSAGE(jmodel.randomConfiguration( -1 * Eigen::VectorXd::Ones(jmodel.nq()),
+ Eigen::VectorXd::Ones(jmodel.nq())).size()
+ == jmodel_composite.randomConfiguration(-1 * Eigen::VectorXd::Ones(jmodel.nq()),
+ Eigen::VectorXd::Ones(jmodel.nq())).size()
+ ,std::string(error_prefix + " - RandomConfiguration dimensions "));
+
+ BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(jmodel_composite.difference(q1,q2)) ,std::string(error_prefix + " - difference "));
+ BOOST_CHECK_MESSAGE(jmodel.distance(q1,q2) == jmodel_composite.distance(q1,q2) ,std::string(error_prefix + " - distance "));
+
+ // pb call-operator car jdata directement le type derivé
+ // BOOST_CHECK_MESSAGE((jdata.S().matrix()).isApprox((jdata_composite.S().matrix())),std::string(error_prefix + " - ConstraintXd "));
+ BOOST_CHECK_MESSAGE(((ConstraintXd)jdata.S).matrix().isApprox((jdata_composite.S.matrix())),std::string(error_prefix + " - ConstraintXd "));
+ BOOST_CHECK_MESSAGE(jdata.M == jdata_composite.M, std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
+ BOOST_CHECK_MESSAGE((Motion)jdata.v == jdata_composite.v, std::string(error_prefix + " - Joint motions "));
+ BOOST_CHECK_MESSAGE((Motion)jdata.c == jdata_composite.c, std::string(error_prefix + " - Joint bias "));
+
+ jmodel.calc_aba(jdata, Ia, update_I);
+ jmodel_composite.calc_aba(jdata_composite, Ia, update_I);
+
+ BOOST_CHECK_MESSAGE((jdata.U).isApprox(jdata_composite.U), std::string(error_prefix + " - Joint U inertia matrix decomposition "));
+ BOOST_CHECK_MESSAGE((jdata.Dinv).isApprox(jdata_composite.Dinv), std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
+ BOOST_CHECK_MESSAGE((jdata.UDinv).isApprox(jdata_composite.UDinv), std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
+
+
+}
+
+struct TestJointComposite{
+
+ template <typename T>
+ void operator()(const T ) const
+ {
+ T jmodel;
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+ template <int NQ, int NV>
+ void operator()(const se3::JointModelDense<NQ,NV> & ) const
+ {
+ // Not yet correctly implemented, test has no meaning for the moment
+ }
+
+ void operator()(const se3::JointModelComposite & ) const
+ {
+ se3::JointModelComposite jmodel_composite_rx(2);
+ jmodel_composite_rx.addJointModel(se3::JointModelRX());
+ jmodel_composite_rx.addJointModel(se3::JointModelRY());
+ jmodel_composite_rx.setIndexes(1,0,0);
+ jmodel_composite_rx.updateComponentsIndexes();
+
+ test_joint_methods(jmodel_composite_rx);
+
+ }
+
+ void operator()(const se3::JointModelRevoluteUnaligned & ) const
+ {
+ se3::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+ void operator()(const se3::JointModelPrismaticUnaligned & ) const
+ {
+ se3::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
+ jmodel.setIndexes(0,0,0);
+
+ test_joint_methods(jmodel);
+ }
+
+};
+
+
+
+
BOOST_AUTO_TEST_SUITE ( JointCompositeTest)
-BOOST_AUTO_TEST_CASE ( JointModel)
+// Test that a composite joint can contain any type of joint
+BOOST_AUTO_TEST_CASE ( test_all_joints )
+{
+ using namespace Eigen;
+ using namespace se3;
+
+ boost::mpl::for_each<JointModelVariant::types>(TestJointComposite());
+
+}
+
+
+
+BOOST_AUTO_TEST_CASE ( test_recursive_variant)
{
+ // functional test. Test if one can create a composite joint containing composite joint
+
using namespace Eigen;
using namespace se3;
- // Constructor from one type
- JointModelComposite jmodel_composite((JointModelRX()));
-
- BOOST_CHECK_MESSAGE(jmodel_composite.isFull(), " Constructor with one RX did not fullfilled the joint composite");
- BOOST_CHECK_MESSAGE(!jmodel_composite.isEmpty(), " Constructor with one RX did not fullfilled the joint composite");
- BOOST_CHECK_MESSAGE(jmodel_composite.nq() == 1, " Dimension nq is not the same");
- BOOST_CHECK_MESSAGE(jmodel_composite.nv() == 1, " Dimension nv is not the same");
-
- // Constructor whith one int
- // Check is empty
- // Add a joint
- // Check not empty, check full
- // check that it does nothing to add a joint if joint composite is full
- // comparison
- JointModelComposite jmodel_composite_one_joint(1);
- BOOST_CHECK_MESSAGE(jmodel_composite_one_joint.isEmpty(), " Constructor with one derived type to be added did not fullfilled the joint composite");
- jmodel_composite_one_joint.addJointModel(JointModelRX());
- BOOST_CHECK_MESSAGE(!jmodel_composite_one_joint.isEmpty(), " Added a joint. but stating that the joint composite is empty");
- BOOST_CHECK_MESSAGE(jmodel_composite_one_joint.isFull(), " Added a joint to composite of max 1. but stating that the joint composite is not full");
- std::size_t nb_joints = jmodel_composite_one_joint.addJointModel(JointModelRY());
- BOOST_CHECK_MESSAGE(nb_joints == 1, " Added a 2nd joint to composite of max 1");
-
-
- /// Update components indexes
/// Create joint composite with two joints,
JointModelComposite jmodel_composite_two_rx(2);
jmodel_composite_two_rx.addJointModel(JointModelRX());
jmodel_composite_two_rx.addJointModel(JointModelRY());
- jmodel_composite_two_rx.setIndexes(3,8,7); // suppose it comes after a freeflyer, and universe
- jmodel_composite_two_rx.updateComponentsIndexes();
- JointModelVariant & first_rx = jmodel_composite_two_rx.joints[0];
- JointModelVariant & second_rx = jmodel_composite_two_rx.joints[1];
- BOOST_CHECK_MESSAGE(idx_q(first_rx) == 8, "");
- BOOST_CHECK_MESSAGE(idx_q(second_rx) == 9, "");
- BOOST_CHECK_MESSAGE(idx_v(first_rx) == 7, "");
- BOOST_CHECK_MESSAGE(idx_v(second_rx) == 8, "");
- BOOST_CHECK_MESSAGE(id(first_rx) == jmodel_composite_two_rx.id(), "");
- BOOST_CHECK_MESSAGE(id(second_rx) == jmodel_composite_two_rx.id(), "");
+ /// Create Joint composite with three joints, and add a composite in it, to test the recursive_wrapper
+ JointModelComposite jmodel_composite_recursive(3);
+ jmodel_composite_recursive.addJointModel(JointModelFreeFlyer());
+ jmodel_composite_recursive.addJointModel(JointModelPlanar());
+ jmodel_composite_recursive.addJointModel(jmodel_composite_two_rx);
+
+}
- /// Comparing computations to JointModelPlanar()
+BOOST_AUTO_TEST_CASE (TestCopyComposite)
+{
+
+ std::cout << "\n\n --- Test Copy composite" << std::endl;
+ using namespace Eigen;
+ using namespace se3;
JointModelComposite jmodel_composite_planar(3);
jmodel_composite_planar.addJointModel(JointModelPX());
@@ -88,34 +193,32 @@ BOOST_AUTO_TEST_CASE ( JointModel)
jmodel_composite_planar.setIndexes(1,0,0);
jmodel_composite_planar.updateComponentsIndexes();
- JointModelPlanar jmodel_planar;
- jmodel_planar.setIndexes(1,0,0);
+ JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();
Eigen::VectorXd q1(Eigen::VectorXd::Random(3));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random(3));
- Eigen::VectorXd q2(Eigen::VectorXd::Random(3));
- double u = 0.3;
- BOOST_CHECK_MESSAGE( jmodel_planar.integrate(q1,q1_dot).isApprox(jmodel_composite_planar.integrate(q1,q1_dot))
- ,"Joint Model Composite vs Planar - integrate error");
- BOOST_CHECK_MESSAGE( jmodel_planar.interpolate(q1,q2,u).isApprox(jmodel_composite_planar.interpolate(q1,q2,u))
- ,"Joint Model Composite vs Planar - interpolate error");
- BOOST_CHECK_MESSAGE(jmodel_planar.randomConfiguration( -1 * Eigen::VectorXd::Ones(jmodel_planar.nq()),
- Eigen::VectorXd::Ones(jmodel_planar.nq())).size()
- == jmodel_composite_planar.randomConfiguration(-1 * Eigen::VectorXd::Ones(jmodel_composite_planar.nq()),
- Eigen::VectorXd::Ones(jmodel_composite_planar.nq())).size()
- ,"Joint Model Composite vs Planar - Dimensions of random configuration are not the same");
-
- BOOST_CHECK_MESSAGE( jmodel_planar.difference(q1,q2).isApprox(jmodel_composite_planar.difference(q1,q2))
- ,"Joint Model Composite vs Planar - difference error");
- BOOST_CHECK_MESSAGE( jmodel_planar.distance(q1,q2) == jmodel_composite_planar.distance(q1,q2)
- ,"Joint Model Composite vs Planar - distance error");
+ JointModelComposite model_copy = jmodel_composite_planar;
+ JointDataComposite data_copy = model_copy.createData();
+ std::cout << model_copy << std::endl;
+ BOOST_CHECK_MESSAGE( model_copy.max_joints == jmodel_composite_planar.max_joints, "Test Copy Composite, max_joints are differents");
+ BOOST_CHECK_MESSAGE( model_copy.nq() == jmodel_composite_planar.nq(), "Test Copy Composite, nq are differents");
+ BOOST_CHECK_MESSAGE( model_copy.nv() == jmodel_composite_planar.nv(), "Test Copy Composite, nv are differents");
+
+ BOOST_CHECK_MESSAGE( model_copy.max_joints == jmodel_composite_planar.max_joints, "Test Copy Composite, max_joints are differents");
+
+ jmodel_composite_planar.calc(jdata_composite_planar,q1, q1_dot);
+ model_copy.calc(data_copy,q1, q1_dot);
+
}
-BOOST_AUTO_TEST_CASE ( JointData )
+
+BOOST_AUTO_TEST_CASE (TestVariantOverComposite)
{
+
+ std::cout << "\n\n --- Test Variant Over composite" << std::endl;
using namespace Eigen;
using namespace se3;
@@ -126,174 +229,128 @@ BOOST_AUTO_TEST_CASE ( JointData )
jmodel_composite_planar.setIndexes(1,0,0);
jmodel_composite_planar.updateComponentsIndexes();
-
JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();
- JointModelPlanar jmodel_planar;
- jmodel_planar.setIndexes(1,0,0);
- JointDataPlanar jdata_planar = jmodel_planar.createData();
-
Eigen::VectorXd q1(Eigen::VectorXd::Random(3));
Eigen::VectorXd q1_dot(Eigen::VectorXd::Random(3));
- ///
- /// S
- ///
- // BOOST_CHECK_MESSAGE(((ConstraintXd)jdata_planar.S).matrix().isApprox(jdata_composite_planar.S.matrix()), "JointComposite vs Planar - constraint not equal");
-
- ///
- /// calc -> M
- ///
- jmodel_planar.calc(jdata_planar,q1);
- jmodel_composite_planar.calc(jdata_composite_planar,q1);
- BOOST_CHECK_MESSAGE(jdata_planar.M == jdata_composite_planar.M, "Joint Composite vs Planar - calc_zero_order error");
-
- ///
- /// calc -> M, (v)
- ///
- jmodel_planar.calc(jdata_planar,q1, q1_dot);
- jmodel_composite_planar.calc(jdata_composite_planar,q1, q1_dot);
- BOOST_CHECK_MESSAGE(jdata_planar.M == jdata_composite_planar.M, "Joint Composite vs Planar - calc_first_order error for M");
- // BOOST_CHECK_MESSAGE(Motion(jdata_planar.v) == jdata_composite_planar.v, "Joint Composite vs Planar - calc_first_order error for v");
-
- ///
- /// calc_aba -> (U, Dinv, UDinv)
- ///
- Inertia::Matrix6 Ia(Inertia::Random().matrix());
- bool update_I = false;
- jmodel_planar.calc_aba(jdata_planar,Ia, update_I);
- jmodel_composite_planar.calc_aba(jdata_composite_planar,Ia, update_I);
- // BOOST_CHECK_MESSAGE(jdata_planar.U.isApprox(jdata_composite_planar.U), "Joint Composite vs Planar - calc_aba error for U");
- // BOOST_CHECK_MESSAGE(jdata_planar.Dinv.isApprox(jdata_composite_planar.Dinv), "Joint Composite vs Planar - calc_aba error for Dinv");
- // BOOST_CHECK_MESSAGE(jdata_planar.UDinv.isApprox(jdata_composite_planar.UDinv), "Joint Composite vs Planar - calc_aba error for UDinv");
-
+ JointModelVariant jmvariant_comp(jmodel_composite_planar);
+ JointDataVariant jdvariant_comp(jdata_composite_planar);
-
-}
-
-BOOST_AUTO_TEST_CASE ( test_recursive_variant)
-{
- using namespace Eigen;
- using namespace se3;
-
-
- /// Update components indexes
- /// Create joint composite with two joints,
- JointModelComposite jmodel_composite_two_rx(2);
- jmodel_composite_two_rx.addJointModel(JointModelRX());
- jmodel_composite_two_rx.addJointModel(JointModelRY());
-
- JointModelComposite jmodel_composite_recursive(3);
- jmodel_composite_recursive.addJointModel(JointModelFreeFlyer());
- jmodel_composite_recursive.addJointModel(JointModelPlanar());
- jmodel_composite_recursive.addJointModel(jmodel_composite_two_rx);
+ std::cout << " Extract the composite joint from the variant, and visit each joint from the stack" << std::endl;
+ JointModelComposite extracted_model = boost::get<JointModelComposite>(jmvariant_comp);
+ for (std::size_t i = 0; i < 3; ++i)
+ {
+ calc_first_order(extracted_model.joints[i], jdata_composite_planar.joints[i], q1, q1_dot);
+ std::cout << se3::nq(extracted_model.joints[i]) << std::endl;
+ }
+ std::cout << " Testing visiting a variant over the composite joint" << std::endl;
+ std::cout << nv(jmvariant_comp) << std::endl;
+ calc_first_order(jmvariant_comp, jdvariant_comp, q1, q1_dot); // here assertion 'false' failed has_fallback_type_
}
+// Compare a stack of joint ( PX, PY, RZ) to a planar joint
BOOST_AUTO_TEST_CASE ( KinematicModelCompositePlanar)
{
+ std::cout << " Testing Planar Model vs composite planar model" << std::endl;
using namespace Eigen;
using namespace se3;
Model model_composite_planar;
- Model model_px_py_rz;
+ Model model_planar;
Inertia body_inertia(Inertia::Random());
SE3 placement(SE3::Identity());
- model_px_py_rz.addBody(model_px_py_rz.getBodyId("universe"),JointModelPX(), placement ,Inertia::Zero(),
- "px_joint", "px_body");
- model_px_py_rz.addBody(model_px_py_rz.getBodyId("px_body"),JointModelPY(), placement ,Inertia::Zero(),
- "py_joint", "py_body");
- model_px_py_rz.addBody(model_px_py_rz.getBodyId("py_body"),JointModelRZ(), placement ,body_inertia,
- "rz_joint", "rz_body");
-
+ model_planar.addJoint(model_planar.getBodyId("universe"),JointModelPlanar(), placement, "planar_joint");
+ model_planar.appendBodyToJoint(model_planar.getJointId("planar_joint"), body_inertia, SE3::Identity(), "planar_body");
JointModelComposite jmodel_composite_planar(3);
jmodel_composite_planar.addJointModel(JointModelPX());
jmodel_composite_planar.addJointModel(JointModelPY());
jmodel_composite_planar.addJointModel(JointModelRZ());
-
- model_composite_planar.addBody(model_composite_planar.getBodyId("universe"), jmodel_composite_planar, placement, body_inertia,
- "composite_planar_joint", "composite_planar_body");
+
+ model_composite_planar.addJoint(model_composite_planar.getBodyId("universe"),jmodel_composite_planar, placement, "composite_planar_joint");
+ model_composite_planar.appendBodyToJoint(model_composite_planar.getJointId("composite_planar_joint"), body_inertia, SE3::Identity(), "composite_planar_body");
// When Model will be cleaned in coming pull request, this will be done in addBody(addJoint)
boost::get<JointModelComposite>(model_composite_planar.joints[model_composite_planar.getJointId("composite_planar_joint")]).updateComponentsIndexes();
- BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_px_py_rz.nq ,"Model with composite planar joint vs PxPyRz - dimensions nq are not equal");
- BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_px_py_rz.nq ,"Model with composite planar joint vs PxPyRz - dimensions nv are not equal");
+ BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_planar.nq ,"Model with planar joint vs composite PxPyRz - dimensions nq are not equal");
+ BOOST_CHECK_MESSAGE(model_composite_planar.nq == model_planar.nq ,"Model with planar joint vs composite PxPyRz - dimensions nv are not equal");
- Data data_px_py_pz(model_px_py_rz);
- Data data_composite_planar(model_composite_planar);
+ // Data data_planar(model_planar);
+ // Data data_composite_planar(model_composite_planar);
- Eigen::VectorXd q(Eigen::VectorXd::Random(model_px_py_rz.nq));
- Eigen::VectorXd q_dot(Eigen::VectorXd::Random(model_px_py_rz.nv));
- Eigen::VectorXd q_ddot(Eigen::VectorXd::Random(model_px_py_rz.nv));
- Eigen::VectorXd q1(Eigen::VectorXd::Random(model_px_py_rz.nq));
- Eigen::VectorXd q2(Eigen::VectorXd::Random(model_px_py_rz.nq));
- Eigen::VectorXd tau(Eigen::VectorXd::Random(model_px_py_rz.nq));
- double u = 0.3;
+ // Eigen::VectorXd q(Eigen::VectorXd::Random(model_planar.nq));
+ // Eigen::VectorXd q_dot(Eigen::VectorXd::Random(model_planar.nv));
+ // Eigen::VectorXd q_ddot(Eigen::VectorXd::Random(model_planar.nv));
+ // Eigen::VectorXd q1(Eigen::VectorXd::Random(model_planar.nq));
+ // Eigen::VectorXd q2(Eigen::VectorXd::Random(model_planar.nq));
+ // Eigen::VectorXd tau(Eigen::VectorXd::Random(model_planar.nq));
+ // double u = 0.3;
+
+ // // Test that algorithms do not crash
+ // integrate(model_composite_planar,q,q_dot);
+ // interpolate(model_composite_planar,q1,q2,u);
+ // differentiate(model_composite_planar,q1,q2);
+ // distance(model_composite_planar,q1,q2);
+ // randomConfiguration(model_composite_planar);
- // Test that algorithms do not crash
- integrate(model_composite_planar,q,q_dot);
- interpolate(model_composite_planar,q1,q2,u);
- differentiate(model_composite_planar,q1,q2);
- distance(model_composite_planar,q1,q2);
- randomConfiguration(model_composite_planar);
-
- // aba(model_composite_planar,data_composite_planar, q,q_dot, tau);
- centerOfMass(model_composite_planar, data_composite_planar,q,q_dot,q_ddot,true,false);
- emptyForwardPass(model_composite_planar, data_composite_planar);
- forwardKinematics(model_composite_planar,data_composite_planar, q );
+ // // aba(model_composite_planar,data_composite_planar, q,q_dot, tau);
+ // centerOfMass(model_composite_planar, data_composite_planar,q,q_dot,q_ddot,true,false);
+ // emptyForwardPass(model_composite_planar, data_composite_planar);
+ // forwardKinematics(model_composite_planar,data_composite_planar, q );
// forwardKinematics(model_composite_planar,data_composite_planar, q, q_dot);
// forwardKinematics(model_composite_planar,data_composite_planar, q, q_dot, q_ddot);
- // computeAllTerms(model_px_py_rz,data_px_py_pz,q,q_dot);
+ // computeAllTerms(model_planar,data_planar,q,q_dot);
// computeAllTerms(model_composite_planar,data_composite_planar,q,q_dot);
- // Model::Index last_joint_pxpyrz = (Model::Index) model_px_py_rz.nbody-1;
+ // Model::Index last_joint_pxpyrz = (Model::Index) model_planar.nbody-1;
// Model::Index last_joint_composite = (Model::Index) model_composite_planar.nbody-1;
// BOOST_CHECK_MESSAGE(data_composite_planar.oMi[last_joint_composite]
- // .isApprox(data_px_py_pz.oMi[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - oMi last joint not equal");
+ // .isApprox(data_planar.oMi[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - oMi last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.v[last_joint_composite]
- // == data_px_py_pz.v[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - v last joint not equal");
+ // == data_planar.v[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - v last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.a[last_joint_composite]
- // == data_px_py_pz.a[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - a last joint not equal");
+ // == data_planar.a[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - a last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.f[last_joint_composite]
- // == data_px_py_pz.f[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - f last joint not equal");
+ // == data_planar.f[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - f last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.com[last_joint_composite]
- // .isApprox(data_px_py_pz.com[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - com last joint not equal");
+ // .isApprox(data_planar.com[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - com last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.vcom[last_joint_composite]
- // .isApprox(data_px_py_pz.vcom[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - vcom last joint not equal");
+ // .isApprox(data_planar.vcom[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - vcom last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.mass[last_joint_composite]
- // == data_px_py_pz.mass[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - mass last joint not equal");
+ // == data_planar.mass[last_joint_pxpyrz] , "composite planar joint vs PxPyRz - mass last joint not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.kinetic_energy
- // == data_px_py_pz.kinetic_energy , "composite planar joint vs PxPyRz - kinetic energy not equal");
+ // == data_planar.kinetic_energy , "composite planar joint vs PxPyRz - kinetic energy not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.potential_energy
- // == data_px_py_pz.potential_energy , "composite planar joint vs PxPyRz - potential energy not equal");
+ // == data_planar.potential_energy , "composite planar joint vs PxPyRz - potential energy not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.nle[last_joint_composite]
- // .isApprox(data_px_py_pz.nle[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - nle not equal");
+ // .isApprox(data_planar.nle[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - nle not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.M[last_joint_composite]
- // .isApprox(data_px_py_pz.M[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Mass Matrix not equal");
+ // .isApprox(data_planar.M[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Mass Matrix not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.J[last_joint_composite]
- // .isApprox(data_px_py_pz.J[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Jacobian not equal");
+ // .isApprox(data_planar.J[last_joint_pxpyrz]) , "composite planar joint vs PxPyRz - Jacobian not equal");
// BOOST_CHECK_MESSAGE(data_composite_planar.Jcom
- // .isApprox(data_px_py_pz.Jcom) , "composite planar joint vs PxPyRz - Jacobian com not equal");
+ // .isApprox(data_planar.Jcom) , "composite planar joint vs PxPyRz - Jacobian com not equal");
}
diff --git a/unittest/joint.cpp b/unittest/joint.cpp
index a191c8d7f91a49fbae4a87420f2a4892bc7ae40e..29d8b6993158a81c2aba4f189edbe1725791188f 100644
--- a/unittest/joint.cpp
+++ b/unittest/joint.cpp
@@ -15,6 +15,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
#define BOOST_TEST_DYN_LINK
@@ -32,8 +33,10 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
- q1 = jmodel.random();
- q2 = jmodel.random();
+ q1 = jmodel.random();
+ q2 = jmodel.random();
+
+ std::cout << "Running calc from composite" << std::endl;
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
@@ -42,7 +45,7 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::JointModel jma(jmodel);
se3::JointData jda(jdata);
-
+ std::cout << "Running calc from Joint" << std::endl;
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
@@ -93,6 +96,21 @@ struct TestJoint{
// JointModelDense will be removed soon
}
+ void operator()(const se3::JointModelComposite & ) const
+ {
+ // se3::JointModelComposite jmodel(2);
+ // jmodel.addJointModel(se3::JointModelRX());
+ // jmodel.addJointModel(se3::JointModelRY());
+
+ se3::JointModelComposite jmodel((se3::JointModelRX()), (se3::JointModelRY()));
+ jmodel.setIndexes(0,0,0);
+ jmodel.updateComponentsIndexes();
+
+ se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
+
+ test_joint_methods(jmodel, jdata);
+ }
+
void operator()(const se3::JointModelRevoluteUnaligned & ) const
{
se3::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);