unittest/reachable-workspace.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2023 CNRS INRIA
3 //
4 
5 #include "pinocchio/extra/reachable-workspace.hpp"
6 #include "pinocchio/algorithm/joint-configuration.hpp"
7 #include "pinocchio/multibody/sample-models.hpp"
8 #include "pinocchio/multibody/geometry.hpp"
9 #include "pinocchio/parsers/urdf.hpp"
10 
11 #include <iostream>
12 
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15 
16 #ifdef PINOCCHIO_WITH_HPP_FCL
17  #include <hpp/fcl/collision_object.h>
18 #endif // PINOCCHIO_WITH_HPP_FCL
19 
23 static pinocchio::Model createSpherical(double length)
24 {
25  using namespace pinocchio;
26  typedef SE3::Vector3 Vector3;
27  typedef SE3::Matrix3 Matrix3;
28 
29  Model modelR;
30  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
31  SE3 pos(1);
32  pos.translation() = SE3::LinearType(0, 0., 0.);
33 
34  Model::JointIndex idx;
35  Eigen::VectorXd effortMax = Eigen::VectorXd::Constant(3, 0.2);
36  Eigen::VectorXd velLim = Eigen::VectorXd::Constant(3, 0.5);
37  Eigen::VectorXd posMin(4);
38  posMin << 0, 0, 0.70710678, 0.70710678;
39  Eigen::VectorXd posMax(4);
40  posMax << 0.0, 0.0, 1.0, 0.0;
41  idx = modelR.addJoint(0, JointModelSpherical(), pos, "s", effortMax, effortMax, posMin, posMax);
42  modelR.appendBodyToJoint(idx, inertia);
43  pos.translation() = SE3::LinearType(length, 0., 0.);
44  Frame f("frame_test", idx, modelR.getFrameId("s"), pos, OP_FRAME);
45  modelR.addFrame(f);
46 
47  return modelR;
48 }
49 
52 struct robotCreationFixture
53 {
54  robotCreationFixture()
55  {
56  using namespace pinocchio;
57  length = 1;
58  filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/3DOF_planar.urdf");
59 
60  pinocchio::urdf::buildModel(filename, model);
61  SE3 pos(1);
62  pos.translation() = SE3::LinearType(0, 0., 0.);
63  Frame f("frame_test", 3, model.getFrameId("px"), pos, OP_FRAME);
64  model.addFrame(f);
65 
66  q = Eigen::VectorXd::Zero(3);
67 
68  param.n_samples = 5;
69  param.facet_dims = 2;
70 
71  time_horizon = 0.5;
72  frame_name = model.nframes - 1;
73  }
74  // Robot model
75  pinocchio::Model model;
76  // Name of the urdf
77  std::string filename;
78  // Configuration vector for which the workspace will be computed
79  Eigen::VectorXd q;
80  // Parameter of the algorithm
81  pinocchio::ReachableSetParams param;
82  // Length of the articulation
83  double length;
84  // Time horizon for reachable workspace computation
85  double time_horizon;
86  // Frame for which the workspace will be computed
87  int frame_name;
88 };
89 
90 #ifdef PINOCCHIO_WITH_HPP_FCL
91 static void
95 addObstacle(pinocchio::GeometryModel & geom_model, const double distance, const double dimension)
96 {
97  std::shared_ptr<pinocchio::fcl::CollisionGeometry> geometry =
98  std::make_shared<pinocchio::fcl::Box>(dimension, dimension, dimension);
99  std::string geometry_object_name = "obstacle";
100  pinocchio::SE3 geomPlacement(1);
101  geomPlacement.translation() = pinocchio::SE3::LinearType(distance, 0, 0);
102 
103  pinocchio::Model::JointIndex idxJ = 0;
104  pinocchio::Model::FrameIndex idxF = 0;
105 
106  pinocchio::GeometryObject geometry_object(
107  geometry_object_name, idxJ, idxF, geomPlacement, geometry);
108  geom_model.addGeometryObject(geometry_object);
109 }
110 #endif
111 BOOST_AUTO_TEST_SUITE(ReachableWorkspace)
112 
113 BOOST_AUTO_TEST_CASE(test_combination_generation)
116 {
117  int ndof = 6;
118  int ncomb = 2;
119 
120  Eigen::VectorXi indices = Eigen::VectorXi::Zero(ncomb);
121 
122  int size = static_cast<int>(
123  boost::math::factorial<double>(ndof) / boost::math::factorial<double>(ncomb)
124  / boost::math::factorial<double>(ndof - ncomb));
125  Eigen::MatrixXi res(size, ncomb);
126  for (int k = 0; k < size; k++)
127  {
128  pinocchio::internal::generateCombination(ndof, ncomb, indices);
129 
130  res.row(k) = indices;
131  }
132 
133  Eigen::MatrixXi trueRes(size, ncomb);
134  trueRes << 0, 1, 0, 2, 0, 3, 0, 4, 0, 5, 1, 2, 1, 3, 1, 4, 1, 5, 2, 3, 2, 4, 2, 5, 3, 4, 3, 5, 4,
135  5;
136  BOOST_CHECK(res == trueRes);
137 }
138 
141 BOOST_AUTO_TEST_CASE(test_compute_product)
142 {
143  int repeat = 5;
144 
145  Eigen::VectorXd temp(3);
146  temp << 2, 1, 0;
147  int n_ps = static_cast<int>(std::pow(temp.size(), repeat));
148 
149  Eigen::VectorXi indices = Eigen::VectorXi::Zero(repeat);
150 
151  Eigen::MatrixXd res(n_ps, repeat);
152  Eigen::VectorXd temp_(repeat);
153  for (int k = 0; k < n_ps; k++)
154  {
155  pinocchio::internal::productCombination(temp, repeat, indices, temp_);
156  res.row(k) = temp_;
157  }
158 
159  Eigen::MatrixXd trueRes(n_ps, repeat);
160  trueRes << 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 2, 2, 2, 2, 0, 2, 2, 2, 1, 2, 2, 2, 2, 1, 1, 2, 2, 2, 1,
161  0, 2, 2, 2, 0, 2, 2, 2, 2, 0, 1, 2, 2, 2, 0, 0, 2, 2, 1, 2, 2, 2, 2, 1, 2, 1, 2, 2, 1, 2, 0, 2,
162  2, 1, 1, 2, 2, 2, 1, 1, 1, 2, 2, 1, 1, 0, 2, 2, 1, 0, 2, 2, 2, 1, 0, 1, 2, 2, 1, 0, 0, 2, 2, 0,
163  2, 2, 2, 2, 0, 2, 1, 2, 2, 0, 2, 0, 2, 2, 0, 1, 2, 2, 2, 0, 1, 1, 2, 2, 0, 1, 0, 2, 2, 0, 0, 2,
164  2, 2, 0, 0, 1, 2, 2, 0, 0, 0, 2, 1, 2, 2, 2, 2, 1, 2, 2, 1, 2, 1, 2, 2, 0, 2, 1, 2, 1, 2, 2, 1,
165  2, 1, 1, 2, 1, 2, 1, 0, 2, 1, 2, 0, 2, 2, 1, 2, 0, 1, 2, 1, 2, 0, 0, 2, 1, 1, 2, 2, 2, 1, 1, 2,
166  1, 2, 1, 1, 2, 0, 2, 1, 1, 1, 2, 2, 1, 1, 1, 1, 2, 1, 1, 1, 0, 2, 1, 1, 0, 2, 2, 1, 1, 0, 1, 2,
167  1, 1, 0, 0, 2, 1, 0, 2, 2, 2, 1, 0, 2, 1, 2, 1, 0, 2, 0, 2, 1, 0, 1, 2, 2, 1, 0, 1, 1, 2, 1, 0,
168  1, 0, 2, 1, 0, 0, 2, 2, 1, 0, 0, 1, 2, 1, 0, 0, 0, 2, 0, 2, 2, 2, 2, 0, 2, 2, 1, 2, 0, 2, 2, 0,
169  2, 0, 2, 1, 2, 2, 0, 2, 1, 1, 2, 0, 2, 1, 0, 2, 0, 2, 0, 2, 2, 0, 2, 0, 1, 2, 0, 2, 0, 0, 2, 0,
170  1, 2, 2, 2, 0, 1, 2, 1, 2, 0, 1, 2, 0, 2, 0, 1, 1, 2, 2, 0, 1, 1, 1, 2, 0, 1, 1, 0, 2, 0, 1, 0,
171  2, 2, 0, 1, 0, 1, 2, 0, 1, 0, 0, 2, 0, 0, 2, 2, 2, 0, 0, 2, 1, 2, 0, 0, 2, 0, 2, 0, 0, 1, 2, 2,
172  0, 0, 1, 1, 2, 0, 0, 1, 0, 2, 0, 0, 0, 2, 2, 0, 0, 0, 1, 2, 0, 0, 0, 0, 1, 2, 2, 2, 2, 1, 2, 2,
173  2, 1, 1, 2, 2, 2, 0, 1, 2, 2, 1, 2, 1, 2, 2, 1, 1, 1, 2, 2, 1, 0, 1, 2, 2, 0, 2, 1, 2, 2, 0, 1,
174  1, 2, 2, 0, 0, 1, 2, 1, 2, 2, 1, 2, 1, 2, 1, 1, 2, 1, 2, 0, 1, 2, 1, 1, 2, 1, 2, 1, 1, 1, 1, 2,
175  1, 1, 0, 1, 2, 1, 0, 2, 1, 2, 1, 0, 1, 1, 2, 1, 0, 0, 1, 2, 0, 2, 2, 1, 2, 0, 2, 1, 1, 2, 0, 2,
176  0, 1, 2, 0, 1, 2, 1, 2, 0, 1, 1, 1, 2, 0, 1, 0, 1, 2, 0, 0, 2, 1, 2, 0, 0, 1, 1, 2, 0, 0, 0, 1,
177  1, 2, 2, 2, 1, 1, 2, 2, 1, 1, 1, 2, 2, 0, 1, 1, 2, 1, 2, 1, 1, 2, 1, 1, 1, 1, 2, 1, 0, 1, 1, 2,
178  0, 2, 1, 1, 2, 0, 1, 1, 1, 2, 0, 0, 1, 1, 1, 2, 2, 1, 1, 1, 2, 1, 1, 1, 1, 2, 0, 1, 1, 1, 1, 2,
179  1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 2, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 0, 2, 2, 1, 1,
180  0, 2, 1, 1, 1, 0, 2, 0, 1, 1, 0, 1, 2, 1, 1, 0, 1, 1, 1, 1, 0, 1, 0, 1, 1, 0, 0, 2, 1, 1, 0, 0,
181  1, 1, 1, 0, 0, 0, 1, 0, 2, 2, 2, 1, 0, 2, 2, 1, 1, 0, 2, 2, 0, 1, 0, 2, 1, 2, 1, 0, 2, 1, 1, 1,
182  0, 2, 1, 0, 1, 0, 2, 0, 2, 1, 0, 2, 0, 1, 1, 0, 2, 0, 0, 1, 0, 1, 2, 2, 1, 0, 1, 2, 1, 1, 0, 1,
183  2, 0, 1, 0, 1, 1, 2, 1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 0, 1, 0, 2, 1, 0, 1, 0, 1, 1, 0, 1, 0, 0,
184  1, 0, 0, 2, 2, 1, 0, 0, 2, 1, 1, 0, 0, 2, 0, 1, 0, 0, 1, 2, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0,
185  0, 0, 2, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 2, 2, 2, 2, 0, 2, 2, 2, 1, 0, 2, 2, 2, 0, 0, 2, 2, 1,
186  2, 0, 2, 2, 1, 1, 0, 2, 2, 1, 0, 0, 2, 2, 0, 2, 0, 2, 2, 0, 1, 0, 2, 2, 0, 0, 0, 2, 1, 2, 2, 0,
187  2, 1, 2, 1, 0, 2, 1, 2, 0, 0, 2, 1, 1, 2, 0, 2, 1, 1, 1, 0, 2, 1, 1, 0, 0, 2, 1, 0, 2, 0, 2, 1,
188  0, 1, 0, 2, 1, 0, 0, 0, 2, 0, 2, 2, 0, 2, 0, 2, 1, 0, 2, 0, 2, 0, 0, 2, 0, 1, 2, 0, 2, 0, 1, 1,
189  0, 2, 0, 1, 0, 0, 2, 0, 0, 2, 0, 2, 0, 0, 1, 0, 2, 0, 0, 0, 0, 1, 2, 2, 2, 0, 1, 2, 2, 1, 0, 1,
190  2, 2, 0, 0, 1, 2, 1, 2, 0, 1, 2, 1, 1, 0, 1, 2, 1, 0, 0, 1, 2, 0, 2, 0, 1, 2, 0, 1, 0, 1, 2, 0,
191  0, 0, 1, 1, 2, 2, 0, 1, 1, 2, 1, 0, 1, 1, 2, 0, 0, 1, 1, 1, 2, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0,
192  1, 1, 0, 2, 0, 1, 1, 0, 1, 0, 1, 1, 0, 0, 0, 1, 0, 2, 2, 0, 1, 0, 2, 1, 0, 1, 0, 2, 0, 0, 1, 0,
193  1, 2, 0, 1, 0, 1, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 2, 0, 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 2, 2, 2,
194  0, 0, 2, 2, 1, 0, 0, 2, 2, 0, 0, 0, 2, 1, 2, 0, 0, 2, 1, 1, 0, 0, 2, 1, 0, 0, 0, 2, 0, 2, 0, 0,
195  2, 0, 1, 0, 0, 2, 0, 0, 0, 0, 1, 2, 2, 0, 0, 1, 2, 1, 0, 0, 1, 2, 0, 0, 0, 1, 1, 2, 0, 0, 1, 1,
196  1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 2, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 2, 1, 0,
197  0, 0, 2, 0, 0, 0, 0, 1, 2, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 1, 0, 0, 0,
198  0, 0;
199  BOOST_CHECK(res == trueRes);
200 }
201 
203 BOOST_AUTO_TEST_CASE(test_compute_vel_config)
204 {
205  int ndof = 9;
206  int c1 = 3;
207  int c2 = 6;
208  Eigen::VectorXd res1 = Eigen::VectorXd::Constant(c1, 2.);
209  Eigen::VectorXd res2 = Eigen::VectorXd::Constant(c2, 4.);
210 
211  Eigen::VectorXi comb(c1);
212  comb << 1, 5, 8;
213 
214  Eigen::VectorXd resTotal(ndof);
215 
216  pinocchio::internal::computeJointVel(res1, res2, comb, resTotal);
217 
218  Eigen::VectorXd resTrue(ndof);
219  resTrue << 4, 2, 4, 4, 4, 2, 4, 4, 2;
220 
221  BOOST_CHECK(resTrue == resTotal);
222 }
223 
226 BOOST_FIXTURE_TEST_CASE(test_compute_vertex, robotCreationFixture)
227 {
228  using namespace pinocchio;
229 
230  Eigen::VectorXd q = Eigen::VectorXd::Zero(3);
231 
232  Eigen::MatrixXd vertex;
233 
234  double constraint = 0.2;
235  auto f_ = [this, &constraint](const Model & model, Data & data) -> bool {
236  SE3 pos = data.oMf[frame_name];
237  if (pos.translation()(0) < constraint)
238  return false;
239  else
240  return true;
241  };
242 
243  pinocchio::internal::computeVertex(model, q, time_horizon, frame_name, f_, vertex, param);
244 
245  BOOST_CHECK(vertex.rows() > 0);
246  BOOST_CHECK(vertex.cols() > 0);
247  for (int k = 0; k < vertex.cols(); k++)
248  {
249  BOOST_CHECK(vertex(0, k) <= length && vertex(0, k) >= constraint);
250  BOOST_CHECK(vertex(1, k) <= length && vertex(1, k) >= 0);
251  BOOST_CHECK(vertex(2, k) <= length && vertex(2, k) >= 0);
252  }
253 }
254 
255 #ifdef PINOCCHIO_WITH_HPP_FCL
256 BOOST_FIXTURE_TEST_CASE(test_reachable_workspace_with_collision, robotCreationFixture)
259 {
260  using namespace pinocchio;
261 
262  double dims = 0.1;
263  double distance = length - dims;
264 
265  GeometryModel geom_model;
266  pinocchio::urdf::buildGeom(model, filename, pinocchio::COLLISION, geom_model);
267  addObstacle(geom_model, distance, dims);
268 
269  geom_model.addAllCollisionPairs();
270 
271  ReachableSetResults res;
272 
273  pinocchio::reachableWorkspaceWithCollisionsHull(
274  model, geom_model, q, time_horizon, frame_name, res, param);
275 
276  BOOST_CHECK(res.vertex.rows() > 0);
277  BOOST_CHECK(res.faces.rows() > 0);
278 
279  for (int k = 0; k < res.vertex.cols(); k++)
280  {
281  BOOST_CHECK(res.vertex(0, k) <= length && res.vertex(0, k) >= 0);
282  BOOST_CHECK(res.vertex(1, k) <= length && res.vertex(1, k) >= 0);
283  BOOST_CHECK(res.vertex(2, k) <= length && res.vertex(2, k) >= 0);
284 
285  if (res.vertex(1, k) > dims / 2 && res.vertex(2, k) > dims / 2)
286  BOOST_CHECK(res.vertex(0, k) < distance);
287  }
288 }
289 #endif
290 
292 BOOST_AUTO_TEST_CASE(test_reachable_workspace)
293 {
294  using namespace Eigen;
295  using namespace pinocchio;
296 
297  // Load the urdf model
298  Model model;
299  pinocchio::buildModels::manipulator(model);
300 
301  Eigen::VectorXd q = (model.upperPositionLimit + model.lowerPositionLimit) / 2;
302  ReachableSetResults res;
303 
304  ReachableSetParams param;
305  param.n_samples = 5;
306  param.facet_dims = 2;
307 
308  double time_horizon = 0.2;
309  int frame_name = model.nframes - 1;
310 
311  pinocchio::reachableWorkspaceHull(model, q, time_horizon, frame_name, res, param);
312 
313  BOOST_CHECK(res.vertex.cols() > 0);
314  BOOST_CHECK(res.faces.rows() > 0);
315 }
316 
318 BOOST_AUTO_TEST_CASE(test_spherical)
319 {
320  double length = 0.4;
321  pinocchio::Model modelR = createSpherical(length);
322 
323  Eigen::VectorXd q = (modelR.upperPositionLimit + modelR.lowerPositionLimit) / 2;
324  pinocchio::normalize(modelR, q);
325 
326  pinocchio::ReachableSetResults res;
327 
328  pinocchio::ReachableSetParams param;
329  param.n_samples = 5;
330  param.facet_dims = 1;
331 
332  double time_horizon = 0.2;
333  int frame_name = modelR.nframes - 1;
334 
335  pinocchio::reachableWorkspaceHull(modelR, q, time_horizon, frame_name, res, param);
336 
337  BOOST_CHECK(res.vertex.cols() > 0);
338  BOOST_CHECK(res.faces.rows() > 0);
339 }
340 
341 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl< context::Scalar, context::Options >
Eigen
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
reachable-workspace.hpp
pinocchio::internal::computeVertex
void computeVertex(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, FilterFunction config_filter, Eigen::MatrixXd &vertex, const ReachableSetParams &params=ReachableSetParams())
Samples points to create the reachable workspace that will respect mechanical limits of the model as ...
pinocchio::DataTpl
Definition: context/generic.hpp:25
robotCreationFixture::frame_name
int frame_name
Definition: unittest/reachable-workspace.cpp:87
robotCreationFixture::robotCreationFixture
robotCreationFixture()
Definition: unittest/reachable-workspace.cpp:54
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: multibody/model.hpp:171
pinocchio::internal::productCombination
void productCombination(const Eigen::VectorXd &element, const int repeat, Eigen::VectorXi &indices, Eigen::VectorXd &combination)
Cartesian product of input element with itself. Number of repetition is specified with repeat argumen...
robotCreationFixture::param
pinocchio::ReachableSetParams param
Definition: unittest/reachable-workspace.cpp:81
robotCreationFixture::q
Eigen::VectorXd q
Definition: unittest/reachable-workspace.cpp:79
pinocchio::SE3Tpl< context::Scalar, context::Options >
robotCreationFixture::filename
std::string filename
Definition: unittest/reachable-workspace.cpp:77
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::internal::computeJointVel
void computeJointVel(const Eigen::VectorXd &res1, const Eigen::VectorXd &res2, const Eigen::VectorXi &comb, Eigen::VectorXd &qv)
Computes the joint configuration associated with the permutation results stored in res1 and res2.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_combination_generation)
test generation combination function and compare to the output of the python function itertools....
Definition: unittest/reachable-workspace.cpp:115
pinocchio::JointModelSpherical
JointModelSphericalTpl< context::Scalar > JointModelSpherical
Definition: multibody/joint/fwd.hpp:73
BOOST_FIXTURE_TEST_CASE
BOOST_FIXTURE_TEST_CASE(test_compute_vertex, robotCreationFixture)
test of the vertex computation for a 2DOf planar robot. Verify that vertex are inside the rectangle o...
Definition: unittest/reachable-workspace.cpp:226
robotCreationFixture::model
pinocchio::Model model
Definition: unittest/reachable-workspace.cpp:75
pinocchio::GeometryObject
Definition: multibody/geometry-object.hpp:87
pinocchio::ReachableSetResults
Structure containing the return value for the reachable algorithm.
Definition: reachable-workspace.hpp:25
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
robotCreationFixture::time_horizon
double time_horizon
Definition: unittest/reachable-workspace.cpp:85
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
pinocchio::ModelTpl::addJoint
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.
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: multibody/frame.hpp:55
pinocchio::reachableWorkspaceHull
void reachableWorkspaceHull(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q0, const double time_horizon, const int frame_id, ReachableSetResults &res, const ReachableSetParams &params=ReachableSetParams())
Computes the convex Hull reachable workspace on a fixed time horizon. For more information,...
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
res
res
pinocchio::ReachableSetParams
Parameters for the reachable space algorithm.
Definition: reachable-workspace.hpp:39
joint-configuration.hpp
pinocchio::ReachableSetParams::facet_dims
int facet_dims
Definition: reachable-workspace.hpp:42
pinocchio::ModelTpl::appendBodyToJoint
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
collision-with-point-clouds.geometry
geometry
Definition: collision-with-point-clouds.py:81
pinocchio::ModelTpl::getFrameId
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
urdf.hpp
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: multibody/model.hpp:70
size
FCL_REAL size() const
geometry.hpp
pos
pos
pinocchio::urdf::buildGeom
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
pinocchio::ModelTpl::nframes
int nframes
Number of operational frames.
Definition: multibody/model.hpp:111
pinocchio::ReachableSetParams::n_samples
int n_samples
Definition: reachable-workspace.hpp:41
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
append-urdf-model-with-another-model.geom_model
geom_model
Definition: append-urdf-model-with-another-model.py:26
distance
FCL_REAL distance(const KDOP< N > &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
pinocchio::internal::generateCombination
void generateCombination(const int n, const int k, Eigen::VectorXi &indices)
Return a subsequence of length k of elements from range 0 to n. Inspired by https://docs....
pinocchio::OP_FRAME
@ OP_FRAME
operational frame: user-defined frames that are defined at runtime
Definition: multibody/frame.hpp:33
robotCreationFixture
Create a fixture structure for boost test case. It will create a 3DOF robot with prismatic joints and...
Definition: unittest/reachable-workspace.cpp:52
pinocchio::ModelTpl::addFrame
FrameIndex addFrame(const Frame &frame, const bool append_inertia=true)
Adds a frame to the kinematic tree. The inertia stored within the frame will be happened to the inert...
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
sample-models.hpp
c2
c2
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: multibody/model.hpp:174
collision_object.h
length
FCL_REAL length[2]
createSpherical
static pinocchio::Model createSpherical(double length)
Create a spherical joint with a stick of length l attached to it.
Definition: unittest/reachable-workspace.cpp:23
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:914
robotCreationFixture::length
double length
Definition: unittest/reachable-workspace.cpp:83
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:49