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