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