5 #include "pinocchio/multibody/model.hpp" 6 #include "pinocchio/multibody/data.hpp" 7 #include "pinocchio/multibody/visitor.hpp" 11 #include <boost/test/unit_test.hpp> 12 #include <boost/utility/binary.hpp> 24 template<
typename Jo
intModel>
27 const pinocchio::Model &
model,
28 pinocchio::Data &
data,
34 BOOST_CHECK(jindex == jmodel.
id());
35 std::cout <<
"joint name: " << jmodel.
shortname() << std::endl;
47 template<
typename Jo
intModel>
49 const pinocchio::Model &
model,
50 pinocchio::Data &
data,
56 BOOST_CHECK(jindex == jmodel.
id());
57 std::cout <<
"joint name: " << jmodel.
shortname() << std::endl;
65 template<
typename Jo
intModel>
69 std::cout <<
"joint name: " << jmodel.
shortname() << std::endl;
77 template<
typename Jo
intModel>
83 std::cout <<
"joint name: " << jmodel.
shortname() << std::endl;
95 template<
typename Jo
intModel1,
typename Jo
intModel2>
100 const pinocchio::Model &
model,
101 pinocchio::Data &
data,
109 BOOST_CHECK(jindex == jmodel1.
id());
110 BOOST_CHECK(jindex == jmodel2.
id());
111 std::cout <<
"joint1 name: " << jmodel1.
shortname() << std::endl;
112 std::cout <<
"joint2 name: " << jmodel2.
shortname() << std::endl;
124 template<
typename Jo
intModel1,
typename Jo
intModel2>
127 const pinocchio::Model &
model,
128 pinocchio::Data &
data,
134 BOOST_CHECK(jindex == jmodel1.
id());
135 BOOST_CHECK(jindex == jmodel2.
id());
136 std::cout <<
"joint1 name: " << jmodel1.
shortname() << std::endl;
137 std::cout <<
"joint2 name: " << jmodel2.
shortname() << std::endl;
144 template<
typename Jo
intModel1,
typename Jo
intModel2>
148 BOOST_CHECK(!jmodel1.
shortname().empty());
149 BOOST_CHECK(!jmodel2.
shortname().empty());
150 std::cout <<
"joint1 name: " << jmodel1.
shortname() << std::endl;
151 std::cout <<
"joint2 name: " << jmodel2.
shortname() << std::endl;
158 template<
typename Jo
intModel1,
typename Jo
intModel2>
166 BOOST_CHECK(!jmodel1.
shortname().empty());
167 BOOST_CHECK(!jmodel2.
shortname().empty());
168 std::cout <<
"joint1 name: " << jmodel1.
shortname() << std::endl;
169 std::cout <<
"joint2 name: " << jmodel2.
shortname() << std::endl;
170 std::cout <<
"jdata1 name: " << jdata1.
classname() << std::endl;
171 std::cout <<
"jdata2 name: " << jdata2.
classname() << std::endl;
177 template<
typename Jo
intModel_>
struct init;
179 template<
typename Jo
intModel_>
189 template<
typename Scalar,
int Options>
190 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
197 JointModel jmodel(Vector3::Random().normalized());
203 template<
typename Scalar,
int Options>
204 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
211 JointModel jmodel(Vector3::Random().normalized());
217 template<
typename Scalar,
int Options>
218 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
225 JointModel jmodel(Vector3::Random().normalized());
231 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
232 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
245 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollection>
246 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
264 template<
typename Jo
intModel_>
265 struct init<pinocchio::JointModelMimic<JointModel_> >
273 JointModel jmodel(boost::get<JointModel_>(model.
joints[joint_id]),1.,0.);
283 template<
typename Jo
intModel>
296 using namespace pinocchio;
329 using namespace pinocchio;
340 data.joints[
i],data.joints[
i],
358 data.joints[
i],data.joints[
i]);
362 BOOST_AUTO_TEST_SUITE_END()
static JointModel run(const pinocchio::Model &)
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
pinocchio::JointModelMimic< JointModel_ > JointModel
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
static JointModel_ run(const pinocchio::Model &)
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2)
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, pinocchio::JointDataBase< typename JointModel1::JointDataDerived > &jdata1, pinocchio::JointDataBase< typename JointModel2::JointDataDerived > &jdata2)
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel)
int njoints
Number of joints.
std::string shortname() const
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
static JointModel run(const pinocchio::Model &)
static std::string classname()
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, pinocchio::JointDataBase< typename JointModel::JointDataDerived > &jdata)
static JointModel run(const pinocchio::Model &)
BOOST_AUTO_TEST_CASE(test_run_over_all_joints_unary_visitor)
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
static void run(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, ArgsTmp args)
static void algo(const pinocchio::JointModelBase< JointModel1 > &jmodel1, const pinocchio::JointModelBase< JointModel2 > &jmodel2, pinocchio::JointDataBase< typename JointModel1::JointDataDerived > &jdata1, pinocchio::JointDataBase< typename JointModel2::JointDataDerived > &jdata2, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, pinocchio::JointDataBase< typename JointModel::JointDataDerived > &jdata, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
static JointModel run(const pinocchio::Model &)
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
AppendJointToModel(pinocchio::Model &model)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
pinocchio::JointIndex JointIndex
static void run(const JointModelBase< JointModelDerived1 > &jmodel1, const JointModelBase< JointModelDerived2 > &jmodel2, typename JointModelBase< JointModelDerived1 >::JointDataDerived &jdata1, typename JointModelBase< JointModelDerived2 >::JointDataDerived &jdata2, ArgsTmp args)
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
#define BOOST_TEST_MODULE
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
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
static JointModel run(const pinocchio::Model &model)
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
static void algo(const pinocchio::JointModelBase< JointModel > &jmodel, const pinocchio::Model &model, pinocchio::Data &data, pinocchio::JointIndex jindex)
static JointModel run(const pinocchio::Model &)
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType
bf::vector< const pinocchio::Model &, pinocchio::Data &, pinocchio::JointIndex > ArgsType