Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Make JointAccessor a variant on its own and a Derived Joint #182

Merged
merged 1 commit into from
Jun 12, 2016
Merged

Make JointAccessor a variant on its own and a Derived Joint #182

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
76 changes: 47 additions & 29 deletions src/multibody/joint/joint-accessor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,102 +56,120 @@ 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);

}

void calc (JointData & data,
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); }
};
Expand Down
55 changes: 27 additions & 28 deletions unittest/joint-accessor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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{
Expand All @@ -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
Expand Down