Skip to content

Commit

Permalink
Merge pull request #182 from fvalenza/devel
Browse files Browse the repository at this point in the history 
Make JointAccessor a variant on its own and a Derived Joint
  • Loading branch information
@jcarpent
jcarpent committed Jun 12, 2016
2 parents 6a26544 + af5b7c5 commit 0e62605
Show file tree
Hide file tree
Showing 2 changed files with 74 additions and 57 deletions.
76 changes: 47 additions & 29 deletions src/multibody/joint/joint-accessor.hpp
View file
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
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

0 comments on commit 0e62605

Please sign in to comment.