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