5 #include "pinocchio/spatial/se3.hpp" 6 #include "pinocchio/spatial/inertia.hpp" 7 #include "pinocchio/multibody/force-set.hpp" 8 #include "pinocchio/multibody/model.hpp" 9 #include "pinocchio/algorithm/joint-configuration.hpp" 15 #include <boost/test/unit_test.hpp> 16 #include <boost/utility/binary.hpp> 20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
31 ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
33 BOOST_CHECK_EQUAL(F.
matrix().col(10).norm() , 0.0 );
34 BOOST_CHECK(std::isnan(F.
matrix()(0,9)));
36 ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
39 BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).
isApprox(F4.
matrix(), 1e-12));
48 BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).
isApprox(F36.
matrix(), 1e-12));
52 BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).
isApprox(F36full.
matrix().block(0,3,6,6),
61 JointDataRX::Constraint_t S;
64 BOOST_CHECK(F1.
matrix().isApprox(Y.
matrix().col(3), 1e-12));
66 ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
67 Eigen::MatrixXd StF2 = S.transpose()*F2.
block(5,3).
matrix();
68 BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.
matrix().block(0,5,6,3)
72 template<
typename Jo
intModel>
76 BOOST_CHECK(jmodel.
nq() == nq_ref);
79 template<
typename Jo
intModel>
83 BOOST_CHECK(jmodel.
jmodel().
nq() == nq_ref);
86 template<
typename Jo
intModel,
typename Constra
intDerived>
90 BOOST_CHECK(constraint.
nv() == jmodel.
nv());
93 template<
typename Jo
intModel,
typename Constra
intDerived>
97 BOOST_CHECK(constraint.
nv() == jmodel.
jmodel().
nv());
100 template<
class Jo
intModel>
112 template<
class Jo
intModel>
127 template<
typename Jo
intModel>
133 typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic>
Matrix6x;
136 typedef typename JointModel::ConfigVector_t ConfigVector_t;
145 ConfigVector_t::Constant(model.
nq,-1.),
146 ConfigVector_t::Constant(model.
nq, 1.));
149 jmodel.
calc(jdata,q);
151 ConstraintType constraint(jdata.
S);
154 BOOST_CHECK(constraint.cols() == constraint.nv());
155 BOOST_CHECK(constraint.rows() == 6);
157 typedef typename JointModel::TangentVector_t TangentVector_t;
158 TangentVector_t
v = TangentVector_t::Random(constraint.nv());
160 typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
169 typename ConstraintType::DenseBase S = M.
act(constraint);
170 typename ConstraintType::DenseBase S_ref(6,constraint.nv());
172 for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
174 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
180 BOOST_CHECK(S.isApprox(S_ref));
186 typename ConstraintType::DenseBase S = M.
actInv(constraint);
187 typename ConstraintType::DenseBase S_ref(6,constraint.nv());
189 for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
191 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
197 BOOST_CHECK(S.isApprox(S_ref));
205 typename ConstraintType::DenseBase S1_vice = M.
actInv(constraint);
206 typename ConstraintType::DenseBase S2_vice = Minv.
act(constraint);
208 BOOST_CHECK(S1_vice.isApprox(S2_vice));
210 typename ConstraintType::DenseBase S1_versa = M.
act(constraint);
211 typename ConstraintType::DenseBase S2_versa = Minv.
actInv(constraint);
213 BOOST_CHECK(S1_versa.isApprox(S2_versa));
221 typename ConstraintType::DenseBase S = v.
cross(constraint);
222 typename ConstraintType::DenseBase S_ref(6,constraint.nv());
224 for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
226 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
229 m_out = v.
cross(m_in);
231 BOOST_CHECK(S.isApprox(S_ref));
236 const Eigen::DenseIndex
dim = 20;
237 const Matrix6x Fin = Matrix6x::Random(6,dim);
238 Eigen::MatrixXd Fout = constraint.transpose() * Fin;
239 Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
240 BOOST_CHECK(Fout.isApprox(Fout_ref));
243 Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
244 Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
245 BOOST_CHECK(Stf_ref.isApprox(Stf));
251 Eigen::MatrixXd YS = Y * constraint;
252 Eigen::MatrixXd YS_ref = Y.
matrix() * constraint_mat;
253 BOOST_CHECK(YS.isApprox(YS_ref));
259 const Inertia::Matrix6 Y_mat = Y.
matrix();
260 Eigen::MatrixXd YS = Y_mat * constraint;
261 Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
262 BOOST_CHECK(YS.isApprox(YS_ref));
267 template<
typename Jo
intModel_>
struct init;
269 template<
typename Jo
intModel_>
275 jmodel.setIndexes(0,0,0);
280 template<
typename Scalar,
int Options>
288 JointModel jmodel(Vector3::Random().normalized());
295 template<
typename Scalar,
int Options>
303 JointModel jmodel(Vector3::Random().normalized());
310 template<
typename Scalar,
int Options>
318 JointModel jmodel(Vector3::Random().normalized());
325 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
340 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
361 template<
typename Jo
intModel_>
370 JointModel jmodel(jmodel_ref,1.,0.);
380 template <
typename Jo
intModel>
397 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
pinocchio::JointModelMimic< JointModel_ > JointModel
ForceSetTpl::Matrix6x matrix() const
void setIndexes(JointIndex id, int q, int v)
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
Block block(const int &idx, const int &len)
static InertiaTpl Random()
JointDataTpl< double > JointData
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointDataDerived createData() const
static Model run(const JointModelBase< JointModel > &jmodel)
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
void operator()(const JointModelBase< JointModel > &) const
void test_jmodel_nq_against_nq_ref(const JointModelBase< JointModel > &jmodel, const int &nq_ref)
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
SE3Tpl inverse() const
aXb = bXa.inverse()
traits< SE3Tpl >::Matrix6 Matrix6
JointModelMimic< JointModel > JointModel_
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
MotionTpl< double, 0 > Motion
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
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 setIndexes(JointIndex id, int nq, int nv)
static Model run(const JointModel_ &jmodel)
Main pinocchio namespace.
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Common traits structure to fully define base classes for CRTP.
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
BOOST_AUTO_TEST_CASE(test_ForceSet)
const JointModel & jmodel() const
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
void test_nv_against_jmodel(const JointModelBase< JointModel > &jmodel, const ConstraintBase< ConstraintDerived > &constraint)
static MotionTpl Random()
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.
void test_constraint_operations(const JointModelBase< JointModel > &jmodel)