Drake
rigid_body_tree.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <set>
6 #include <string>
7 #include <unordered_map>
8 #include <unordered_set>
9 #include <utility>
10 #include <vector>
11 
12 #include <Eigen/Dense>
13 #include <Eigen/LU>
14 
15 #include "drake/common/constants.h"
34 
35 #define BASIS_VECTOR_HALF_COUNT \
36  2 // number of basis vectors over 2 (i.e. 4 basis vectors in this case)
37 
38 typedef Eigen::Matrix<double, 3, BASIS_VECTOR_HALF_COUNT> Matrix3kd;
39 
40 /**
41  * Defines RigidBodyTree constants. A separate struct is necessary to avoid
42  * having these constants being templated on `<T>`. For more details about the
43  * problem with having these templated on `<T>`, see #4169.
44  */
46  /**
47  * Defines the name of the RigidBody within a RigidBodyTree that represents
48  * the world.
49  */
50  static const char* const kWorldName;
51 
52  /**
53  * Defines the index of the RigidBody within a RigidBodyTree that represents
54  * the world.
55  */
56  static const int kWorldBodyIndex;
57 
58  /**
59  * The ID of the first non-world model instance in the tree.
60  */
62 
63  /**
64  * Defines the default model instance ID set. This is a set containing the
65  * model instance ID of the first model instance that is added to the tree.
66  */
67  static const std::set<int> default_model_instance_id_set;
68 };
69 
70 /**
71  * Maintains a vector of RigidBody objects that are arranged into a kinematic
72  * tree via DrakeJoint objects. It provides various utility methods for
73  * computing kinematic and dynamics properties of the RigidBodyTree.
74  *
75  * The internal organization of a RigidBodyTree's generalized state vector is as
76  * follows:
77  *
78  * <pre>
79  * [model instance 1's generalized coordinate vector]
80  * [model instance 2's generalized coordinate vector]
81  * ...
82  * [model instance 1's generalized velocity vector]
83  * [model instance 2's generalized velocity vector]
84  * ...
85  * </pre>
86  *
87  * Each RigidBody maintains for its joint that connects to its parent the
88  * indices of the joint's generalized coordinate vector and generalized velocity
89  * vector in the RigidBodyTree's generalized state vector.
90  *
91  * The starting index of the joint's generalized coordinate vector in the
92  * RigidBodyTree's generalized state vector can be obtained by executing
93  * RigidBody::get_position_start_index().
94  *
95  * The starting index of the joint's generalized velocity vector in the
96  * RigidBodyTree's generalized state vector can be computed as
97  * follows: RigidBodyTree::get_num_positions() +
98  * RigidBody::get_velocity_start_index().
99  *
100  * Note that the velocity index starts at the beginning of the velocity state
101  * variables and not at the beginning of the full state of the RigidBodyTree.
102  * This is why the total number of positions needs to be added to the velocity
103  * index to get its index in the RigidBodyTree's full state vector.
104  *
105  * @tparam T The scalar type. Must be a valid Eigen scalar.
106  */
107 template <typename T>
108 class RigidBodyTree {
109  public:
110  /// A constructor that initializes the gravity vector to be [0, 0, -9.81] and
111  /// a single RigidBody named "world". This RigidBody can be accessed by
112  /// calling RigidBodyTree::world().
113  RigidBodyTree();
114 
115  virtual ~RigidBodyTree();
116 
117  /**
118  * Returns a deep clone of this RigidBodyTree<double>. Currently, everything
119  * *except* for collision and visual elements are cloned. Only supported for
120  * T = double.
121  */
122  std::unique_ptr<RigidBodyTree<double>> Clone() const {
123  // N.B. We specialize this method below for T = double. This method body
124  // is the default implementation for all _other_ values of T.
125  throw std::logic_error("RigidBodyTree::Clone only supports T = double");
126  }
127 
128  /**
129  * Adds a new model instance to this `RigidBodyTree`. The model instance is
130  * identified by a unique model instance ID, which is the return value of
131  * this method.
132  */
133  // This method is not thread safe!
134  int add_model_instance();
135 
136  // This method is not thread safe.
137  int get_next_clique_id() { return next_available_clique_++; }
138 
139  // TODO(liang.fok) Update this method implementation once the world
140  // is assigned its own model instance ID (#3088). It should return
141  // num_model_instances_ - 1.
142  /**
143  * Returns the number of model instances in the tree, not including the world.
144  */
145  int get_num_model_instances() const { return num_model_instances_; }
146 
147  DRAKE_DEPRECATED("Please use get_num_model_instances().")
148  int get_number_of_model_instances() const;
149 
150 
151  /**
152  * Adds a frame.
153  *
154  * @param frame Frame to be added.
155  * @pre Neither a body nor frame with the same identifying information (name
156  * and model id / name) should already exist in the tree.
157  * @throws std::runtime_error if preconditions are not met.
158  */
159  // TODO(eric.cousineau): Rename to `AddFrame`.
160  void addFrame(std::shared_ptr<RigidBodyFrame<T>> frame);
161 
162  /**
163  * Returns a map from DOF position name to DOF index within the output vector
164  * of this RigidBodyTree.
165  *
166  * <b>WARNING:</b> There is a known bug in this method, see: #4697.
167  */
168  std::map<std::string, int> computePositionNameToIndexMap() const;
169 
170  void surfaceTangents(
171  Eigen::Map<Eigen::Matrix3Xd> const& normals,
172  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
173  std::vector<Eigen::Map<Eigen::Matrix3Xd>>& tangents) const;
174 
175  /**
176  * Applies the given transform to the given @p body's collision elements,
177  * displacing them from their current configurations. These new poses
178  * will be considered the elements' pose with respect to the body.
179  *
180  * This is important to the parsing code to maintain a Drake RigidBodyTree
181  * invariant. RigidBody instances do not maintain their own pose relative
182  * to their in-board joint. The joint's space is considered to be the body's
183  * space. So, if a URDF or SDF file defines the body with a non-identity pose
184  * relative to the parent, the parser uses this to move the collision elements
185  * relative to the effective body frame -- that of the parent joint.
186  *
187  * @param body The body whose collision elements will be moved.
188  * @param displace_transform The transform to apply to each collision element.
189  * @param true if the collision element was successfully updated.
190  * @returns true if the @body's elements were successfully transformed.
191  */
192  bool transformCollisionFrame(
193  RigidBody<T>* body,
194  const Eigen::Isometry3d& displace_transform);
195 
196  void compile(); // call me after the model is loaded
197 
198  Eigen::VectorXd getZeroConfiguration() const;
199 
200  Eigen::VectorXd getRandomConfiguration(
201  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
202  std::default_random_engine& generator) const;
203 
204  /**
205  * Returns the name of the position state at index @p position_num
206  * within this `RigidBodyTree`'s state vector.
207  *
208  * @param[in] position_num An index value between zero and
209  * number_of_positions().
210  *
211  * @return The name of the position value at index @p position_num.
212  */
213  std::string get_position_name(int position_num) const;
214 
215  /**
216  * Returns the name of the velocity state at index @p velocity_num
217  * within this `RigidBodyTree`'s state vector.
218  *
219  * @param[in] velocity_num An index value between number_of_positions() and
220  * number_of_veocities().
221  *
222  * @return The name of the velocity value at index @p velocity_num.
223  */
224  std::string get_velocity_name(int velocity_num) const;
225 
226 // TODO(liang.fok) Remove this deprecated method prior to release 1.0.
227  DRAKE_DEPRECATED("Please use get_position_name.")
228  std::string getPositionName(int position_num) const;
229 
230 // TODO(liang.fok) Remove this deprecated method prior to release 1.0.
231  DRAKE_DEPRECATED("Please use get_velocity_name.")
232  std::string getVelocityName(int velocity_num) const;
233 
234  std::string getStateName(int state_num) const;
235 
236  void drawKinematicTree(std::string graphviz_dotfile_filename) const;
237 
238  /// Checks whether @p cache is valid for use with this RigidBodyTree. Throws
239  /// a std::runtime_error exception if it is not valid.
240  template <typename Scalar>
241  void CheckCacheValidity(const KinematicsCache<Scalar>& cache) const;
242 
243  /// Creates a KinematicsCache to perform computations with this RigidBodyTree.
244  /// The returned KinematicsCache is consistently templated on the scalar type
245  /// for this RigidBodyTree instance.
246  /// Aborts if this RigidBodyTree was not previously initialized with a call
247  /// to RigidBodyTree::compile().
248  /// @returns The created KinematicsCache.
249  KinematicsCache<T> CreateKinematicsCache() const;
250 
251  /// A helper template method used to create a KinematicsCache templated on
252  /// `CacheT` from a RigidBodyTree templated on `T`, with `CacheT` and `T`
253  /// not necessarily the same scalar type.
254  /// This method is particularly useful in mex files where only a reference
255  /// to a `RigidBodyTree<double>` is available to create kinematics caches
256  /// on different scalar types.
257  /// Aborts if this RigidBodyTree was not previously initialized with a call
258  /// to RigidBodyTree::compile().
259  ///
260  /// Users should not call this method but instead create KinematicsCache
261  /// objects with RigidBodyTree:CreateKinematicsCache().
262  ///
263  /// @tparam CacheT The scalar type for the returned KinematicsCache.
264  /// @returns A KinematicsCache templated on `CacheT` that can be used for
265  /// computations on this RigidBodyTree with methods instantiated on `CacheT`.
266  // TODO(amcastro-tri): Remove this method once older pieces of code such as
267  // createKinematicsCacheAutoDiffmex.cpp are updated to use a RigidBodyTree to
268  // manage cache creation.
269  template <typename CacheT>
270  KinematicsCache<CacheT> CreateKinematicsCacheWithType() const;
271 
272  /// Creates a KinematicsCache given a reference to a vector of rigid bodies
273  /// contained within a RigidBodyTree.
274  /// This method is static since all the information to create the
275  /// corresponding KinematicsCache resides in the input parameter vector
276  /// `bodies`.
277  ///
278  /// @param bodies A vector of unique pointers to the rigid bodies of a given
279  /// RigidBodyTree for which a KinematicsCache needs to be created.
280  /// @returns The created KinematicsCache.
281  //
282  // TODO(amcastro-tri): Remove this method once older pieces of code such as
283  // KinematicsCacheHelper are updated to use a RigidBodyTree to manage cache
284  // creation.
285  static KinematicsCache<T>
286  CreateKinematicsCacheFromBodiesVector(
287  const std::vector<std::unique_ptr<RigidBody<T>>>& bodies);
288 
289  /// Initializes a `KinematicsCache` with the given configuration @p q,
290  /// computes the kinematics, and returns the cache.
291  ///
292  /// This method is explicitly instantiated in rigid_body_tree.cc for a
293  /// small set of supported `DerivedQ`.
294  template <typename DerivedQ>
296  const Eigen::MatrixBase<DerivedQ>& q) const;
297 
298  /// Initializes a `KinematicsCache` with the given configuration @p q
299  /// and velocity @p v, computes the kinematics, and returns the cache.
300  ///
301  /// This method is explicitly instantiated in rigid_body_tree.cc for a
302  /// small set of supported `DerivedQ` and `DerivedV`.
303  template <typename DerivedQ, typename DerivedV>
305  const Eigen::MatrixBase<DerivedQ>& q,
306  const Eigen::MatrixBase<DerivedV>& v, bool compute_JdotV = true) const;
307 
308  /// Computes the kinematics on the given @p cache.
309  ///
310  /// This method is explicitly instantiated in rigid_body_tree.cc for a
311  /// small set of supported Scalar types.
312  template <typename Scalar>
313  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
314  void doKinematics(KinematicsCache<Scalar>& cache,
315  bool compute_JdotV = false) const;
316 
317  /**
318  * Returns true if @p body is part of a model instance whose ID is in
319  * @p model_instance_id_set.
320  */
321  bool is_part_of_model_instances(
322  const RigidBody<T>& body,
323  const std::set<int>& model_instance_id_set) const;
324 
325  /**
326  * Computes the total combined mass of a set of model instances.
327  *
328  * @param[in] model_instance_id_set A set of model instance ID values
329  * corresponding to the model instances whose masses should be included in the
330  * returned value.
331  *
332  * @returns The total combined mass of the model instances in
333  * @p model_instance_id_set.
334  */
335  double getMass(const std::set<int>& model_instance_id_set =
337 
338  template <typename Scalar>
339  Eigen::Matrix<Scalar, drake::kSpaceDimension, 1> centerOfMass(
340  const KinematicsCache<Scalar>& cache,
341  const std::set<int>& model_instance_id_set =
343 
344  /**
345  * Computes the summed spatial inertia in the world frame of all bodies that
346  * belong to model instances in @p model_instance_id_set.
347  * @param cache Reference to the KinematicsCache.
348  * @param model_instance_id_set A set of model instance ID values
349  * corresponding to the model instances whose spatial inertia should be
350  * included in the returned value.
351  * @return The summed spatial inertia.
352  */
353  drake::Matrix6<T> LumpedSpatialInertiaInWorldFrame(
354  const KinematicsCache<T>& cache,
355  const std::set<int>& model_instance_id_set =
357 
358  /**
359  * Computes the summed spatial inertia in the world frame of all the bodies
360  * in @p bodies_of_interest.
361  * @param cache Reference to the KinematicsCache.
362  * @param bodies_of_interest Vector of bodies, whose spatial inertia will be
363  * summed and returned.
364  * @return The summed spatial inertia.
365  */
366  drake::Matrix6<T> LumpedSpatialInertiaInWorldFrame(
367  const KinematicsCache<T>& cache,
368  const std::vector<const RigidBody<T>*>& bodies_of_interest) const;
369 
370  /// Computes the pose `X_WB` of @p body's frame B in the world frame W.
371  /// @param cache Reference to the KinematicsCache.
372  /// @param body Reference to the RigidBody.
373  /// @retval `X_WB`
375  const KinematicsCache<T>& cache, const RigidBody<T>& body) const {
376  return CalcFramePoseInWorldFrame(
377  cache, body, drake::Isometry3<T>::Identity());
378  }
379 
380  /// Computes the pose `X_WF` of @p frame_F in the world frame W. @p frame_F
381  /// does not necessarily need to be owned by this RigidBodyTree. However,
382  /// the RigidBody to which @p frame_F attaches to has to be owned by this
383  /// RigidBodyTree.
384  /// @param cache Reference to the KinematicsCache.
385  /// @param frame_F Reference to the RigidBodyFrame.
386  /// @retval `X_WF`
388  const KinematicsCache<T>& cache, const RigidBodyFrame<T>& frame_F) const {
389  return CalcFramePoseInWorldFrame(cache, frame_F.get_rigid_body(),
390  frame_F.get_transform_to_body().template cast<T>());
391  }
392 
393  /// Computes the pose `X_WF` of the rigid body frame F in the world frame W.
394  /// Frame F is rigidly attached to @p body.
395  /// @param cache Reference to the KinematicsCache.
396  /// @param body Reference to the RigidBody.
397  /// @param X_BF The pose of frame F in body frame B.
398  /// @retval `X_WF`
399  drake::Isometry3<T> CalcFramePoseInWorldFrame(
400  const KinematicsCache<T>& cache, const RigidBody<T>& body,
401  const drake::Isometry3<T>& X_BF) const;
402 
403  /// Computes the spatial velocity `V_WB` of @p body's frame B measured and
404  /// expressed in the world frame W.
405  /// @param cache Reference to the KinematicsCache.
406  /// @param body Reference to the RigidBody.
407  /// @retval `V_WB`
408  drake::Vector6<T> CalcBodySpatialVelocityInWorldFrame(
409  const KinematicsCache<T>& cache, const RigidBody<T>& body) const;
410 
411  /// Computes the spatial velocity `V_WF` of RigidBodyFrame @p frame_F measured
412  /// and expressed in the world frame W. @p frame_F does not necessarily need
413  /// to be owned by this RigidBodyTree. However, the RigidBody to which
414  /// @p frame_F attaches to has to be owned by this RigidBodyTree.
415  /// @param cache Reference to the KinematicsCache.
416  /// @param frame_F Reference to the RigidBodyFrame.
417  /// @retval `V_WF`
419  const KinematicsCache<T>& cache, const RigidBodyFrame<T>& frame_F) const {
420  return CalcFrameSpatialVelocityInWorldFrame(
421  cache, frame_F.get_rigid_body(),
422  frame_F.get_transform_to_body().template cast<T>());
423  }
424 
425  /// Computes the spatial velocity `V_WF` of the frame F measured and expressed
426  /// in the world frame W. Frame F is rigidly attached to @p body.
427  /// @param cache Reference to the KinematicsCache.
428  /// @param body Reference to the RigidBody.
429  /// @param X_BF The pose of frame F in body frame B.
430  /// @retval `V_WF`
431  drake::Vector6<T> CalcFrameSpatialVelocityInWorldFrame(
432  const KinematicsCache<T>& cache, const RigidBody<T>& body,
433  const drake::Isometry3<T>& X_BF) const;
434 
435  /// Computes the Jacobian `J_WF` of the spatial velocity `V_WF` of frame F
436  /// measured and expressed in the world frame W such that `V_WF = J_WF * v`,
437  /// where `v` is the generalized velocity. Frame F is rigidly attached to
438  /// @p body.
439  /// @param cache Reference to the KinematicsCache.
440  /// @param B Reference to the RigidBody.
441  /// @param X_BF The pose of frame F in body frame B.
442  /// @param in_terms_of_qdot `true` for `J_WF` computed with respect to the
443  /// time derivative of the generalized position such that
444  /// `V_WF = J_WF * qdot`. `false` for `J_WF` computed with respect to `v`.
445  /// @retval `J_WF`
446  drake::Matrix6X<T> CalcFrameSpatialVelocityJacobianInWorldFrame(
447  const KinematicsCache<T>& cache, const RigidBody<T>& body,
448  const drake::Isometry3<T>& X_BF,
449  bool in_terms_of_qdot = false) const;
450 
451  /// Computes the Jacobian `J_WF` of the spatial velocity `V_WF` of frame F
452  /// measured and expressed in the world frame W such that `V_WF = J_WF * v`,
453  /// where `v` is the generalized velocity. Frame F is rigidly attached to
454  /// @p body. This version does not allocate memory and will assert if `J_WF`
455  /// is incorrectly sized.
456  /// @param[in] cache Reference to the KinematicsCache.
457  /// @param[in] B Reference to the RigidBody.
458  /// @param[in] X_BF The pose of frame F in body frame B.
459  /// @param[in] in_terms_of_qdot `true` for `J_WF` computed with respect to the
460  /// time derivative of the generalized position such that
461  /// `V_WF = J_WF * qdot`. `false` for `J_WF` computed with respect to `v`.
462  /// @param[out] J_WF Pointer to the output Jacobian.
463  void CalcFrameSpatialVelocityJacobianInWorldFrame(
464  const KinematicsCache<T>& cache, const RigidBody<T>& body,
465  const drake::Isometry3<T>& X_BF, bool in_terms_of_qdot,
466  drake::Matrix6X<T>* J_WF) const;
467 
468  /// Computes the Jacobian `J_WF` of the spatial velocity `V_WF` of frame F
469  /// measured and expressed in the world frame W such that `V_WF = J_WF * v`,
470  /// where `v` is the generalized velocity. @p frame_F does not necessarily
471  /// need to be owned by this RigidBodyTree. However, the RigidBody to which
472  /// @p frame_F attaches to has to be owned by this RigidBodyTree.
473  /// @param cache Reference to the KinematicsCache.
474  /// @param frame_F Reference to the RigidBodyFrame.
475  /// @param in_terms_of_qdot `true` for `J_WF` computed with respect to the
476  /// time derivative of the generalized position such that
477  /// `V_WF = J_WF * qdot`. `false` for `J_WF` computed with respect to `v`.
478  /// @retval `J_WF`
480  const KinematicsCache<T>& cache, const RigidBodyFrame<T>& frame_F,
481  bool in_terms_of_qdot = false) const {
482  return CalcFrameSpatialVelocityJacobianInWorldFrame(
483  cache, frame_F.get_rigid_body(),
484  frame_F.get_transform_to_body().template cast<T>(), in_terms_of_qdot);
485  }
486 
487  /// Computes the Jacobian `J_WF` of the spatial velocity `V_WF` of frame F
488  /// measured and expressed in the world frame W such that `V_WF = J_WF * v`,
489  /// where `v` is the generalized velocity. @p frame_F does not necessarily
490  /// need to be owned by this RigidBodyTree. However, the RigidBody to which
491  /// @p frame_F attaches to has to be owned by this RigidBodyTree. This
492  /// version does not allocate memory and will assert if `J_WF` is incorrectly
493  /// sized.
494  /// @param[in] cache Reference to the KinematicsCache.
495  /// @param[in] frame_F Reference to the RigidBodyFrame.
496  /// @param[in] in_terms_of_qdot `true` for `J_WF` computed with respect to the
497  /// time derivative of the generalized position such that
498  /// `V_WF = J_WF * qdot`. `false` for `J_WF` computed with respect to `v`.
499  /// @param[out] J_WF Pointer to the output Jacobian.
501  const KinematicsCache<T>& cache, const RigidBodyFrame<T>& frame_F,
502  bool in_terms_of_qdot, drake::Matrix6X<T>* J_WF) const {
503  return CalcFrameSpatialVelocityJacobianInWorldFrame(
504  cache, frame_F.get_rigid_body(),
505  frame_F.get_transform_to_body().template cast<T>(),
506  in_terms_of_qdot, J_WF);
507  }
508 
509  /// Computes the Jacobian `J_WB` of the spatial velocity `V_WB` of body
510  /// frame B measured and expressed in the world frame `W` such that
511  /// `V_WB = J_WB * v`, where `v` is the generalized velocity.
512  /// @param cache Reference to the KinematicsCache.
513  /// @param body Reference to the RigidBody.
514  /// @param in_terms_of_qdot `true` for `J_WB` computed with respect to the
515  /// time derivative of the generalized position such that
516  /// `V_WB = J_WB * qdot`. `false` for `J_WB` computed with respect to `v`.
517  /// @retval `J_WB`
519  const KinematicsCache<T>& cache, const RigidBody<T>& body,
520  bool in_terms_of_qdot = false) const {
521  return CalcFrameSpatialVelocityJacobianInWorldFrame(
522  cache, body, drake::Isometry3<T>::Identity(), in_terms_of_qdot);
523  }
524 
525  /// Computes the Jacobian `J_WB` of the spatial velocity `V_WB` of body
526  /// frame B measured and expressed in the world frame `W` such that
527  /// `V_WB = J_WB * v`, where `v` is the generalized velocity. This version
528  /// does not allocate memory and will assert if `J_WB` is incorrectly sized.
529  /// @param[in] cache Reference to the KinematicsCache.
530  /// @param[in] body Reference to the RigidBody.
531  /// @param[in] in_terms_of_qdot `true` for `J_WB` computed with respect to the
532  /// time derivative of the generalized position such that
533  /// `V_WB = J_WB * qdot`. `false` for `J_WB` computed with respect to `v`.
534  /// @param[out] J_WB Pointer to the output Jacobian.
536  const KinematicsCache<T>& cache, const RigidBody<T>& body,
537  bool in_terms_of_qdot, drake::Matrix6X<T>* J_WB) const {
538  CalcFrameSpatialVelocityJacobianInWorldFrame(
539  cache, body, drake::Isometry3<T>::Identity(), in_terms_of_qdot, J_WB);
540  }
541 
542  /// Computes `Jdot_WF * v`, where `J_WF` is the Jacobian of spatial velocity,
543  /// `V_WF`, of frame F measured and expressed in the world frame W, and
544  /// `v` is the generalized velocity. Frame F is rigidly attached to @p body.
545  /// @param cache Reference to the KinematicsCache.
546  /// @param body Reference to the RigidBody.
547  /// @param X_BF The pose of frame F in body frame B.
548  /// @retval `Jdot_WF * v`
549  drake::Vector6<T> CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(
550  const KinematicsCache<T>& cache, const RigidBody<T>& body,
551  const drake::Isometry3<T>& X_BF) const;
552 
553  /// Computes `Jdot_WF * v`, where `J_WF` is the Jacobian of spatial velocity
554  /// `V_WF` of frame F measured and expressed in the world frame W, and
555  /// `v` is the generalized velocity. @p frame_F does not necessarily need to
556  /// be owned by this RigidBodyTree. However, the RigidBody to which @p frame_F
557  /// attaches to has to be owned by this RigidBodyTree.
558  /// @param cache Reference to the KinematicsCache.
559  /// @param frame_F Reference to the RigidBodyFrame.
560  /// @retval `Jdot_WF * v`
562  const KinematicsCache<T>& cache, const RigidBodyFrame<T>& frame_F) const {
563  return CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(
564  cache, frame_F.get_rigid_body(),
565  frame_F.get_transform_to_body().template cast<T>());
566  }
567 
568  /// Computes `Jdot_WB * v`, where `J_WB` is the Jacobian of the spatial
569  /// velocity `V_WB` of body frame B measured and expressed in the world
570  /// frame W, and `v` is the generalized velocity.
571  /// @param cache Reference to the KinematicsCache.
572  /// @param body Reference to the RigidBody.
573  /// @retval `Jdot_WB * v`
575  const KinematicsCache<T>& cache, const RigidBody<T>& body) const {
576  return CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(
577  cache, body, drake::Isometry3<T>::Identity());
578  }
579 
580  /// Converts a vector of the time derivative of generalized coordinates (qdot)
581  /// to generalized velocity (v).
582  /// @param cache the kinematics cache, which is assumed to be up-to-date with
583  /// respect to the state
584  /// @param qdot a `nq` dimensional vector, where `nq` is the dimension of the
585  /// generalized coordinates.
586  /// @returns a `nv` dimensional vector, where `nv` is the dimension of the
587  /// generalized velocities.
588  /// @sa transformVelocityToQDot()
589  template <typename Derived>
591  transformQDotToVelocity(
593  const Eigen::MatrixBase<Derived>& qdot);
594 
595  /// Converts a vector of generalized velocities (v) to the time
596  /// derivative of generalized coordinates (qdot).
597  /// @param cache the kinematics cache, which is assumed to be up-to-date with
598  /// respect to the state
599  /// @param v a `nv` dimensional vector, where `nv` is the dimension of the
600  /// generalized velocities.
601  /// @retval qdot a `nq` dimensional vector, where `nq` is the dimension of the
602  /// generalized coordinates.
603  /// @sa transformQDotToVelocity()
604  template <typename Derived>
606  transformVelocityToQDot(
608  const Eigen::MatrixBase<Derived>& v);
609 
610  /**
611  * Converts a matrix B, which transforms generalized velocities (v) to an
612  * output space X, to a matrix A, which transforms the time
613  * derivative of generalized coordinates (qdot) to the same output X. For
614  * example, B could be a Jacobian matrix that transforms generalized
615  * velocities to spatial velocities at the end-effector. Formally, this would
616  * be the matrix of partial derivatives of end-effector configuration computed
617  * with respect to quasi-coordinates (ꝗ). This function would allow
618  * transforming that Jacobian so that all partial derivatives would be
619  * computed with respect to qdot.
620  * @param Av, a `m x nv` sized matrix, where `nv` is the dimension of the
621  * generalized velocities.
622  * @retval A a `m x nq` sized matrix, where `nq` is the dimension of the
623  * generalized coordinates.
624  * @sa transformQDotMappingToVelocityMapping()
625  */
626  template <typename Derived>
628  transformVelocityMappingToQDotMapping(
630  const Eigen::MatrixBase<Derived>& Av);
631 
632  /**
633  * Converts a matrix A, which transforms the time derivative of generalized
634  * coordinates (qdot) to an output space X, to a matrix B, which transforms
635  * generalized velocities (v) to the same space X. For example, A could be a
636  * Jacobian matrix that transforms qdot to spatial velocities at the end
637  * effector. Formally, this would be the matrix of partial derivatives of
638  * end-effector configuration computed with respect to the generalized
639  * coordinates (q). This function would allow the user to
640  * transform this Jacobian matrix to the more commonly used one: the matrix of
641  * partial derivatives of end-effector configuration computed with respect to
642  * quasi-coordinates (ꝗ).
643  * @param Ap a `m x nq` sized matrix, where `nq` is the dimension of the
644  * generalized coordinates.
645  * @retval B, a `m x nv` sized matrix, where `nv` is the dimension of the
646  * generalized velocities.
647  * @sa transformVelocityMappingToQDotMapping()
648  */
649  template <typename Derived>
651  transformQDotMappingToVelocityMapping(
653  const Eigen::MatrixBase<Derived>& Ap);
654 
655  template <typename Scalar>
656  static drake::MatrixX<Scalar> GetVelocityToQDotMapping(
657  const KinematicsCache<Scalar>& cache);
658 
659  template <typename Scalar>
660  static drake::MatrixX<Scalar> GetQDotToVelocityMapping(
661  const KinematicsCache<Scalar>& cache);
662 
663  template <typename Scalar>
664  drake::TwistMatrix<Scalar> worldMomentumMatrix(
665  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
667  const std::set<int>& model_instance_id_set =
669  bool in_terms_of_qdot = false) const;
670 
671  template <typename Scalar>
672  drake::TwistVector<Scalar> worldMomentumMatrixDotTimesV(
673  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
675  const std::set<int>& model_instance_id_set =
677 
678  template <typename Scalar>
679  drake::TwistMatrix<Scalar> centroidalMomentumMatrix(
680  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
682  const std::set<int>& model_instance_id_set =
684  bool in_terms_of_qdot = false) const;
685 
686  template <typename Scalar>
687  drake::TwistVector<Scalar> centroidalMomentumMatrixDotTimesV(
688  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
690  const std::set<int>& model_instance_id_set =
692 
693  template <typename Scalar>
694  Eigen::Matrix<Scalar, drake::kSpaceDimension, Eigen::Dynamic>
695  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
696  centerOfMassJacobian(KinematicsCache<Scalar>& cache,
697  const std::set<int>& model_instance_id_set =
699  bool in_terms_of_qdot = false) const;
700 
701  template <typename Scalar>
702  Eigen::Matrix<Scalar, drake::kSpaceDimension, 1>
703  centerOfMassJacobianDotTimesV(
704  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
706  const std::set<int>& model_instance_id_set =
708 
709  template <typename DerivedA, typename DerivedB, typename DerivedC>
710  void jointLimitConstraints(
711  Eigen::MatrixBase<DerivedA> const& q,
712  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
713  Eigen::MatrixBase<DerivedB>& phi,
714  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
715  Eigen::MatrixBase<DerivedC>& J) const;
716 
717  size_t getNumJointLimitConstraints() const;
718 
719  int getNumContacts(const std::set<int>& body_idx) const; // = emptyIntSet);
720 
721  /**
722  * Computes CoP in world frame. Normal and point on contact plane should be in
723  * world frame too.
724  */
725  template <typename DerivedNormal, typename DerivedPoint>
726  std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(
727  const KinematicsCache<double>& cache,
728  const std::vector<ForceTorqueMeasurement>& force_torque_measurements,
729  const Eigen::MatrixBase<DerivedNormal>& normal,
730  const Eigen::MatrixBase<DerivedPoint>& point_on_contact_plane) const;
731 
732  /**
733  * Finds the ancestors of a body. The ancestors include the body's parent,
734  * the parent's parent, etc., all the way to the root of this RigidBodyTree,
735  * which represents the world.
736  *
737  * @param[in] body_index The index of the body in this RigidBodyTree for which
738  * the ancestors of the body are found. Ancestors are returned in a vector of
739  * body indexes.
740  *
741  * @param[out] ancestor_bodies A vector of body indexes of the ancestor bodies
742  * of the body with index @p body_index.
743  */
744  void FindAncestorBodies(int body_index,
745  std::vector<int>* ancestor_bodies) const;
746 
747  /// Identical to the above overload, expect that this function return the
748  /// ancestor bodies instead of using an output argument.
749  std::vector<int> FindAncestorBodies(int body_index) const;
750 
751  DRAKE_DEPRECATED("Please use void RigidBodyTree::FindAncestorBodies().")
752  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
753  void findAncestorBodies(std::vector<int>& ancestor_bodies, int body) const;
754 
755  /// Find the kinematic path between two bodies or frames. This function will
756  /// not allocate memory if `path`, `start_body_ancestors` and
757  /// `end_body_ancestors` are preallocated.
758  void FindKinematicPath(int start_body_or_frame_idx,
759  int end_body_or_frame_idx,
760  std::vector<int>* start_body_ancestors,
761  std::vector<int>* end_body_ancestors,
762  KinematicPath* path) const;
763 
764  KinematicPath findKinematicPath(int start_body_or_frame_idx,
765  int end_body_or_frame_idx) const;
766 
767  /** \brief Compute the positive definite mass (configuration space) matrix \f$
768  *H(q) \f$, defined by \f$T = \frac{1}{2} v^T H(q) v \f$, where \f$ T \f$ is
769  *kinetic energy.
770  *
771  * The mass matrix also appears in the manipulator equations
772  * \f[
773  * H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u
774  * \f]
775  *
776  * \param cache a KinematicsCache constructed given \f$ q \f$
777  * \return the mass matrix \f$ H(q) \f$
778  */
779  template <typename Scalar>
780  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> massMatrix(
781  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
782  KinematicsCache<Scalar>& cache) const;
783 
784  /// Convenience alias for rigid body to external wrench map, for use with
785  /// inverseDynamics and dynamicsBiasTerm.
788 
789  /** \brief Compute the term \f$ C(q, v, f_\text{ext}) \f$ in the manipulator
790  *equations
791  * \f[
792  * H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u
793  * \f]
794  *
795  * Convenience method that calls inverseDynamics with \f$ \dot{v} = 0 \f$. See
796  *inverseDynamics for argument descriptions.
797  * \see inverseDynamics
798  */
799  template <typename Scalar>
800  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> dynamicsBiasTerm(
801  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
804  RigidBody<T> const*, drake::WrenchVector<Scalar>>& external_wrenches,
805  bool include_velocity_terms = true) const;
806 
807  /** \brief Compute
808  * \f[
809  * H(q) \dot{v} + C(q, v, f_\text{ext})
810  * \f]
811  * that is, the left hand side of the manipulator equations
812  * \f[
813  * H(q) \dot{v} + C(q, v, f_\text{ext}) = B(q) u
814  * \f]
815  *
816  * Note that the 'dynamics bias term' \f$ C(q, v, f_\text{ext}) \f$ can be
817  *computed by simply setting \f$ \dot{v} = 0\f$.
818  * Note also that if only the gravitational terms contained in \f$ C(q, v,
819  *f_\text{ext}) \f$ are required, one can set \a include_velocity_terms to
820  *false.
821  * Alternatively, one can pass in a KinematicsCache created with \f$ v = 0\f$
822  *or without specifying the velocity vector.
823  *
824  * Algorithm: recursive Newton-Euler. Does not explicitly compute mass matrix.
825  * \param cache a KinematicsCache constructed given \f$ q \f$ and \f$ v \f$
826  * \param external_wrenches external wrenches exerted upon bodies
827  * (\f$ f_\text{ext} \f$). Expressed in body frame.
828  * \param vd \f$ \dot{v} \f$
829  * \param include_velocity_terms whether to include velocity-dependent terms in
830  *\f$ C(q, v, f_\text{ext}) \f$. Setting \a include_velocity_terms to false is
831  *Equivalent to setting \f$ v = 0 \f$
832  * \return \f$ H(q) \dot{v} + C(q, v, f_\text{ext}) \f$
833  */
834  template <typename Scalar>
835  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inverseDynamics(
836  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
839  RigidBody<T> const*, drake::WrenchVector<Scalar>>& external_wrenches,
840  const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& vd,
841  bool include_velocity_terms = true) const;
842 
843  template <typename DerivedV>
844  Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> frictionTorques(
845  Eigen::MatrixBase<DerivedV> const& v) const;
846 
847  template <
848  typename Scalar,
849  typename DerivedPoints> // not necessarily any relation between the two;
850  // a major use case is having an AutoDiff
851  // KinematicsCache, but double points matrix
852  Eigen::Matrix<Scalar, 3, DerivedPoints::ColsAtCompileTime>
854  const Eigen::MatrixBase<DerivedPoints>& points,
855  int from_body_or_frame_ind, int to_body_or_frame_ind) const {
856  static_assert(DerivedPoints::RowsAtCompileTime == 3 ||
857  DerivedPoints::RowsAtCompileTime == Eigen::Dynamic,
858  "points argument has wrong number of rows");
859  // Relative transformation from frame "from_body_or_frame_ind" to frame
860  // "to_body_or_frame_ind".
861  auto relative_transform =
862  relativeTransform(cache, to_body_or_frame_ind, from_body_or_frame_ind);
863  return relative_transform * points.template cast<Scalar>();
864  }
865 
866  template <typename Scalar>
867  Eigen::Matrix<Scalar, 4, 1> relativeQuaternion(
868  const KinematicsCache<Scalar>& cache, int from_body_or_frame_ind,
869  int to_body_or_frame_ind) const {
870  const drake::Matrix3<Scalar> R = relativeTransform(cache,
871  to_body_or_frame_ind, from_body_or_frame_ind).linear();
873  }
874 
875  template <typename Scalar>
876  Eigen::Matrix<Scalar, 3, 1> relativeRollPitchYaw(
877  const KinematicsCache<Scalar>& cache, int from_body_or_frame_ind,
878  int to_body_or_frame_ind) const {
880  relativeTransform(cache, to_body_or_frame_ind, from_body_or_frame_ind)
881  .linear());
882  }
883 
884  template <typename Scalar, typename DerivedPoints>
885  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> transformPointsJacobian(
886  const KinematicsCache<Scalar>& cache,
887  const Eigen::MatrixBase<DerivedPoints>& points,
888  int from_body_or_frame_ind, int to_body_or_frame_ind,
889  bool in_terms_of_qdot) const {
890  using PointScalar = typename std::conditional<
893  return DoTransformPointsJacobian<Scalar, PointScalar>(
894  cache,
895  points.template cast<PointScalar>().eval(),
896  from_body_or_frame_ind, to_body_or_frame_ind, in_terms_of_qdot);
897  }
898 
899  template <typename Scalar>
900  Eigen::Matrix<Scalar, drake::kQuaternionSize, Eigen::Dynamic>
901  relativeQuaternionJacobian(const KinematicsCache<Scalar>& cache,
902  int from_body_or_frame_ind,
903  int to_body_or_frame_ind,
904  bool in_terms_of_qdot) const;
905 
906  template <typename Scalar>
907  Eigen::Matrix<Scalar, drake::kRpySize, Eigen::Dynamic>
908  relativeRollPitchYawJacobian(const KinematicsCache<Scalar>& cache,
909  int from_body_or_frame_ind,
910  int to_body_or_frame_ind,
911  bool in_terms_of_qdot) const;
912 
913  template <typename Scalar>
914  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
915  forwardKinPositionGradient(const KinematicsCache<Scalar>& cache, int npoints,
916  int from_body_or_frame_ind,
917  int to_body_or_frame_ind) const;
918 
919  template <typename Scalar, typename DerivedPoints>
920  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> transformPointsJacobianDotTimesV(
921  const KinematicsCache<Scalar>& cache,
922  const Eigen::MatrixBase<DerivedPoints>& points,
923  int from_body_or_frame_ind, int to_body_or_frame_ind) const {
924  return DoTransformPointsJacobianDotTimesV<Scalar>(
925  cache, points, from_body_or_frame_ind, to_body_or_frame_ind);
926  }
927 
928  template <typename Scalar>
929  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> relativeQuaternionJacobianDotTimesV(
930  const KinematicsCache<Scalar>& cache, int from_body_or_frame_ind,
931  int to_body_or_frame_ind) const;
932 
933  template <typename Scalar>
934  Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
935  relativeRollPitchYawJacobianDotTimesV(const KinematicsCache<Scalar>& cache,
936  int from_body_or_frame_ind,
937  int to_body_or_frame_ind) const;
938 
939  template <typename Scalar>
940  drake::TwistMatrix<Scalar> geometricJacobian(
941  const KinematicsCache<Scalar>& cache, int base_body_or_frame_ind,
942  int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind,
943  bool in_terms_of_qdot = false,
944  std::vector<int>* v_indices = nullptr) const;
945 
946  private:
947  template <typename Scalar>
948  void GeometricJacobian(
949  const KinematicsCache<Scalar>& cache, int base_body_or_frame_ind,
950  int end_effector_body_or_frame_ind, int expressed_in_body_or_frame_ind,
951  bool in_terms_of_qdot, std::vector<int>* v_indices,
952  drake::Matrix6X<Scalar>* J) const;
953 
954  public:
955  template <typename Scalar>
956  drake::TwistVector<Scalar> geometricJacobianDotTimesV(
957  const KinematicsCache<Scalar>& cache, int base_body_or_frame_ind,
958  int end_effector_body_or_frame_ind,
959  int expressed_in_body_or_frame_ind) const;
960 
961  template <typename Scalar>
962  drake::TwistVector<Scalar> relativeTwist(
963  const KinematicsCache<Scalar>& cache, int base_or_frame_ind,
964  int body_or_frame_ind, int expressed_in_body_or_frame_ind) const;
965 
966  template <typename Scalar>
967  drake::TwistVector<Scalar> transformSpatialAcceleration(
968  const KinematicsCache<Scalar>& cache,
969  const drake::TwistVector<Scalar>& spatial_acceleration,
970  int base_or_frame_ind, int body_or_frame_ind, int old_body_or_frame_ind,
971  int new_body_or_frame_ind) const;
972 
973  template <typename Scalar>
974  Eigen::Transform<Scalar, drake::kSpaceDimension, Eigen::Isometry>
975  relativeTransform(const KinematicsCache<Scalar>& cache, int base_or_frame_ind,
976  int body_or_frame_ind) const;
977 
978  /**
979  * Computes the Jacobian for many points in the format currently used by
980  * MATLAB. (possibly should be scheduled for deletion, taking
981  * accumulateContactJacobians() with it)
982  */
983  template <typename Scalar>
984  void computeContactJacobians(
985  const KinematicsCache<Scalar>& cache,
986  Eigen::Ref<const Eigen::VectorXi> const& idxA,
987  Eigen::Ref<const Eigen::VectorXi> const& idxB,
988  Eigen::Ref<const Eigen::Matrix3Xd> const& xA,
989  Eigen::Ref<const Eigen::Matrix3Xd> const& xB,
990  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
991  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& J) const;
992 
993  /**
994  * Adds a new collision element to the tree. The input @p element will be
995  * copied and that copy will be stored in the tree, associated with the
996  * given @p body. This association is pending. It is necessary to call
997  * compile() in order for the element to be fully integrated into the
998  * RigidBodyTree.
999  * @param element the element to add.
1000  * @param body the body to associate the element with.
1001  * @param group_name a group name to tag the associated element with.
1002  */
1003  void addCollisionElement(
1004  const drake::multibody::collision::Element& element,
1005  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1006  RigidBody<T>& body, const std::string& group_name);
1007 
1008  /// Retrieve a `const` pointer to an element of the collision model.
1009  /// Note: The use of Find (instead of get) and the use of CamelCase both
1010  /// imply a potential runtime cost are carried over from the collision model
1011  /// accessor method.
1013  const drake::multibody::collision::ElementId& id) const {
1014  return collision_model_->FindElement(id);
1015  }
1016 
1017  template <class UnaryPredicate>
1018  void removeCollisionGroupsIf(UnaryPredicate test) {
1019  for (const auto& body_ptr : bodies_) {
1020  std::vector<std::string> names_of_groups_to_delete;
1021  for (const auto& group : body_ptr->get_group_to_collision_ids_map()) {
1022  const std::string& group_name = group.first;
1023  if (test(group_name)) {
1024  auto& ids = body_ptr->get_mutable_collision_element_ids();
1025  for (const auto& id : group.second) {
1026  ids.erase(std::find(ids.begin(), ids.end(), id));
1027  collision_model_->RemoveElement(id);
1028  }
1029  names_of_groups_to_delete.push_back(group_name);
1030  }
1031  }
1032  for (const auto& group_name : names_of_groups_to_delete) {
1033  body_ptr->get_mutable_group_to_collision_ids_map().erase(group_name);
1034  }
1035  }
1036  }
1037 
1038  /**
1039  * Updates the collision elements registered with the collision detection
1040  * engine. Note: If U is not a double then the transforms from kinematics
1041  * cache will be forcefully cast to doubles (discarding any gradient
1042  * information). Callers that set @p throw_if_missing_gradient to
1043  * `false` are responsible for ensuring that future code is secure despite all
1044  * gradients with respect to the collision engine being arbitrarily set to
1045  * zero.
1046  * @see ComputeMaximumDepthCollisionPoints for an example.
1047  *
1048  * @throws std::runtime_error based on the criteria of DiscardZeroGradient()
1049  * only if @p throws_if_missing_gradient is true.
1050  */
1051  template <typename U>
1052  void updateCollisionElements(
1053  const RigidBody<T>& body,
1054  const Eigen::Transform<U, 3, Eigen::Isometry>& transform_to_world,
1055  bool throw_if_missing_gradient = true);
1056 
1057  /**
1058  * @see updateCollisionElements
1059  * @throws std::runtime_error based on the criteria of DiscardZeroGradient()
1060  * only if @p throws_if_missing_gradient is true.
1061  */
1062  template <typename U>
1063  void updateDynamicCollisionElements(const KinematicsCache<U>& kin_cache,
1064  bool throw_if_missing_gradient = true);
1065 
1066  /**
1067  * Gets the contact points defined by a body's collision elements.
1068  *
1069  * @param[in] body The body who's collision elements are searched.
1070  *
1071  * @param[out] terrain_points Contact points are added to this matrix.
1072  *
1073  * @param[in] group_name If a group name was given, use it to look up the
1074  * subset of collision elements that belong to that collision group.
1075  * Otherwise, uses the full set of collision elements that belong to the body.
1076  *
1077  * @throws std::runtime_error if an invalid group name is given.
1078  */
1079  void getTerrainContactPoints(const RigidBody<T>& body,
1080  Eigen::Matrix3Xd* terrain_points,
1081  const std::string& group_name = "") const;
1082 
1083  bool collisionRaycast(const KinematicsCache<double>& cache,
1084  const Eigen::Matrix3Xd& origins,
1085  const Eigen::Matrix3Xd& ray_endpoints,
1086  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1087  Eigen::VectorXd& distances,
1088  bool use_margins = false);
1089  bool collisionRaycast(const KinematicsCache<double>& cache,
1090  const Eigen::Matrix3Xd& origins,
1091  const Eigen::Matrix3Xd& ray_endpoints,
1092  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1093  Eigen::VectorXd& distances, Eigen::Matrix3Xd& normals,
1094  bool use_margins = false);
1095 
1096  /**
1097  * Computes the *signed* distance from the given points to the nearest body
1098  * in the RigidBodyTree.
1099  *
1100  * @param[in] cache a KinematicsCache constructed by
1101  * RigidBodyTree::doKinematics given `q` and `v`.
1102  * @param[in] points A 3xN matrix of points, in world frame, to which signed
1103  * distance will be computed.
1104  * @param[out] phi Resized to N elements and filled with the computed signed
1105  * distances, or inf if no closest point was found.
1106  * @param[out] normal Resized to 3xN elements and filled with collision
1107  * element normals in world frame, at the closest point on the collision
1108  * geometry to each point in `points`. Undefined where no closest point was
1109  * found.
1110  * @param[out] x Resized to 3xN elements and filled with the closest points
1111  * on the collision geometry to each point in `points`, in world frame.
1112  * Undefined where no closest point was found.
1113  * @param[out] body_x Resized to 3xN elements and filled with the closest
1114  * points on the collision geometry to each point in `points`, in the body
1115  * frame of the closest body. Undefined where no closest point was found.
1116  * @param[out] body_idx Resized to N elements and filled with the body idx
1117  * of the closest body to each point in `points`, or -1 where no closest
1118  * body was found.
1119  * @param[in] use_margins Whether to pad each collision body with a narrow
1120  * (see bullet_model) margin to improve stability of normal estimation at
1121  * the cost of the accuracy of closest points calculations.
1122  */
1123  void collisionDetectFromPoints(
1124  const KinematicsCache<double>& cache,
1125  const Eigen::Matrix3Xd& points,
1126  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1127  Eigen::VectorXd& phi, Eigen::Matrix3Xd& normal,
1128  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1129  Eigen::Matrix3Xd& x, Eigen::Matrix3Xd& body_x,
1130  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1131  std::vector<int>& body_idx, bool use_margins);
1132 
1133  bool collisionDetect(
1134  const KinematicsCache<double>& cache,
1135  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1136  Eigen::VectorXd& phi,
1137  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1138  Eigen::Matrix3Xd& normal,
1139  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1140  Eigen::Matrix3Xd& xA,
1141  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1142  Eigen::Matrix3Xd& xB,
1143  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1144  std::vector<int>& bodyA_idx,
1145  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1146  std::vector<int>& bodyB_idx,
1147  const std::vector<drake::multibody::collision::ElementId>& ids_to_check,
1148  bool use_margins);
1149 
1150  bool collisionDetect(
1151  const KinematicsCache<double>& cache,
1152  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1153  Eigen::VectorXd& phi,
1154  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1155  Eigen::Matrix3Xd& normal,
1156  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1157  Eigen::Matrix3Xd& xA,
1158  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1159  Eigen::Matrix3Xd& xB,
1160  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1161  std::vector<int>& bodyA_idx,
1162  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1163  std::vector<int>& bodyB_idx,
1164  const std::vector<int>& bodies_idx,
1165  const std::set<std::string>& active_element_groups,
1166  bool use_margins = true);
1167 
1168  bool collisionDetect(
1169  const KinematicsCache<double>& cache,
1170  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1171  Eigen::VectorXd& phi,
1172  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1173  Eigen::Matrix3Xd& normal,
1174  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1175  Eigen::Matrix3Xd& xA,
1176  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1177  Eigen::Matrix3Xd& xB,
1178  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1179  std::vector<int>& bodyA_idx,
1180  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1181  std::vector<int>& bodyB_idx,
1182  const std::vector<int>& bodies_idx,
1183  bool use_margins = true);
1184 
1185  bool collisionDetect(
1186  const KinematicsCache<double>& cache,
1187  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1188  Eigen::VectorXd& phi,
1189  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1190  Eigen::Matrix3Xd& normal,
1191  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1192  Eigen::Matrix3Xd& xA,
1193  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1194  Eigen::Matrix3Xd& xB,
1195  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1196  std::vector<int>& bodyA_idx,
1197  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1198  std::vector<int>& bodyB_idx,
1199  const std::set<std::string>& active_element_groups,
1200  bool use_margins = true);
1201 
1202  bool collisionDetect(
1203  const KinematicsCache<double>& cache,
1204  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1205  Eigen::VectorXd& phi,
1206  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1207  Eigen::Matrix3Xd& normal,
1208  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1209  Eigen::Matrix3Xd& xA,
1210  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1211  Eigen::Matrix3Xd& xB,
1212  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1213  std::vector<int>& bodyA_idx,
1214  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1215  std::vector<int>& bodyB_idx,
1216  bool use_margins = true);
1217 
1218  bool allCollisions(
1219  const KinematicsCache<double>& cache,
1220  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1221  std::vector<int>& bodyA_idx,
1222  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1223  std::vector<int>& bodyB_idx,
1224  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1225  Eigen::Matrix3Xd& ptsA,
1226  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1227  Eigen::Matrix3Xd& ptsB,
1228  bool use_margins = true);
1229 
1230  /** Computes the point of closest approach between bodies in the
1231  RigidBodyTree that are in contact.
1232 
1233  @param cache[in] a KinematicsCache constructed by RigidBodyTree::doKinematics
1234  given `q` and `v`.
1235 
1236  Collision points are returned as a vector of PointPair's.
1237  See the documentation for PointPair for details. The collision point on the
1238  surface of each body is stored in the PointPair structure in the frame of the
1239  corresponding body.
1240 
1241  @param[in] use_margins If `true` the model uses the representation with
1242  margins. If `false`, the representation without margins is used instead.
1243 
1244  @throws std::runtime_error based on the criteria of DiscardZeroGradient()
1245  only if @p throws_if_missing_gradient is true.
1246  **/
1247  template <typename U>
1248  std::vector<drake::multibody::collision::PointPair<U>>
1249  ComputeMaximumDepthCollisionPoints(const KinematicsCache<U>& cache,
1250  bool use_margins = true, bool
1251  throw_if_missing_gradient = true);
1252 
1253  virtual bool collidingPointsCheckOnly(
1254  const KinematicsCache<double>& cache,
1255  const std::vector<Eigen::Vector3d>& points, double collision_threshold);
1256 
1257  virtual std::vector<size_t> collidingPoints(
1258  const KinematicsCache<double>& cache,
1259  const std::vector<Eigen::Vector3d>& points, double collision_threshold);
1260 
1261  /**
1262  * Finds a body with the specified \p body_name belonging to a model
1263  * with the specified \p model_name and \p model_id. Note that if
1264  * \p model_name is the empty string and \p model_id is -1, every model is
1265  * searched. If \p model_name and \p model_id are inconsistent, no body
1266  * will be found and an exception will be thrown.
1267  *
1268  * @param[in] body_name The name of the body to find.
1269  * @param[in] model_name The name of the model to which the body belongs. If
1270  * this value is an empty string, every model is searched.
1271  * @param[in] model_id The ID of the model to which the body belongs. If this
1272  * value is -1, every model is searched.
1273  * @throws std::logic_error if multiple matching bodies are found or no
1274  * matching bodies are found.
1275  */
1276  RigidBody<T>* FindBody(const std::string& body_name,
1277  const std::string& model_name = "",
1278  int model_id = -1) const;
1279 
1280  /**
1281  * Reports the RigidBody that owns the collision element indicated by the id.
1282  * @param element_id The id to query.
1283  * @return A pointer to the owning RigidBody.
1284  * @throws std::logic_error if no body can be mapped to the element id.
1285  */
1286  const RigidBody<double>* FindBody(
1287  drake::multibody::collision::ElementId element_id) const;
1288 
1289  /**
1290  * Returns a vector of pointers to all rigid bodies in this tree that belong
1291  * to a particular model instance.
1292  *
1293  * @param[in] model_instance_id The ID of the model instance whose rigid
1294  * bodies are being searched for.
1295  *
1296  * @return A vector of pointers to every rigid body belonging to the specified
1297  * model instance.
1298  */
1299  std::vector<const RigidBody<T>*>
1300  FindModelInstanceBodies(int model_instance_id) const;
1301 
1302 /**
1303  * This is a deprecated version of `FindBody(...)`. Please use `FindBody(...)`
1304  * instead.
1305  */
1306  DRAKE_DEPRECATED("Please use RigidBodyTree::FindBody().")
1307  RigidBody<T>* findLink(const std::string& link_name,
1308  const std::string& model_name = "",
1309  int model_id = -1) const;
1310 
1311  /**
1312  * Obtains a vector of indexes of the bodies that are directly attached to the
1313  * world via any type of joint. This method has a time complexity of `O(N)`
1314  * where `N` is the number of bodies in the tree, which can be determined by
1315  * calling RigidBodyTree::get_num_bodies().
1316  */
1317  std::vector<int> FindBaseBodies(int model_instance_id = -1) const;
1318 
1319  /**
1320  * Obtains the index of a rigid body within this rigid body tree. The rigid
1321  * body tree maintains a vector of pointers to all rigid bodies that are part
1322  * of the rigid body tree. The index of a rigid body is the index within this
1323  * vector at which a pointer to the rigid body is stored.
1324  *
1325  * @param[in] body_name The body whose index we want to find. It should
1326  * be unique within the searched models, otherwise an exception will be
1327  * thrown.
1328  *
1329  * @param[in] model_instance_id The ID of the model instance. This parameter
1330  * is optional. If supplied, only the model instance with the specified
1331  * instance ID is searched; otherwise, all model instances are searched.
1332  *
1333  * @return The index of the specified rigid body.
1334  *
1335  * @throws std::logic_error if no rigid body with the specified \p body_name
1336  * and \p model_id was found or if multiple matching rigid bodies were found.
1337  */
1338  int FindBodyIndex(const std::string& body_name,
1339  int model_instance_id = -1) const;
1340 
1341  /**
1342  * Returns a vector of indexes of bodies that are the children of the body at
1343  * index @p parent_body_index. The resulting list can be further filtered to
1344  * be bodies that belong to model instance ID @p model_instance_id. This
1345  * method has a time complexity of `O(N)` where `N` is the number of bodies
1346  * in the tree, which can be determined by calling
1347  * RigidBodyTree::get_num_bodies().
1348  */
1349  std::vector<int> FindChildrenOfBody(int parent_body_index,
1350  int model_instance_id = -1) const;
1351 
1352 /**
1353  * This is a deprecated version of `FindBodyIndex(...)`. Please use
1354  * `FindBodyIndex(...)` instead.
1355  */
1356  DRAKE_DEPRECATED("Please use RigidBodyTree::FindBodyIndex().")
1357  int findLinkId(const std::string& link_name, int model_id = -1) const;
1358 
1359  /**
1360  * Obtains a pointer to the rigid body whose parent joint is named
1361  * @p joint_name and is part of a model instance with ID @p model_instance_id.
1362  *
1363  * @param[in] joint_name The name of the parent joint of the rigid body to
1364  * find.
1365  *
1366  * @param[in] model_instance_id The ID of the model instance that owns the
1367  * rigid body to find. This parameter is optional. If supplied, the set of
1368  * rigid bodies to search through is restricted to those that belong to the
1369  * specified model instance. Otherwise, all rigid bodies in this tree are
1370  * searched.
1371  *
1372  * @return A pointer to the rigid body whose parent joint is named
1373  * @p joint_name joint and, if @p model_instance_id is specified, is part of
1374  * the specified model instance.
1375  *
1376  * @throws std::runtime_error If either no rigid body is found or multiple
1377  * matching rigid bodies are found.
1378  */
1379  RigidBody<T>* FindChildBodyOfJoint(const std::string& joint_name,
1380  int model_instance_id = -1) const;
1381 
1382  DRAKE_DEPRECATED("Please use FindChildBodyOfJoint().")
1383  RigidBody<T>* findJoint(
1384  const std::string& joint_name, int model_id = -1) const;
1385 
1386  /**
1387  * Returns the index within the vector of rigid bodies of the rigid body whose
1388  * parent joint is named @p joint_name and is part of a model instance with ID
1389  * @p model_instance_id.
1390  *
1391  * @param[in] joint_name The name of the parent joint of the rigid body whose
1392  * index is being searched for.
1393  *
1394  * @param[in] model_instance_id The ID of the model instance that owns the
1395  * rigid body to find. This parameter is optional. If supplied, the set of
1396  * rigid bodies to search through is restricted to those that belong to the
1397  * specified model instance. Otherwise, all rigid bodies in this tree are
1398  * searched.
1399  *
1400  * @return The index of the rigid body whose parent joint is named
1401  * @p joint_name and, if @p model_instance_id is specified, is part of
1402  * the specified model instance.
1403  *
1404  * @throws std::runtime_error If either no rigid body is found or multiple
1405  * matching rigid bodies are found.
1406  */
1407  int FindIndexOfChildBodyOfJoint(const std::string& joint_name,
1408  int model_instance_id = -1) const;
1409 
1410  DRAKE_DEPRECATED("Please use FindIndexOfChildBodyOfJoint().")
1411  int findJointId(const std::string& joint_name, int model_id = -1) const;
1412 
1413  /**
1414  * Finds a frame of the specified \p frame_name belonging to a model with the
1415  * specified \p model_id.
1416  *
1417  * @param[in] frame_name The name of the frame to find.
1418  *
1419  * @param[in] model_id The ID of the model to which the frame belongs. If this
1420  * value is -1, search all models.
1421  *
1422  * @return The frame with the specified name and model instance ID.
1423  *
1424  * @throws std::logic_error if either multiple matching frames are found or no
1425  * matching frame is found.
1426  */
1427  std::shared_ptr<RigidBodyFrame<T>> findFrame(const std::string& frame_name,
1428  int model_id = -1) const;
1429 
1430  /**
1431  * Returns the body at index @p body_index. Parameter @p body_index must be
1432  * between zero and the number of bodies in this tree, which can be determined
1433  * by calling RigidBodyTree::get_num_bodies().
1434  */
1435  const RigidBody<T>& get_body(int body_index) const {
1436  DRAKE_DEMAND(body_index >= 0 && body_index < get_num_bodies());
1437  return *bodies_[body_index].get();
1438  }
1439 
1440  /**
1441  * Returns the body at index @p body_index. Parameter @p body_index must be
1442  * between zero and the number of bodies in this tree, which can be determined
1443  * by calling RigidBodyTree::get_num_bodies().
1444  */
1445  RigidBody<T>* get_mutable_body(int body_index);
1446 
1447  /**
1448  * Returns the number of bodies in this tree. This includes the one body that
1449  * represents the world.
1450  */
1451  int get_num_bodies() const {
1452  return static_cast<int>(bodies_.size());
1453  }
1454 
1455  /**
1456  * Returns the number of frames in this tree.
1457  */
1458  int get_num_frames() const;
1459 
1460  DRAKE_DEPRECATED("Please use get_num_bodies().")
1461  int get_number_of_bodies() const;
1462 
1463  std::string getBodyOrFrameName(int body_or_frame_id) const;
1464  // @param body_or_frame_id the index of the body or the id of the frame.
1465 
1466  /**
1467  * Obtains a rigid body actuator from this rigid body tree. The actuator is
1468  * selected based on its name.
1469  *
1470  * @param name The name of the rigid body actuator to get.
1471  * @returns A const reference to the rigid body actuator with name @p name.
1472  * @throws std::invalid_argument if no rigid body actuator with name @p name
1473  * exists.
1474  */
1475  const RigidBodyActuator& GetActuator(const std::string& name) const;
1476 
1477  // TODO(tkoolen): remove parseBodyOrFrameID methods
1478  template <typename Scalar>
1479  int parseBodyOrFrameID(
1480  const int body_or_frame_id,
1481  Eigen::Transform<Scalar, 3, Eigen::Isometry>* Tframe) const;
1482  int parseBodyOrFrameID(const int body_or_frame_id) const;
1483 
1484  /// For details on parameters see RigidBodyDistanceContraint.
1485  void addDistanceConstraint(int bodyA_index_in, const Eigen::Vector3d& r_AP_in,
1486  int bodyB_index_in, const Eigen::Vector3d& r_BQ_in,
1487  double distance_in);
1488 
1489  template <typename Scalar>
1490  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> positionConstraints(
1491  const KinematicsCache<Scalar>& cache) const;
1492 
1493  template <typename Scalar>
1494  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
1495  positionConstraintsJacobian(const KinematicsCache<Scalar>& cache,
1496  bool in_terms_of_qdot = true) const;
1497 
1498  template <typename Scalar>
1499  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> positionConstraintsJacDotTimesV(
1500  const KinematicsCache<Scalar>& cache) const;
1501 
1502  size_t getNumPositionConstraints() const;
1503 
1504  template <typename Derived>
1505  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime,
1506  Eigen::Dynamic>
1507  compactToFull(const Eigen::MatrixBase<Derived>& compact,
1508  const std::vector<int>& joint_path,
1509  bool in_terms_of_qdot) const {
1510  /*
1511  * This method is used after calling geometric Jacobian, where compact is
1512  * the Jacobian on the joints that are on the kinematic path; if we want to
1513  * reconstruct the full Jacobian on all joints, then we should call this
1514  * method.
1515  */
1516  int ncols = in_terms_of_qdot ? num_positions_ : num_velocities_;
1517  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime,
1518  Eigen::Dynamic>
1519  full(compact.rows(), ncols);
1520  full.setZero();
1521  int compact_col_start = 0;
1522  for (std::vector<int>::const_iterator it = joint_path.begin();
1523  it != joint_path.end(); ++it) {
1524  RigidBody<T>& body = *bodies_[*it];
1525  int ncols_joint = in_terms_of_qdot ? body.getJoint().get_num_positions()
1526  : body.getJoint().get_num_velocities();
1527  int col_start = in_terms_of_qdot ? body.get_position_start_index()
1528  : body.get_velocity_start_index();
1529  full.middleCols(col_start, ncols_joint) =
1530  compact.middleCols(compact_col_start, ncols_joint);
1531  compact_col_start += ncols_joint;
1532  }
1533  return full;
1534  }
1535 
1536  /**
1537  * A toString method for this class.
1538  */
1539  friend std::ostream& operator<<(std::ostream&, const RigidBodyTree<double>&);
1540 
1541  /**
1542  * @brief Adds and takes ownership of a rigid body. This also adds a frame
1543  * whose pose is the same as the body's.
1544  *
1545  * A RigidBodyTree is the sole owner and manager of the RigidBody's in it.
1546  * A body is assigned a unique id (RigidBody::id()) when added to a
1547  * RigidBodyTree. This unique id can be use to access a body using
1548  * RigidBodyTree::get_bodies()[id].
1549  *
1550  * @param[in] body The rigid body to add to this rigid body tree.
1551  * @return A bare, unowned pointer to the @p body.
1552  * @pre Neither a body nor frame with the same identifying information (name
1553  * and model id / name) should already exist in the tree.
1554  * @throws std::runtime_error if preconditions are not met.
1555  */
1556  // TODO(eric.cousineau): Rename to `AddRigidBody`.
1557  RigidBody<T>* add_rigid_body(std::unique_ptr<RigidBody<T>> body);
1558 
1559  /**
1560  * Attempts to define a new collision filter group. The given name *must*
1561  * be unique since the last invocation of compile() (or construction,
1562  * whichever is more recent). Duplicate names or attempting to add more
1563  * collision filter groups than the system can handle will lead to failure. In
1564  * the event of failure, an exception is thrown. kMaxNumCollisionFilterGroups
1565  * defines the limit of total collision filter groups that are supported.
1566  * @param name The unique name of the new group.
1567  * @trhows std::logic_error in response to failure conditions.
1568  */
1569  void DefineCollisionFilterGroup(const std::string& name);
1570 
1571  /**
1572  * Adds a RigidBody to a collision filter group. The RigidBody is referenced
1573  * by name and model instance id. The process will fail if the body cannot be
1574  * found or if the group cannot be found.
1575  * @param group_name The collision filter group name to add the body to.
1576  * @param body_name The name of the body to add.
1577  * @param model_id The id of the model instance to which this body
1578  * belongs.
1579  * @throws std::logic_error in response to failure conditions.
1580  */
1581  void AddCollisionFilterGroupMember(const std::string& group_name,
1582  const std::string& body_name,
1583  int model_id);
1584 
1585  /**
1586  * Adds a collision group to the set of groups ignored by the specified
1587  * collision filter group. Will fail if the specified group name does not
1588  * refer to an existing collision filter group. (The target group name need
1589  * not exist at this time.)
1590  * @param group_name
1591  * @param target_group_name
1592  * @throws std::logic_error in response to failure conditions.
1593  */
1594  void AddCollisionFilterIgnoreTarget(const std::string& group_name,
1595  const std::string& target_group_name);
1596 
1597  /**
1598  * @brief Returns a mutable reference to the RigidBody associated with the
1599  * world in the model. This is the root of the RigidBodyTree.
1600  */
1601  RigidBody<T>& world() { return *bodies_[0]; }
1602 
1603  /**
1604  * @brief Returns a const reference to the RigidBody associated with the
1605  * world in the model. This is the root of the RigidBodyTree.
1606  */
1607  const RigidBody<T>& world() const { return *bodies_[0]; }
1608 
1609  /**
1610  * Returns the number of position states outputted by this %RigidBodyTree.
1611  */
1612  int get_num_positions() const { return num_positions_; }
1613 
1614  DRAKE_DEPRECATED("Please use get_num_positions().")
1615  int number_of_positions() const;
1616 
1617  /**
1618  * Returns the number of velocity states outputted by this %RigidBodyTree.
1619  */
1620  int get_num_velocities() const { return num_velocities_; }
1621 
1622  DRAKE_DEPRECATED("Please use get_num_velocities().")
1623  int number_of_velocities() const;
1624 
1625  /**
1626  * Returns the number of actuators in this %RigidBodyTree.
1627  */
1628  int get_num_actuators() const { return static_cast<int>(actuators.size()); }
1629 
1630  /**
1631  * Returns whether this %RigidBodyTree is initialized. It is initialized after
1632  * compile() is called.
1633  */
1634  bool initialized() const { return initialized_; }
1635 
1636  public:
1637  Eigen::VectorXd joint_limit_min;
1638  Eigen::VectorXd joint_limit_max;
1639 
1640  private:
1641  // Rigid body objects
1642  std::vector<std::unique_ptr<RigidBody<T>>> bodies_;
1643 
1644  // Rigid body frames
1645  std::vector<std::shared_ptr<RigidBodyFrame<T>>> frames_;
1646 
1647  public:
1649  "Direct access to `bodies` has been deprecated, "
1650  "mutable access has been removed. Use `get_bodies` and `add_rigid_body` "
1651  "instead")
1652  const std::vector<std::unique_ptr<RigidBody<T>>>& bodies{bodies_};
1653 
1654  /// List of bodies.
1655  // TODO(amcastro-tri): start using accessors body(int).
1656  auto& get_bodies() const { return bodies_; }
1657 
1659  "Direct access to `frames` has been deprecated, "
1660  "mutable access has been removed. Use `get_frames` and `addFrame` "
1661  "instead")
1662  const std::vector<std::shared_ptr<RigidBodyFrame<T>>>& frames{frames_};
1663 
1664  /// List of frames.
1665  auto& get_frames() const { return frames_; }
1666 
1667  // Rigid body actuators
1668  std::vector<RigidBodyActuator, Eigen::aligned_allocator<RigidBodyActuator>>
1670 
1671  // Rigid body loops
1672  std::vector<RigidBodyLoop<T>,
1673  Eigen::aligned_allocator<RigidBodyLoop<T>>> loops;
1674 
1675  // Rigid body distance constraints
1677  Eigen::aligned_allocator<RigidBodyDistanceConstraint>>
1679 
1681  Eigen::MatrixXd B; // the B matrix maps inputs into joint-space forces
1682 
1683  private:
1684  // The number of generalized position states in this rigid body tree.
1685  int num_positions_{};
1686 
1687  // The number of generalized velocity states in this rigid body tree.
1688  int num_velocities_{};
1689 
1690  // The number of model instances in this rigid body tree.
1691  int num_model_instances_{};
1692 
1693  // Helper functions for contactConstraints.
1694  template <typename Scalar>
1695  void accumulateContactJacobian(
1696  const KinematicsCache<Scalar>& cache, const int bodyInd,
1697  Eigen::Matrix3Xd const& bodyPoints, std::vector<size_t> const& cindA,
1698  std::vector<size_t> const& cindB,
1699  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1700  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>& J) const;
1701 
1702  template <typename Scalar>
1703  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
1704  void updateCompositeRigidBodyInertias(KinematicsCache<Scalar>& cache) const;
1705 
1706  // Examines the state of the tree, and confirms that all nodes (i.e,
1707  // RigidBody instances) have a kinematics path to the root. In other words,
1708  // there should only be a single body that has no parent: the world.
1709  // Throws an exception if it is *not* a complete tree.
1710  void ConfirmCompleteTree() const;
1711 
1712  // Given the body, tests to see if it has a kinematic path to the world node.
1713  // Uses a cache of known "connected" bodies to accelerate the computation.
1714  // The connected set consist of the body indices (see
1715  // RigidBody::get_body_index) which are known to be connected to the world.
1716  // This function has a side-effect of updating the set of known connected.
1717  // This assumes that the connected set has been initialized with the value
1718  // 0 (the world body).
1719  // If not connected, throws an exception.
1720  void TestConnectedToWorld(const RigidBody<T>& body,
1721  std::set<int>* connected) const;
1722 
1723  // Reorder body list to ensure parents are before children in the list
1724  // of bodies.
1725  //
1726  // See RigidBodyTree::compile
1727  void SortTree();
1728 
1729  // Performs the work that is required for the collision state of the
1730  // RigidBodyTree to become finalized.
1731  void CompileCollisionState();
1732 
1733  // Defines a number of collision cliques to be used by
1734  // drake::multibody::collision::Model.
1735  // Collision cliques are defined so that:
1736  // - Collision elements on a single body are *not* considered for collision.
1737  // - Collision elements on *pairs* of RigidBody instances are configured not
1738  // to be considered for collision if RigidBody::CanCollideWith() returns
1739  // false.
1740  //
1741  // @see RigidBody::CanCollideWith.
1742  void CreateCollisionCliques(std::unordered_set<RigidBody<T>*>* recompile_set);
1743 
1744  // collision_model maintains a collection of the collision geometry in the
1745  // RBM for use in collision detection of different kinds. Small margins are
1746  // applied to all collision geometry when that geometry is added, to improve
1747  // the numerical stability of contact gradients taken using the model.
1748  std::unique_ptr<drake::multibody::collision::Model> collision_model_;
1749 
1750  public:
1751  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1752 
1753  private:
1754  RigidBodyTree(const RigidBodyTree&);
1755  RigidBodyTree& operator=(const RigidBodyTree&) { return *this; }
1756 
1757  // The positional arguments identically match the public non-Do variant above.
1758  template <typename Scalar, typename PointScalar>
1759  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
1760  DoTransformPointsJacobian(
1761  const KinematicsCache<Scalar>&,
1762  const Eigen::Ref<const drake::Matrix3X<PointScalar>>&,
1763  int, int, bool) const;
1764 
1765  // The positional arguments identically match the public non-Do variant above.
1766  template <typename Scalar>
1767  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> DoTransformPointsJacobianDotTimesV(
1768  const KinematicsCache<Scalar>&, const Eigen::Ref<const Eigen::Matrix3Xd>&,
1769  int, int) const;
1770 
1771  // TODO(SeanCurtis-TRI): This isn't properly used.
1772  // No query operations should work if it hasn't been initialized. Calling
1773  // compile() is the only thing that should set this. Furthermore, any
1774  // operation that changes the tree (e.g., adding a body, collision element,
1775  // etc.) should clear the bit again, requiring another call to compile().
1776  bool initialized_{false};
1777 
1778  int next_available_clique_ = 0;
1779 
1780  private:
1781  // A utility class for storing body collision data during RBT instantiation.
1782  struct BodyCollisionItem {
1783  BodyCollisionItem(const std::string& grp_name, size_t element_index)
1784  : group_name(grp_name), element(element_index) {
1785  }
1786  std::string group_name;
1787  size_t element;
1788  };
1789 
1790  typedef std::vector<BodyCollisionItem> BodyCollisions;
1791  // This data structure supports an orderly instantiation of the collision
1792  // elements. It is populated during tree construction, exercised during
1793  // RigidBodyTree::compile() at the conclusion of which it is emptied.
1794  // It has no run-time value. This is a hacky alternative to having a
1795  // proper Intermediate Representation (IR).
1796  std::unordered_map<RigidBody<T>*, BodyCollisions> body_collision_map_;
1797 
1798  // Bullet's collision results are affected by the order in which the collision
1799  // elements are added. This queues the collision elements in the added order
1800  // so that when they are registered with the collision engine, they'll be
1801  // submitted in the invocation order.
1802  //
1803  // For more information, see:
1804  // https://github.com/bulletphysics/bullet3/issues/888
1805  std::vector<std::unique_ptr<drake::multibody::collision::Element>>
1806  element_order_;
1807 
1808  // A manager for instantiating and managing collision filter groups.
1810  collision_group_manager_{};
1811 };
1812 
1813 // This template method specialization is defined in the cc file.
1814 template <>
1815 std::unique_ptr<RigidBodyTree<double>> RigidBodyTree<double>::Clone() const;
1816 
1817 
1818 // To mitigate compilation time, only one `*.o` file will instantiate the
1819 // methods of RigidBodyTree that are templated only on `T` (and not on, e.g.,
1820 // `Scalar`). Refer to the the rigid_body_tree.cc comments for details.
1821 extern template class RigidBodyTree<double>;
1822 
uintptr_t ElementId
Definition: element.h:27
Definition: kinematic_path.h:6
const RigidBody< T > & world() const
Returns a const reference to the RigidBody associated with the world in the model.
Definition: rigid_body_tree.h:1607
std::vector< RigidBodyLoop< T >, Eigen::aligned_allocator< RigidBodyLoop< T > > > loops
Definition: rigid_body_tree.h:1673
Eigen::Transform< Scalar, 3, Eigen::Isometry > Isometry3
An Isometry templated on scalar type.
Definition: eigen_types.h:127
Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Eigen::Dynamic > compactToFull(const Eigen::MatrixBase< Derived > &compact, const std::vector< int > &joint_path, bool in_terms_of_qdot) const
Definition: rigid_body_tree.h:1507
Eigen::AutoDiffScalar< Eigen::VectorXd > AutoDiffXd
An autodiff variable with a dynamic number of partials.
Definition: eigen_autodiff_types.h:22
drake::Matrix6X< T > CalcFrameSpatialVelocityJacobianInWorldFrame(const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F, bool in_terms_of_qdot=false) const
Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the worl...
Definition: rigid_body_tree.h:479
int get_num_model_instances() const
Returns the number of model instances in the tree, not including the world.
Definition: rigid_body_tree.h:145
int num_velocities_
Definition: gyroscope_test.cc:72
Vector4< T > ToQuaternionAsVector4() const
Utility method to return the Vector4 associated with ToQuaterion().
Definition: rotation_matrix.h:501
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Eigen::Matrix< Scalar, 6, 6 > Matrix6
A matrix of 6 rows and 6 columns, templated on scalar type.
Definition: eigen_types.h:88
double value
Definition: wrap_test_util_py.cc:12
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
std::vector< Number > x
Definition: ipopt_solver.cc:150
Eigen::VectorXd joint_limit_max
Definition: rigid_body_tree.h:1638
std::vector< RigidBodyDistanceConstraint, Eigen::aligned_allocator< RigidBodyDistanceConstraint > > distance_constraints
Definition: rigid_body_tree.h:1678
void removeCollisionGroupsIf(UnaryPredicate test)
Definition: rigid_body_tree.h:1018
Eigen::Matrix< Scalar, kTwistSize, Eigen::Dynamic > TwistMatrix
A matrix with one twist per column, and dynamically many columns.
Definition: eigen_types.h:139
const RigidBody< T > & get_rigid_body() const
Returns the rigid body to which this frame is attached.
Definition: rigid_body_frame.h:77
int get_num_bodies() const
Returns the number of bodies in this tree.
Definition: rigid_body_tree.h:1451
STL namespace.
int get_num_positions() const
Returns the number of position states of this joint.
Definition: drake_joint.h:123
std::unique_ptr< RigidBodyTree< double > > Clone() const
Returns a deep clone of this RigidBodyTree<double>.
Definition: rigid_body_tree.h:122
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
Eigen::Matrix< Scalar, 6, Eigen::Dynamic > Matrix6X
A matrix of 6 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:104
int get_position_start_index() const
Returns the start index of this body&#39;s parent jont&#39;s position states; see RigidBody::set_position_sta...
Definition: rigid_body.h:195
Template method implementations for kinematics_cache.h.
drake::eigen_aligned_std_unordered_map< RigidBody< double > const *, drake::WrenchVector< double >> BodyToWrenchMap
Convenience alias for rigid body to external wrench map, for use with inverseDynamics and dynamicsBia...
Definition: rigid_body_tree.h:787
int get_num_velocities() const
Returns the number of velocity states of this joint.
Definition: drake_joint.h:128
const RigidBody< T > & get_body(int body_index) const
Returns the body at index body_index.
Definition: rigid_body_tree.h:1435
static const char *const kWorldName
Defines the name of the RigidBody within a RigidBodyTree that represents the world.
Definition: rigid_body_tree.h:50
drake::Vector6< T > CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
Computes Jdot_WF * v, where J_WF is the Jacobian of spatial velocity V_WF of frame F measured and exp...
Definition: rigid_body_tree.h:561
int get_velocity_start_index() const
Returns the start index of this body&#39;s parent jont&#39;s velocity states; see RigidBody::set_velocity_sta...
Definition: rigid_body.h:211
std::vector< double > vector
Definition: translator_test.cc:20
This file contains definitions for using Eigen with the STL.
drake::TwistVector< double > a_grav
Definition: rigid_body_tree.h:1680
std::string id
Definition: loader.cc:37
Eigen::MatrixXd B
Definition: rigid_body_tree.h:1681
vector< FrameId > frames_
Definition: geometry_state_test.cc:311
Multibody systems typically have distinguished frames of interest that need to be monitored...
Definition: rigid_body_frame.h:30
std::vector< const RigidBody< double > * > bodies_
Definition: multibody_tree_creation_test.cc:398
Eigen::Matrix< Scalar, 4, 1 > relativeQuaternion(const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
Definition: rigid_body_tree.h:867
Definition: collision_filter.h:13
int get_num_velocities() const
Returns the number of velocity states outputted by this RigidBodyTree.
Definition: rigid_body_tree.h:1620
static const int kWorldBodyIndex
Defines the index of the RigidBody within a RigidBodyTree that represents the world.
Definition: rigid_body_tree.h:56
Defines a physical actuator (i.e., an electric motor and step-down transmission) that operates on a j...
Definition: rigid_body_actuator.h:14
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:108
Vector3d normal
Definition: model_test.cc:39
This class provides management utilities for the definition of collision filter groups for RigidBodyT...
Definition: collision_filter.h:142
drake::Isometry3< T > CalcFramePoseInWorldFrame(const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
Computes the pose X_WF of frame_F in the world frame W.
Definition: rigid_body_tree.h:387
Eigen::Matrix< Scalar, kTwistSize, 1 > TwistVector
A column vector consisting of one twist.
Definition: eigen_types.h:135
std::pair< Eigen::Vector3d, double > resolveCenterOfPressure(const Eigen::MatrixBase< DerivedTorque > &torque, const Eigen::MatrixBase< DerivedForce > &force, const Eigen::MatrixBase< DerivedNormal > &normal, const Eigen::MatrixBase< DerivedPoint > &point_on_contact_plane)
Definition: resolve_center_of_pressure.cc:8
Vector3< typename Derived::Scalar > rotmat2rpy(const Eigen::MatrixBase< Derived > &R)
Computes SpaceXYZ Euler angles from rotation matrix.
Definition: rotation_matrix_deprecated.h:50
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
int get_num_actuators() const
Returns the number of actuators in this RigidBodyTree.
Definition: rigid_body_tree.h:1628
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > transformPointsJacobian(const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind, bool in_terms_of_qdot) const
Definition: rigid_body_tree.h:885
Eigen::Matrix< Scalar, 6, 1 > WrenchVector
A column vector consisting of one wrench (spatial force) = [r X f; f], where f is a force (translatio...
Definition: eigen_types.h:150
Eigen::VectorXd joint_limit_min
Definition: rigid_body_tree.h:1637
Defines a "relative distance constraint" that models a constraint between points on two different rig...
Definition: rigid_body_distance_constraint.h:12
bool initialized() const
Returns whether this RigidBodyTree is initialized.
Definition: rigid_body_tree.h:1634
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > transformPointsJacobianDotTimesV(const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind) const
Definition: rigid_body_tree.h:920
const DrakeJoint & getJoint() const
An accessor to this rigid body&#39;s parent joint.
Definition: rigid_body.h:119
drake::Vector6< T > CalcFrameSpatialVelocityInWorldFrame(const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F) const
Computes the spatial velocity V_WF of RigidBodyFrame frame_F measured and expressed in the world fram...
Definition: rigid_body_tree.h:418
Eigen::Matrix< double, 3, BASIS_VECTOR_HALF_COUNT > Matrix3kd
Definition: rigid_body_tree.h:38
drake::Isometry3< T > CalcBodyPoseInWorldFrame(const KinematicsCache< T > &cache, const RigidBody< T > &body) const
Computes the pose X_WB of body&#39;s frame B in the world frame W.
Definition: rigid_body_tree.h:374
Eigen::Matrix< Scalar, 6, 1 > Vector6
A column vector of size 6.
Definition: eigen_types.h:42
#define DRAKE_DEPRECATED(message)
Use DRAKE_DEPRECATED("message") to discourage use of particular classes, typedefs, variables, non-static data members, functions, arguments, enumerations, and template specializations.
Definition: drake_deprecated.h:33
RigidBody< T > & world()
Returns a mutable reference to the RigidBody associated with the world in the model.
Definition: rigid_body_tree.h:1601
std::unordered_map< Key, T, std::hash< Key >, std::equal_to< Key >, Eigen::aligned_allocator< std::pair< Key const, T >>> eigen_aligned_std_unordered_map
A std::unordered_map that uses Eigen::aligned_allocator so that the contained types may be fixed-size...
Definition: eigen_stl_types.h:31
RigidBodyTree< double > RigidBodyTreed
Definition: rigid_body_tree.h:1823
int num_positions_
Definition: gyroscope_test.cc:71
const Eigen::Isometry3d & get_transform_to_body() const
Returns the transform between the coordinate frame that belongs to this RigidBodyFrame and the coordi...
Definition: rigid_body_frame.h:107
int get_num_positions() const
Returns the number of position states outputted by this RigidBodyTree.
Definition: rigid_body_tree.h:1612
int get_next_clique_id()
Definition: rigid_body_tree.h:137
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:80
drake::Matrix6X< T > CalcBodySpatialVelocityJacobianInWorldFrame(const KinematicsCache< T > &cache, const RigidBody< T > &body, bool in_terms_of_qdot=false) const
Computes the Jacobian J_WB of the spatial velocity V_WB of body frame B measured and expressed in the...
Definition: rigid_body_tree.h:518
std::vector< RigidBodyActuator, Eigen::aligned_allocator< RigidBodyActuator > > actuators
Definition: rigid_body_tree.h:1669
Eigen::Matrix< Scalar, 3, 1 > relativeRollPitchYaw(const KinematicsCache< Scalar > &cache, int from_body_or_frame_ind, int to_body_or_frame_ind) const
Definition: rigid_body_tree.h:876
static const std::set< int > default_model_instance_id_set
Defines the default model instance ID set.
Definition: rigid_body_tree.h:67
Defines RigidBodyTree constants.
Definition: rigid_body_tree.h:45
drake::Vector6< T > CalcBodySpatialVelocityJacobianDotTimesVInWorldFrame(const KinematicsCache< T > &cache, const RigidBody< T > &body) const
Computes Jdot_WB * v, where J_WB is the Jacobian of the spatial velocity V_WB of body frame B measure...
Definition: rigid_body_tree.h:574
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3X
A matrix of 3 rows, dynamic columns, templated on scalar type.
Definition: eigen_types.h:96
ReferenceType type
Definition: loader.cc:34
Maintains a vector of RigidBody objects that are arranged into a kinematic tree via DrakeJoint object...
Definition: ik_options.h:6
void CalcBodySpatialVelocityJacobianInWorldFrame(const KinematicsCache< T > &cache, const RigidBody< T > &body, bool in_terms_of_qdot, drake::Matrix6X< T > *J_WB) const
Computes the Jacobian J_WB of the spatial velocity V_WB of body frame B measured and expressed in the...
Definition: rigid_body_tree.h:535
void surfaceTangents(const Vector3d &normal, Matrix< double, 3, m_surface_tangents > &d)
Definition: controlUtil.cpp:89
The underyling primitive class used for collision analysis.
Definition: element.h:43
static const int kFirstNonWorldModelInstanceId
The ID of the first non-world model instance in the tree.
Definition: rigid_body_tree.h:61
Eigen::Matrix< Scalar, 3, DerivedPoints::ColsAtCompileTime > transformPoints(const KinematicsCache< Scalar > &cache, const Eigen::MatrixBase< DerivedPoints > &points, int from_body_or_frame_ind, int to_body_or_frame_ind) const
Definition: rigid_body_tree.h:853
const drake::multibody::collision::Element * FindCollisionElement(const drake::multibody::collision::ElementId &id) const
Retrieve a const pointer to an element of the collision model.
Definition: rigid_body_tree.h:1012
void CalcFrameSpatialVelocityJacobianInWorldFrame(const KinematicsCache< T > &cache, const RigidBodyFrame< T > &frame_F, bool in_terms_of_qdot, drake::Matrix6X< T > *J_WF) const
Computes the Jacobian J_WF of the spatial velocity V_WF of frame F measured and expressed in the worl...
Definition: rigid_body_tree.h:500