Skip to content
GitLab
Commit af5b7c54 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Changed JointAccessor to be a variant instead of being composed of a variant

parent 6a265445
Hide whitespace changes
Inline Side-by-side
src/multibody/joint/joint-accessor.hpp
View file @ af5b7c54
......@@ -56,51 +56,56 @@ namespace se3
template<> struct traits<JointDataAccessor> { typedef JointAccessor Joint; };
template<> struct traits<JointModelAccessor> { typedef JointAccessor Joint; };
struct JointDataAccessor : public JointDataBase<JointDataAccessor>
struct JointDataAccessor : public JointDataBase<JointDataAccessor> , JointDataVariant
{
typedef JointDataVariant super;
typedef JointAccessor Joint;
SE3_JOINT_TYPEDEF;
JointDataVariant& toVariant() { return *static_cast<JointDataVariant*>(this); }
const JointDataVariant& toVariant() const { return *static_cast<const JointDataVariant*>(this); }
JointDataVariant & j_data_variant;
const Constraint_t S() const { return constraint_xd(j_data_variant); }
const Transformation_t M() const { return joint_transform(j_data_variant); } // featherstone book, page 78 (sXp)
const Motion_t v() const { return motion(j_data_variant); }
const Bias_t c() const { return bias(j_data_variant); }
const Constraint_t S() const { return constraint_xd(toVariant()); }
const Transformation_t M() const { return joint_transform(toVariant()); } // featherstone book, page 78 (sXp)
const Motion_t v() const { return motion(toVariant()); }
const Bias_t c() const { return bias(toVariant()); }
// // [ABA CCRBA]
const U_t U() const { return u_inertia(j_data_variant); }
U_t U() { return u_inertia(j_data_variant); }
const D_t Dinv() const { return dinv_inertia(j_data_variant); }
const UD_t UDinv() const { return udinv_inertia(j_data_variant); }
const U_t U() const { return u_inertia(toVariant()); }
U_t U() { return u_inertia(toVariant()); }
const D_t Dinv() const { return dinv_inertia(toVariant()); }
const UD_t UDinv() const { return udinv_inertia(toVariant()); }
// JointDataAccessor() {} // can become necessary if we want a vector of jointDataAccessor ?
JointDataAccessor( JointDataVariant & jdata ) : j_data_variant(jdata){}
JointDataAccessor(const JointDataVariant & jdata ) : super(jdata){}
};
struct JointModelAccessor : public JointModelBase<JointModelAccessor>
struct JointModelAccessor : public JointModelBase<JointModelAccessor> , JointModelVariant
{
typedef JointModelVariant super;
typedef JointAccessor Joint;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
using JointModelBase<JointModelAccessor>::id;
using JointModelBase<JointModelAccessor>::setIndexes;
const JointModelVariant & j_model_variant;
JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
const JointModelVariant& toVariant() const { return *static_cast<const JointModelVariant*>(this); }
JointDataVariant createData()
{
return ::se3::createData(j_model_variant);
return ::se3::createData(toVariant());
}
void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
calc_zero_order(j_model_variant, data.j_data_variant, qs);
calc_zero_order(toVariant(), data.toVariant(), qs);
}
......@@ -108,50 +113,63 @@ namespace se3
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
calc_first_order(j_model_variant, data.j_data_variant, qs, vs);
calc_first_order(toVariant(), data.toVariant(), qs, vs);
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
::se3::calc_aba(j_model_variant, data.j_data_variant, I, update_I);
::se3::calc_aba(toVariant(), data.toVariant(), I, update_I);
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
return ::se3::integrate(j_model_variant, qs, vs);
return ::se3::integrate(toVariant(), qs, vs);
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
return ::se3::interpolate(j_model_variant, q0, q1, u);
return ::se3::interpolate(toVariant(), q0, q1, u);
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
{
return ::se3::randomConfiguration(j_model_variant, lower_pos_limit, upper_pos_limit);
return ::se3::randomConfiguration(toVariant(), lower_pos_limit, upper_pos_limit);
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::difference(j_model_variant, q0, q1);
return ::se3::difference(toVariant(), q0, q1);
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::distance(j_model_variant, q0, q1);
return ::se3::distance(toVariant(), q0, q1);
}
// JointModelAccessor() : j_model_variant() {} // can become necessary if we want a vector of jointModelAccessor ?
JointModelAccessor( const JointModelVariant & model_variant ) : j_model_variant(model_variant)
JointModelAccessor( const JointModelVariant & model_variant ) : super(model_variant)
{}
int nq_impl() const { return ::se3::nq(j_model_variant); }
int nv_impl() const { return ::se3::nv(j_model_variant); }
int idx_q() const { return ::se3::idx_q(j_model_variant); }
int idx_v() const { return ::se3::idx_v(j_model_variant); }
JointModelAccessor& operator=( const JointModelAccessor& other)
{
*this = other;
return *this;
}
JointModelAccessor& operator=( const JointModelVariant& other)
{
toVariant() = other; // or *this = other ?
return *this;
}
int nq_impl() const { return ::se3::nq(toVariant()); }
int nv_impl() const { return ::se3::nv(toVariant()); }
int idx_q() const { return ::se3::idx_q(toVariant()); }
int idx_v() const { return ::se3::idx_v(toVariant()); }
JointIndex id() const { return ::se3::id(j_model_variant); }
JointIndex id() const { return ::se3::id(toVariant()); }
void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
};
......
unittest/joint-accessor.cpp
View file @ af5b7c54
......@@ -44,46 +44,45 @@ void test_joint_methods (T & jmodel, typename T::JointData & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
jmodel.calc(jdata, q1, q1_dot); // To be removed when cherry-picked the fix of calc visitor
jmodel.calc_aba(jdata, Ia, update_I); // To be removed when cherry-picked the fix of calc_aba visitor
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
se3::JointModelVariant jmodelvariant(jmodel);
se3::JointDataVariant jdatavariant(jdata);
se3::JointModelAccessor jma(jmodelvariant);
se3::JointDataAccessor jda(jdatavariant);
// calc_first_order(jmodelvariant, jdatavariant, q1, q1_dot); // To be added instead of line 134
// jma.calc(jda, q1, q1_dot); // or test with this one
// jma.calc_aba(jda, Ia, update_I)
se3::JointModelAccessor jma(jmodel);
se3::JointDataAccessor jda(jdata);
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
std::string error_prefix("Joint Model Accessor on " + T::shortname());
BOOST_CHECK_MESSAGE(nq(jmodelvariant) == jma.nq() ,std::string(error_prefix + " - nq "));
BOOST_CHECK_MESSAGE(nv(jmodelvariant) == jma.nv() ,std::string(error_prefix + " - nv "));
BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));
BOOST_CHECK_MESSAGE(idx_q(jmodelvariant) == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
BOOST_CHECK_MESSAGE(idx_v(jmodelvariant) == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
BOOST_CHECK_MESSAGE(id(jmodelvariant) == jma.id() ,std::string(error_prefix + " - JointId "));
BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));
BOOST_CHECK_MESSAGE(integrate(jmodelvariant,q1,q1_dot).isApprox(jma.integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
BOOST_CHECK_MESSAGE(interpolate(jmodelvariant,q1,q2,u).isApprox(jma.interpolate(q1,q2,u)) ,std::string(error_prefix + " - interpolate "));
BOOST_CHECK_MESSAGE(randomConfiguration(jmodelvariant, -1 * Eigen::VectorXd::Ones(nq(jmodelvariant)),
Eigen::VectorXd::Ones(nq(jmodelvariant))).size()
BOOST_CHECK_MESSAGE(jmodel.integrate(q1,q1_dot).isApprox(jma.integrate(q1,q1_dot)) ,std::string(error_prefix + " - integrate "));
BOOST_CHECK_MESSAGE(jmodel.interpolate(q1,q2,u).isApprox(jma.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()
== jma.randomConfiguration(-1 * Eigen::VectorXd::Ones(jma.nq()),
Eigen::VectorXd::Ones(jma.nq())).size()
,std::string(error_prefix + " - RandomConfiguration dimensions "));
BOOST_CHECK_MESSAGE(difference(jmodelvariant,q1,q2).isApprox(jma.difference(q1,q2)) ,std::string(error_prefix + " - difference "));
BOOST_CHECK_MESSAGE(distance(jmodelvariant,q1,q2) == jma.distance(q1,q2) ,std::string(error_prefix + " - distance "));
BOOST_CHECK_MESSAGE(jmodel.difference(q1,q2).isApprox(jma.difference(q1,q2)) ,std::string(error_prefix + " - difference "));
BOOST_CHECK_MESSAGE(jmodel.distance(q1,q2) == jma.distance(q1,q2) ,std::string(error_prefix + " - distance "));
BOOST_CHECK_MESSAGE((jda.S().matrix()).isApprox((constraint_xd(jdatavariant).matrix())),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE((jda.M()) == (joint_transform(jdatavariant)),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
BOOST_CHECK_MESSAGE((jda.v()) == (motion(jdatavariant)),std::string(error_prefix + " - Joint motions "));
BOOST_CHECK_MESSAGE((jda.c()) == (bias(jdatavariant)),std::string(error_prefix + " - Joint bias "));
BOOST_CHECK_MESSAGE((jda.S().matrix()).isApprox((((se3::ConstraintXd)jdata.S).matrix())),std::string(error_prefix + " - ConstraintXd "));
BOOST_CHECK_MESSAGE((jda.M()) == (jdata.M),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
BOOST_CHECK_MESSAGE((jda.v()) == (jdata.v),std::string(error_prefix + " - Joint motions "));
BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c),std::string(error_prefix + " - Joint bias "));
BOOST_CHECK_MESSAGE((jda.U()).isApprox((u_inertia(jdatavariant))),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox((dinv_inertia(jdatavariant))),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox((udinv_inertia(jdatavariant))),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
}
struct TestJointAccessor{
......@@ -101,7 +100,7 @@ struct TestJointAccessor{
template <int NQ, int NV>
void operator()(const se3::JointModelDense<NQ,NV> & ) const
{
// Not yet correctly implemented, test has no meaning for the moment
// JointModelDense will be removed soon
}
void operator()(const se3::JointModelRevoluteUnaligned & ) const
......
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