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 
49  static const char* const kWorldName;
50 
55  static const int kWorldBodyIndex;
56 
61 
66  static const std::set<int> default_model_instance_id_set;
67 };
68 
106 template <typename T>
107 class RigidBodyTree {
108  public:
112  RigidBodyTree();
113 
114  virtual ~RigidBodyTree();
115 
120  std::unique_ptr<RigidBodyTree<double>> Clone() const;
121 
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 
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 
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 
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 
194  std::string get_position_name(int position_num) const;
195 
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 
221  template <typename Scalar>
222  void CheckCacheValidity(const KinematicsCache<Scalar>& cache) const;
223 
230  KinematicsCache<T> CreateKinematicsCache() const;
231 
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 
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 
275  template <typename DerivedQ>
277  const Eigen::MatrixBase<DerivedQ>& q) const;
278 
284  template <typename DerivedQ, typename DerivedV>
286  const Eigen::MatrixBase<DerivedQ>& q,
287  const Eigen::MatrixBase<DerivedV>& v, bool compute_JdotV = true) const;
288 
293  template <typename Scalar>
294  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
295  void doKinematics(KinematicsCache<Scalar>& cache,
296  bool compute_JdotV = false) const;
297 
302  bool is_part_of_model_instances(
303  const RigidBody<T>& body,
304  const std::set<int>& model_instance_id_set) const;
305 
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 
334  drake::Matrix6<T> LumpedSpatialInertiaInWorldFrame(
335  const KinematicsCache<T>& cache,
336  const std::set<int>& model_instance_id_set =
338 
347  drake::Matrix6<T> LumpedSpatialInertiaInWorldFrame(
348  const KinematicsCache<T>& cache,
349  const std::vector<const RigidBody<T>*>& bodies_of_interest) const;
350 
356  const KinematicsCache<T>& cache, const RigidBody<T>& body) const {
357  return CalcFramePoseInWorldFrame(
358  cache, body, drake::Isometry3<T>::Identity());
359  }
360 
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 
380  drake::Isometry3<T> CalcFramePoseInWorldFrame(
381  const KinematicsCache<T>& cache, const RigidBody<T>& body,
382  const drake::Isometry3<T>& X_BF) const;
383 
389  drake::Vector6<T> CalcBodySpatialVelocityInWorldFrame(
390  const KinematicsCache<T>& cache, const RigidBody<T>& body) const;
391 
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 
412  drake::Vector6<T> CalcFrameSpatialVelocityInWorldFrame(
413  const KinematicsCache<T>& cache, const RigidBody<T>& body,
414  const drake::Isometry3<T>& X_BF) const;
415 
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 
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 
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 
474  drake::Vector6<T> CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(
475  const KinematicsCache<T>& cache, const RigidBody<T>& body,
476  const drake::Isometry3<T>& X_BF) const;
477 
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 
500  const KinematicsCache<T>& cache, const RigidBody<T>& body) const {
501  return CalcFrameSpatialVelocityJacobianDotTimesVInWorldFrame(
502  cache, body, drake::Isometry3<T>::Identity());
503  }
504 
514  static drake::VectorX<T> transformQDotToVelocity(
515  const KinematicsCache<T>& cache,
516  const drake::VectorX<T>& qdot);
517 
527  static drake::VectorX<T> transformVelocityToQDot(
528  const KinematicsCache<T>& cache,
529  const drake::VectorX<T>& v);
530 
547  template <typename Derived>
549  transformVelocityMappingToQDotMapping(
551  const Eigen::MatrixBase<Derived>& Av);
552 
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 
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 
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 
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 
695 
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 
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 
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 
890  void addCollisionElement(
892  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
893  RigidBody<T>& body, const std::string& group_name);
894 
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 
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 
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 
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 
1136  RigidBody<T>* FindBody(const std::string& body_name,
1137  const std::string& model_name = "",
1138  int model_id = -1) const;
1139 
1146  const RigidBody<double>* FindBody(
1147  drake::multibody::collision::ElementId element_id) const;
1148 
1159  std::vector<const RigidBody<T>*>
1160  FindModelInstanceBodies(int model_instance_id) const;
1161 
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 
1177  std::vector<int> FindBaseBodies(int model_instance_id = -1) const;
1178 
1198  int FindBodyIndex(const std::string& body_name,
1199  int model_instance_id = -1) const;
1200 
1209  std::vector<int> FindChildrenOfBody(int parent_body_index,
1210  int model_instance_id = -1) const;
1211 
1216  DRAKE_DEPRECATED("Please use RigidBodyTree::FindBodyIndex().")
1217  int findLinkId(const std::string& link_name, int model_id = -1) const;
1218 
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 
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 
1287  std::shared_ptr<RigidBodyFrame<T>> findFrame(const std::string& frame_name,
1288  int model_id = -1) const;
1289 
1295  const RigidBody<T>& get_body(int body_index) const;
1296 
1302  RigidBody<T>* get_mutable_body(int body_index);
1303 
1308  int get_num_bodies() const;
1309 
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 
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 
1389  friend std::ostream& operator<<(std::ostream&, const RigidBodyTree<double>&);
1390 
1402  RigidBody<T>* add_rigid_body(std::unique_ptr<RigidBody<T>> body);
1403 
1412  void DefineCollisionFilterGroup(const std::string& name);
1413 
1425  void AddCollisionFilterGroupMember(const std::string& group_name,
1426  const std::string& body_name,
1427  int model_id);
1428 
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.
1451  void SetBodyCollisionFilters(
1452  const RigidBody<T>& body,
1454  const drake::multibody::collision::bitmask& ignores);
1455 
1460  RigidBody<T>& world() { return *bodies[0]; }
1461 
1466  const RigidBody<T>& world() const { return *bodies[0]; }
1467 
1471  int get_num_positions() const;
1472 
1473  DRAKE_DEPRECATED("Please use get_num_positions().")
1474  int number_of_positions() const;
1475 
1479  int get_num_velocities() const;
1480 
1481  DRAKE_DEPRECATED("Please use get_num_velocities().")
1482  int number_of_velocities() const;
1483 
1487  int get_num_actuators() const;
1488 
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
int v
Definition: rgbd_camera_test.cc:165
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
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
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:79
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:33
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
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: drakeUtil.cpp:36
void surfaceTangents(const Vector3d &normal, Matrix< double, 3, m_surface_tangents > &d)
Definition: controlUtil.cpp:84
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