5 #include "pinocchio/multibody/data.hpp" 6 #include "pinocchio/algorithm/joint-configuration.hpp" 7 #include "pinocchio/algorithm/kinematics.hpp" 8 #include "pinocchio/algorithm/geometry.hpp" 10 #include "pinocchio/serialization/fwd.hpp" 11 #include "pinocchio/serialization/archive.hpp" 13 #include "pinocchio/serialization/spatial.hpp" 15 #include "pinocchio/serialization/frame.hpp" 17 #include "pinocchio/serialization/joints.hpp" 18 #include "pinocchio/serialization/model.hpp" 19 #include "pinocchio/serialization/data.hpp" 21 #include "pinocchio/serialization/geometry.hpp" 23 #include "pinocchio/parsers/sample-models.hpp" 27 #include <boost/test/unit_test.hpp> 28 #include <boost/utility/binary.hpp> 30 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
32 template<
typename T1,
typename T2 = T1>
35 static bool run(
const T1 & v1,
const T2 & v2)
48 #ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE 49 template<
typename Scalar,
int NumIndices,
int Options,
typename IndexType>
50 struct call_equality_op< pinocchio::Tensor<Scalar,NumIndices,Options,IndexType> >
54 static bool run(
const T & v1,
const T & v2)
56 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options>
VectorXd;
57 Eigen::Map<const VectorXd> map1(v1.data(),v1.size(),1);
58 Eigen::Map<const VectorXd> map2(v2.data(),v2.size(),1);
66 const std::string & filename,
67 const std::string & tag_name)
72 const std::string txt_filename = filename +
".txt";
84 std::stringstream ss_out;
89 std::istringstream is(ss_out.str());
101 std::string str_in(str_out);
109 const std::string xml_filename = filename +
".xml";
121 const std::string bin_filename = filename +
".bin";
133 boost::asio::streambuf buffer;
160 const size_t size = 10000000;
162 BOOST_CHECK(size == static_buffer.
size());
164 const size_t new_size = 2*
size;
165 static_buffer.
resize(new_size);
166 BOOST_CHECK(new_size == static_buffer.
size());
168 BOOST_CHECK(static_buffer.
data() != NULL);
169 BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).
data() != NULL);
170 BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).
data() == static_buffer.
data());
175 using namespace pinocchio;
177 const Eigen::DenseIndex num_cols = 10;
178 const Eigen::DenseIndex num_rows = 20;
180 const Eigen::DenseIndex array_size = 3;
182 Eigen::MatrixXd Mat = Eigen::MatrixXd::Random(num_rows,num_cols);
183 generic_test(Mat,TEST_SERIALIZATION_FOLDER
"/eigen_matrix",
"matrix");
186 generic_test(Vec,TEST_SERIALIZATION_FOLDER
"/eigen_vector",
"vector");
189 generic_test(array,TEST_SERIALIZATION_FOLDER
"/eigen_array",
"array");
191 const Eigen::DenseIndex tensor_size = 3;
192 const Eigen::DenseIndex
x_dim = 10,
y_dim = 20, z_dim = 30;
195 Tensor3x tensor(x_dim,
y_dim,z_dim);
197 Eigen::Map<Eigen::VectorXd>(tensor.data(),tensor.size(),1).setRandom();
199 generic_test(tensor,TEST_SERIALIZATION_FOLDER
"/eigen_tensor",
"tensor");
204 using namespace pinocchio;
206 SE3 M(SE3::Random());
210 generic_test(m,TEST_SERIALIZATION_FOLDER
"/Motion",
"Motion");
212 Force f(Force::Random());
213 generic_test(f,TEST_SERIALIZATION_FOLDER
"/Force",
"Force");
216 generic_test(S,TEST_SERIALIZATION_FOLDER
"/Symmetric3",
"Symmetric3");
219 generic_test(I,TEST_SERIALIZATION_FOLDER
"/Inertia",
"Inertia");
224 using namespace pinocchio;
227 generic_test(frame,TEST_SERIALIZATION_FOLDER
"/Frame",
"Frame");
230 template<
typename Jo
intModel_>
struct init;
232 template<
typename Jo
intModel_>
238 jmodel.setIndexes(0,0,0);
243 template<
typename Scalar,
int Options>
251 JointModel jmodel(Vector3::Random().normalized());
258 template<
typename Scalar,
int Options>
266 JointModel jmodel(Vector3::Random().normalized());
273 template<
typename Scalar,
int Options>
281 JointModel jmodel(Vector3::Random().normalized());
288 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
303 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
320 template<
typename Jo
intModel_>
329 JointModel jmodel(jmodel_ref,1.,0.);
337 template <
typename Jo
intModel>
344 template<
typename Jo
intType>
345 static void test(JointType & jmodel)
347 generic_test(jmodel,TEST_SERIALIZATION_FOLDER
"/Joint",
"jmodel");
354 using namespace pinocchio;
360 template <
typename Jo
intModel>
363 typedef typename JointModel::JointDerived JointDerived;
371 JointDataBase & jdata_base =
static_cast<JointDataBase &
>(jdata);
381 jmodel.
calc(jdata,q_random);
382 Transform &
m = jdata_base.
M();
385 Constraint & S = jdata_base.
S();
389 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
395 template<
typename Jo
intModel>
399 typedef typename JointModelMimic::JointDerived JointDerived;
407 JointDataMimic jdata_mimic = jmodel_mimic.
createData();
408 JointDataBase & jdata_mimic_base =
static_cast<JointDataBase &
>(jdata_mimic);
418 jmodel_mimic.
calc(jdata_mimic,q_random);
419 Transform &
m = jdata_mimic_base.
M();
422 Constraint & S = jdata_mimic_base.
S();
426 template<
typename Transform>
427 static void test(Transform & m)
429 generic_test(m,TEST_SERIALIZATION_FOLDER
"/JointTransform",
"transform");
436 using namespace pinocchio;
442 template <
typename Jo
intModel>
445 typedef typename JointModel::JointDerived JointDerived;
453 JointDataBase & jdata_base =
static_cast<JointDataBase &
>(jdata);
464 jmodel.
calc(jdata,q_random,v_random);
465 Motion &
m = jdata_base.
v();
469 Bias &
b = jdata_base.
c();
473 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
479 template<
typename Jo
intModel>
483 typedef typename JointModelMimic::JointDerived JointDerived;
491 JointDataMimic jdata_mimic = jmodel_mimic.
createData();
492 JointDataBase & jdata_mimic_base =
static_cast<JointDataBase &
>(jdata_mimic);
503 jmodel_mimic.
calc(jdata_mimic,q_random,v_random);
504 Motion &
m = jdata_mimic_base.
v();
508 Bias &
b = jdata_mimic_base.
c();
512 template<
typename Motion>
515 generic_test(m,TEST_SERIALIZATION_FOLDER
"/JointMotion",
"motion");
522 using namespace pinocchio;
528 template <
typename Jo
intModel>
531 typedef typename JointModel::JointDerived JointDerived;
546 jmodel.
calc(jdata,q_random,v_random);
547 pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
552 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
556 typedef typename JointModel::JointDerived JointDerived;
566 JointModel & jmodel = boost::get<JointModel>(model.
joints[1]);
571 JointData & jdata = boost::get<JointData>(data.
joints[1]);
573 jmodel.
calc(jdata,q_random,v_random);
574 pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
580 template<
typename Jo
intModel>
584 typedef typename JointModelMimic::JointDerived JointDerived;
589 JointDataMimic jdata_mimic = jmodel_mimic.
createData();
600 jmodel_mimic.
calc(jdata_mimic,q_random,v_random);
601 pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
602 jmodel_mimic.
calc_aba(jdata_mimic,I,
false);
606 template<
typename Jo
intData>
609 generic_test(joint_data,TEST_SERIALIZATION_FOLDER
"/JointData",
"data");
616 using namespace pinocchio;
617 boost::mpl::for_each<JointModelVariant::types>(
TestJointData());
622 using namespace pinocchio;
627 generic_test(model,TEST_SERIALIZATION_FOLDER
"/Model",
"Model");
632 using namespace pinocchio;
637 const std::string & fake_filename =
"this_is_a_fake_filename";
640 const std::string complete_filename = fake_filename +
".txt";
641 BOOST_REQUIRE_THROW(
loadFromText(model,complete_filename),
642 std::invalid_argument);
645 saveToText(model,TEST_SERIALIZATION_FOLDER
"/model.txt");
646 saveToXML(model,TEST_SERIALIZATION_FOLDER
"/model.xml",
"model");
647 saveToBinary(model,TEST_SERIALIZATION_FOLDER
"/model.bin");
650 const std::string complete_filename = fake_filename +
".txte";
652 BOOST_REQUIRE_THROW(
loadFromText(model,complete_filename),
653 std::invalid_argument);
657 const std::string complete_filename = fake_filename +
".xmle";
658 BOOST_REQUIRE_THROW(
loadFromXML(model,complete_filename,
"model"),
659 std::invalid_argument);
663 const std::string complete_filename = fake_filename +
".bine";
665 std::invalid_argument);
672 using namespace pinocchio;
679 generic_test(data,TEST_SERIALIZATION_FOLDER
"/Data",
"Data");
684 using namespace pinocchio;
690 #ifdef PINOCCHIO_WITH_HPP_FCL 692 pinocchio::buildModels::humanoidGeometries(model,geom_model);
699 generic_test(geom_data,TEST_SERIALIZATION_FOLDER
"/GeomData",
"GeomData");
705 BOOST_AUTO_TEST_SUITE_END()
static void test(Motion &m)
void operator()(const pinocchio::JointModelBase< JointModel > &) const
pinocchio::JointModelMimic< JointModel_ > JointModel
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
void loadFromXML(T &object, const std::string &filename, const std::string &tag_name)
Loads an object from a XML file.
void operator()(const pinocchio::JointModelBase< JointModel > &) const
BOOST_AUTO_TEST_CASE(test_static_buffer)
void saveToStringStream(const T &object, std::stringstream &ss)
Saves an object inside a std::stringstream.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
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.
std::string saveToString(const T &object)
Saves an object inside a std::string.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
BiasTypeConstRef c() const
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
JointDataTpl< double > JointData
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
void saveToText(const T &object, const std::string &filename)
Saves an object inside a TXT file.
TansformTypeConstRef M() const
MotionTypeConstRef v() const
void operator()(const pinocchio::JointModelBase< JointModel > &) const
void operator()(const pinocchio::JointModelMimic< JointModel > &)
char * data()
Returns the pointer on the data.
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.
void humanoid(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a 28-DOF kinematic chain of a floating humanoid robot.
void operator()(const pinocchio::JointModelMimic< JointModel > &)
void loadFromBinary(T &object, boost::asio::streambuf &buffer)
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
Static buffer with pre-allocated memory.
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.
size_t size() const
Returns the current size of the buffer.
static void test(JointData &joint_data)
MotionTpl< double, 0 > Motion
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
void generic_test(const T &object, const std::string &filename, const std::string &tag_name)
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void loadFromText(T &object, const std::string &filename)
Loads an object from a TXT file.
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
static bool run(const T1 &v1, const T2 &v2)
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
void resize(const size_t new_size)
Increase the capacity of the vector to a value that's greater or equal to new_size.
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
void setIndexes(JointIndex id, int nq, int nv)
void loadFromString(T &object, const std::string &str)
Loads an object from a std::string.
void loadFromStringStream(T &object, std::istringstream &is)
Loads an object from a std::stringstream.
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
void saveToXML(const T &object, const std::string &filename, const std::string &tag_name)
Saves an object inside a XML file.
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
int nv
Dimension of the velocity vector space.
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Common traits structure to fully define base classes for CRTP.
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > &)
ConstraintTypeConstRef S() const
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
sensor frame: defined in a sensor element
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
bool run_call_equality_op(const T &v1, const T &v2)
JointDataDerived createData() const
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > &)
static void test(JointType &jmodel)
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointDataDerived createData() const
void saveToBinary(const T &object, boost::asio::streambuf &buffer)