5 #include "pinocchio/autodiff/casadi.hpp" 7 #include "pinocchio/multibody/joint/joint-generic.hpp" 8 #include "pinocchio/multibody/liegroup/liegroup.hpp" 9 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp" 11 #include <boost/test/unit_test.hpp> 12 #include <boost/utility/binary.hpp> 14 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
18 typedef casadi::SX AD_double;
28 typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
29 typedef Eigen::Matrix<AD_double,6,1> Vector6AD;
32 typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
37 typedef JointModelRX::ConfigVector_t ConfigVector;
38 typedef JointModelRX::TangentVector_t TangentVector;
41 JointModelRX jmodel; jmodel.setIndexes(0,0,0);
42 JointDataRX jdata(jmodel.createData());
44 JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
45 JointDataRXAD jdata_ad(jmodel_ad.createData());
48 ConfigVector
q(jmodel.nq()); JointOperation().random(
q);
51 ConfigVectorAD q_ad(jmodel.nq());
52 for(Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
58 jmodel_ad.calc(jdata_ad,q_ad);
64 casadi::SX cs_trans(3,1);
65 for(Eigen::DenseIndex k = 0; k < 3; ++k)
67 cs_trans(k) = M2.translation()[k];
69 casadi::SX cs_rot(3,3);
70 for(Eigen::DenseIndex
i = 0;
i < 3; ++
i)
72 for(Eigen::DenseIndex j = 0; j < 3; ++j)
74 cs_rot(
i,j) = M2.rotation()(
i,j);
78 casadi::Function eval_placement(
"eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
79 std::cout <<
"Joint Placement = " << eval_placement << std::endl;
81 std::vector<double> q_vec((
size_t)jmodel.nq());
82 Eigen::Map<ConfigVector>(q_vec.data(),jmodel.nq(),1) =
q;
83 casadi::DMVector
res = eval_placement(casadi::DMVector {q_vec});
84 std::cout <<
"M(q)=" << res << std::endl;
86 BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->
data())));
87 BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->
data())));
91 TangentVector
v(TangentVector::Random(jmodel.nv()));
92 VectorXAD v_ad(jmodel_ad.nv());
94 std::vector<double> v_vec((
size_t)jmodel.nv());
95 Eigen::Map<TangentVector>(v_vec.data(),jmodel.nv(),1) = v;
97 for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
102 jmodel.calc(jdata,
q,v);
104 ConstraintXd Sref(jdata.S.matrix());
106 jmodel_ad.calc(jdata_ad,q_ad,v_ad);
108 MotionAD m_ad(jdata_ad.v);
110 casadi::SX cs_vel(6,1);
111 for(Eigen::DenseIndex k = 0; k < 6; ++k)
113 cs_vel(k) = m_ad.toVector()[k];
115 casadi::Function eval_velocity(
"eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
116 std::cout <<
"Joint Velocity = " << eval_velocity << std::endl;
118 casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
119 std::cout <<
"v(q,v)=" << res_vel << std::endl;
121 BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->
data())));
122 BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->
data()+3)));
124 casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
125 casadi::Function eval_S(
"eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
126 std::cout <<
"S = " << eval_S << std::endl;
128 casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
129 std::cout <<
"res_S:" << res_S << std::endl;
130 ConstraintXd::DenseBase Sref_mat = Sref.matrix();
132 for(Eigen::DenseIndex
i = 0;
i < 6; ++
i)
134 for(Eigen::DenseIndex j = 0;
i < Sref.nv(); ++
i)
135 BOOST_CHECK(std::fabs(Sref_mat(
i,j) - (
double)res_S[0](
i,j)) <= Eigen::NumTraits<double>::dummy_precision());
139 template<
typename Jo
intModel_>
struct init;
141 template<
typename Jo
intModel_>
147 jmodel.setIndexes(0,0,0);
153 return "default " + JointModel_::classname();
157 template<
typename Scalar,
int Options>
158 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
165 JointModel jmodel(Vector3::Random().normalized());
173 return JointModel::classname();
177 template<
typename Scalar,
int Options>
178 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
185 JointModel jmodel(Vector3::Random().normalized());
193 return JointModel::classname();
197 template<
typename Scalar,
int Options>
198 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
205 JointModel jmodel(Vector3::Random().normalized());
213 return JointModel::classname();
217 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
218 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
233 return JointModel::classname();
237 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
238 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
255 return JointModel::classname();
259 template<
typename Jo
intModel_>
260 struct init<pinocchio::JointModelMimic<JointModel_> >
268 JointModel jmodel(jmodel_ref,1.,0.);
275 return JointModel::classname();
281 template<
typename Jo
intModel_>
285 jmodel.setIndexes(0,0,0);
290 template<
typename Jo
intModel_>
294 template<
typename Jo
intModel>
297 std::cout <<
"--" << std::endl;
298 std::cout <<
"jmodel: " << jmodel.
shortname() << std::endl;
300 typedef casadi::SX AD_double;
308 typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
309 typedef Eigen::Matrix<AD_double,6,1> Vector6AD;
312 typedef typename JointModelAD::JointDataDerived JointDataAD;
314 typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;
316 typedef typename JointModel::JointDataDerived
JointData;
317 typedef typename JointModel::ConfigVector_t ConfigVector;
318 typedef typename JointModel::TangentVector_t TangentVector;
324 JointModelAD jmodel_ad = jmodel.template cast<AD_double>();
325 JointDataAD jdata_ad(jmodel_ad.createData());
328 ConfigVector
q(jmodel.
nq());
330 ConfigVector lb(ConfigVector::Constant(jmodel.
nq(),-1.));
331 ConfigVector ub(ConfigVector::Constant(jmodel.
nq(),1.));
333 typedef pinocchio::RandomConfigurationStep<pinocchio::LieGroupMap,ConfigVector,ConfigVector,ConfigVector> RandomConfigAlgo;
337 ConfigVectorAD q_ad(jmodel.
nq());
338 for(Eigen::DenseIndex k = 0; k < jmodel.
nq(); ++k)
344 jmodel_ad.calc(jdata_ad,q_ad);
345 jmodel.
calc(jdata,
q);
347 SE3 M1(jdata_base.M());
348 SE3AD M2(jdata_ad_base.M());
350 casadi::SX cs_trans(3,1);
351 for(Eigen::DenseIndex k = 0; k < 3; ++k)
353 cs_trans(k) = M2.translation()[k];
355 casadi::SX cs_rot(3,3);
356 for(Eigen::DenseIndex
i = 0;
i < 3; ++
i)
358 for(Eigen::DenseIndex j = 0; j < 3; ++j)
360 cs_rot(
i,j) = M2.rotation()(
i,j);
364 casadi::Function eval_placement(
"eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
365 std::cout <<
"Joint Placement = " << eval_placement << std::endl;
367 std::vector<double> q_vec((
size_t)jmodel.
nq());
368 Eigen::Map<ConfigVector>(q_vec.data(),jmodel.
nq(),1) =
q;
369 casadi::DMVector
res = eval_placement(casadi::DMVector {q_vec});
370 std::cout <<
"M(q)=" << res << std::endl;
372 BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->
data())));
373 BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->
data())));
377 TangentVector
v(TangentVector::Random(jmodel.
nv()));
378 VectorXAD v_ad(jmodel_ad.nv());
380 std::vector<double> v_vec((
size_t)jmodel.
nv());
381 Eigen::Map<TangentVector>(v_vec.data(),jmodel.
nv(),1) = v;
383 for(Eigen::DenseIndex k = 0; k < jmodel.
nv(); ++k)
388 jmodel.
calc(jdata,
q,v);
389 Motion
m(jdata_base.v());
390 ConstraintXd Sref(jdata_base.S().matrix());
392 jmodel_ad.calc(jdata_ad,q_ad,v_ad);
394 MotionAD m_ad(jdata_ad_base.v());
396 casadi::SX cs_vel(6,1);
397 for(Eigen::DenseIndex k = 0; k < 6; ++k)
399 cs_vel(k) = m_ad.toVector()[k];
401 casadi::Function eval_velocity(
"eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
402 std::cout <<
"Joint Velocity = " << eval_velocity << std::endl;
404 casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
405 std::cout <<
"v(q,v)=" << res_vel << std::endl;
407 BOOST_CHECK(
m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->
data())));
408 BOOST_CHECK(
m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->
data()+3)));
410 casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
411 casadi::Function eval_S(
"eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
412 std::cout <<
"S = " << eval_S << std::endl;
414 casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
415 std::cout <<
"res_S:" << res_S << std::endl;
416 ConstraintXd::DenseBase Sref_mat = Sref.matrix();
418 for(Eigen::DenseIndex
i = 0;
i < 6; ++
i)
420 for(Eigen::DenseIndex j = 0;
i < Sref.nv(); ++
i)
421 BOOST_CHECK(std::fabs(Sref_mat(
i,j) - (
double)res_S[0](
i,j)) <= Eigen::NumTraits<double>::dummy_precision());
424 std::cout <<
"--" << std::endl << std::endl;
436 BOOST_AUTO_TEST_SUITE_END()
pinocchio::JointModelMimic< JointModel_ > JointModel
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
void setIndexes(JointIndex id, int q, int v)
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
static std::string name()
std::string shortname() const
JointDataTpl< double > JointData
static void test(const pinocchio::JointModelMimic< JointModel_ > &)
static std::string name()
BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
JointDataDerived createData() const
static std::string name()
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
static std::string name()
MotionTpl< double, 0 > Motion
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelMimicRX, JointModelMimicRY, JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelTranslation, JointModelRUBX, JointModelRUBY, JointModelRUBZ, JointModelRevoluteUnboundedUnaligned, boost::recursive_wrapper< JointModelComposite > > JointModelVariant
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
void operator()(const pinocchio::JointModelBase< JointModel_ > &) const
static std::string name()
void setIndexes(JointIndex id, int nq, int nv)
ConstraintTpl< Eigen::Dynamic, double, 0 > ConstraintXd
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
static std::string name()
JointCollectionDefault::JointModelVariant JointModelVariant
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
static void test(const pinocchio::JointModelBase< JointModel > &jmodel)
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
static std::string name()