Drake
rigid_body.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <iostream>
4 #include <map>
5 #include <memory>
6 #include <set>
7 #include <string>
8 #include <utility>
9 #include <vector>
10 
11 #include <Eigen/Dense>
12 
18 
19 template <typename T>
20 class RigidBody {
21  public:
22  RigidBody();
23 
40  std::unique_ptr<RigidBody<T>> Clone() const;
41 
45  const std::string& get_name() const;
46 
50  void set_name(const std::string& name);
51 
55  const std::string& get_model_name() const;
56 
60  void set_model_name(const std::string& name);
61 
65  int get_model_instance_id() const;
66 
70  void set_model_instance_id(int model_instance_id);
71 
79  // TODO(liang.fok): Deprecate this method in favor of add_joint().
80  // This requires, among potentially other things, updating the parsers.
81  void setJoint(std::unique_ptr<DrakeJoint> joint);
82 
101  template<typename JointType>
102  JointType* add_joint(RigidBody* parent, std::unique_ptr<JointType> joint) {
103  if (joint_ != nullptr) {
105  "Attempting to assign a new joint to a body that already has one");
106  }
107  set_parent(parent);
108  setJoint(move(joint));
109  return static_cast<JointType*>(joint_.get());
110  }
111 
119  const DrakeJoint& getJoint() const;
120 
124  bool has_joint() const { return joint_ != nullptr; }
125 
132  void set_parent(RigidBody* parent);
133 
137  const RigidBody* get_parent() const;
138 
147  bool has_parent_body() const;
148 
149  // TODO(liang.fok): Remove this deprecated method prior to Release 1.0.
150  DRAKE_DEPRECATED("Please use has_parent_body().")
151  bool hasParent() const;
152 
160  bool has_as_parent(const RigidBody& other) const {
161  return parent_ == &other;
162  }
163 
164 
170  void set_body_index(int body_index);
171 
176  int get_body_index() const;
177 
186  void set_position_start_index(int position_start_index);
187 
192  int get_position_start_index() const;
193 
202  void set_velocity_start_index(int velocity_start_index);
203 
208  int get_velocity_start_index() const;
209 
210  void AddVisualElement(const DrakeShapes::VisualElement& elements);
211 
213 
214  // TODO(SeanCurtis-TRI): This shouldn't be called publicly. Collision elements
215  // have to be processed in the context of the rigid body tree. Long term,
216  // this will be displaced into GeometryWorld. Short term, just don't call it.
217  // If you need to add a collision element to a body, add it through
218  // RigidBodyTree::addCollisionElement.
224  void AddCollisionElement(const std::string& group_name,
226 
231  const std::vector<drake::multibody::collision::ElementId>&
233 
238  std::vector<drake::multibody::collision::ElementId>&
240 
246  const std::map<std::string,
247  std::vector<drake::multibody::collision::ElementId>>&
249 
255  std::map<std::string, std::vector<drake::multibody::collision::ElementId>>&
257 
266  bool IsRigidlyFixedToWorld() const;
267 
293  Eigen::Isometry3d ComputeWorldFixedPose() const;
294 
295  // TODO(amcastro-tri): Change to is_adjacent_to().
296  // TODO(SeanCurtis-TRI): This method is only used by the collision clique
297  // calculation. Maybe it would be better if the name reflected this: e.g.,
298  // is_collision_adjacent(), or some such thing.
314  bool adjacentTo(const RigidBody& other) const;
315 
323  bool CanCollideWith(const RigidBody& other) const;
324 
326  const std::string& group_name,
327  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
328  std::vector<drake::multibody::collision::ElementId>& ids) const;
329 
331  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
332  std::vector<drake::multibody::collision::ElementId>& ids) const;
333 
339  const Eigen::Matrix3Xd& get_contact_points() const;
340 
346  void set_contact_points(const Eigen::Matrix3Xd& contact_points);
347 
351  void set_mass(double mass);
352 
356  double get_mass() const;
357 
362  void set_center_of_mass(const Eigen::Vector3d& center_of_mass);
363 
368  const Eigen::Vector3d& get_center_of_mass() const;
369 
374  inertia_matrix);
375 
380  const;
381 
393  const Eigen::Isometry3d& transform_body_to_joint);
394 
395  public:
396  friend std::ostream& operator<<(
397  std::ostream& out, const RigidBody<double>& b);
398 
399  public:
400  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
401 
402  typedef std::vector<drake::multibody::collision::Element*>
404  typedef typename CollisionElementsVector::iterator CollisionElementsIterator;
405 
406  CollisionElementsIterator collision_elements_begin() {
407  return collision_elements_.begin();
408  }
409 
410  CollisionElementsIterator collision_elements_end() {
411  return collision_elements_.end();
412  }
413 
419  return static_cast<int>(collision_elements_.size());
420  }
421 
422  private:
423  // TODO(tkoolen): It's very ugly, but parent, dofnum, and pitch also exist
424  // currently (independently) at the RigidBodyTree level to represent the
425  // featherstone structure. This version is for the kinematics.
426 
427  // The "parent" joint of this rigid body. This is the joint through which this
428  // rigid body connects to the rest of the rigid body tree.
429  std::unique_ptr<DrakeJoint> joint_;
430 
431  // The name of this rigid body.
432  std::string name_;
433 
434  // TODO(liang.fok) Remove this member variable, see:
435  // https://github.com/RobotLocomotion/drake/issues/3053
436  // The name of the model that defined this rigid body.
437  std::string model_name_;
438 
439  // A unique ID for the model instance to which this body belongs.
440  int model_instance_id_{0};
441 
442  // The rigid body that's connected to this rigid body's joint.
443  RigidBody* parent_{nullptr};
444 
445  // The index of this rigid body in the rigid body tree.
446  int body_index_{0};
447 
448  // The starting index of this rigid body's joint's position value(s) within
449  // the parent tree's state vector.
450  int position_start_index_{0};
451 
452  // The starting index of this rigid body's joint's velocity value(s) within
453  // the parent tree's state vector.
454  int velocity_start_index_{0};
455 
456  // A list of visual elements for this RigidBody.
457  DrakeShapes::VectorOfVisualElements visual_elements_;
458 
459  // A list of collision element IDs of collision elements that represent the
460  // geometry of this rigid body.
461  std::vector<drake::multibody::collision::ElementId> collision_element_ids_;
462 
463  // A map of groups of collision element IDs. This is just for conveniently
464  // accessing particular groups of collision elements. The groups do not imply
465  // anything in terms of how the collision elements relate to each other.
466  std::map<std::string, std::vector<drake::multibody::collision::ElementId>>
467  collision_element_groups_;
468 
469  // The contact points this rigid body has with its environment.
470  Eigen::Matrix3Xd contact_points_;
471 
472  // The mass of this rigid body.
473  double mass_{0};
474 
475  // The center of mass of this rigid body.
476  Eigen::Vector3d center_of_mass_;
477 
478  // The spatial inertia of this rigid body.
479  drake::SquareTwistMatrix<double> spatial_inertia_;
480 
481  // TODO(SeanCurtis-TRI): This data is only used in the compilation of the
482  // body. As such, it should be moved into a factory so that the runtime
483  // class only has runtime data.
484  CollisionElementsVector collision_elements_;
485 };
void set_name(const std::string &name)
Sets the name of this rigid body.
Definition: rigid_body.cc:47
double get_mass() const
Returns the mass of this rigid body.
Definition: rigid_body.cc:273
const std::string & get_name() const
Returns the name of this rigid body.
Definition: rigid_body.cc:44
RigidBody()
Definition: rigid_body.cc:22
void set_position_start_index(int position_start_index)
Sets the start index of this rigid body&#39;s mobilizer joint&#39;s contiguous generalized coordinates q (joi...
Definition: rigid_body.cc:101
std::unique_ptr< RigidBody< T > > Clone() const
Returns a clone of this RigidBody.
Definition: rigid_body.cc:28
int get_model_instance_id() const
Returns the ID of the model instance to which this rigid body belongs.
Definition: rigid_body.cc:58
const Eigen::Vector3d & get_center_of_mass() const
Gets the center of mass of this rigid body.
Definition: rigid_body.cc:283
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Provides a portable macro for use in generating compile-time warnings for use of code that is permitt...
void set_model_instance_id(int model_instance_id)
Sets the ID of the model instance to which this rigid body belongs.
Definition: rigid_body.cc:61
const DrakeJoint & getJoint() const
An accessor to this rigid body&#39;s parent joint.
Definition: rigid_body.cc:71
void set_center_of_mass(const Eigen::Vector3d &center_of_mass)
Sets the center of mass of this rigid body.
Definition: rigid_body.cc:278
A joint defines a spatial relationship between two rigid bodies.
Definition: drake_joint.h:62
bool has_parent_body() const
Returns whether this RigidBody has a "parent", which is a RigidBody that is connected to this RigidBo...
Definition: rigid_body.cc:88
std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_mutable_group_to_collision_ids_map()
Returns a map of collision element group names to vectors of collision element IDs.
Definition: rigid_body.cc:162
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
#define DRAKE_ABORT_MSG(message)
Aborts the program (via ::abort) with a message showing at least the given message (macro argument)...
Definition: drake_assert.h:53
const drake::SquareTwistMatrix< double > & get_spatial_inertia() const
Returns the spatial inertia of this rigid body.
Definition: rigid_body.cc:294
bool adjacentTo(const RigidBody &other) const
Reports if this body is considered "adjacent" to the given body.
Definition: rigid_body.cc:208
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef std::vector< drake::multibody::collision::Element * > CollisionElementsVector
Definition: rigid_body.h:403
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
bool has_as_parent(const RigidBody &other) const
Checks if a particular rigid body is the parent of this rigid body.
Definition: rigid_body.h:160
CollisionElementsIterator collision_elements_begin()
Definition: rigid_body.h:406
void set_velocity_start_index(int velocity_start_index)
Sets the start index of this rigid body&#39;s mobilizer joint&#39;s contiguous generalized velocity v (joint ...
Definition: rigid_body.cc:111
int get_body_index() const
Returns the "body index" of this RigidBody.
Definition: rigid_body.cc:98
int b
Definition: rgbd_camera.cc:91
Definition: collision_filter.h:13
void set_contact_points(const Eigen::Matrix3Xd &contact_points)
Saves the points on this rigid body that should be checked for collision between this rigid body and ...
Definition: rigid_body.cc:263
bool appendCollisionElementIdsFromThisBody(const std::string &group_name, std::vector< drake::multibody::collision::ElementId > &ids) const
Definition: rigid_body.cc:221
CollisionElementsVector::iterator CollisionElementsIterator
Definition: rigid_body.h:404
bool IsRigidlyFixedToWorld() const
Reports if there is a path in this tree from this body to the world where all joints are fixed...
Definition: rigid_body.cc:167
Provides Drake&#39;s assertion implementation.
void ApplyTransformToJointFrame(const Eigen::Isometry3d &transform_body_to_joint)
Transforms all of the visual, collision, and inertial elements associated with this body to the prope...
Definition: rigid_body.cc:248
bool hasParent() const
Definition: rigid_body.cc:92
CollisionElementsIterator collision_elements_end()
Definition: rigid_body.h:410
const std::string & get_model_name() const
Returns the name of the model defining this rigid body.
Definition: rigid_body.cc:50
Eigen::Isometry3d ComputeWorldFixedPose() const
Reports X_WBₙ, the pose of this body, Bₙ, in the world frame based on the rigid kinematic path from...
Definition: rigid_body.cc:189
bool has_joint() const
Reports if the body has a parent joint.
Definition: rigid_body.h:124
void set_mass(double mass)
Sets the mass of this rigid body.
Definition: rigid_body.cc:268
#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
JointType * add_joint(RigidBody *parent, std::unique_ptr< JointType > joint)
Adds degrees of freedom to this body by connecting it to parent with joint.
Definition: rigid_body.h:102
std::vector< drake::multibody::collision::ElementId > & get_mutable_collision_element_ids()
Returns a reference to an std::vector of collision elements that represent the collision geometry of ...
Definition: rigid_body.cc:149
const std::vector< drake::multibody::collision::ElementId > & get_collision_element_ids() const
Definition: rigid_body.cc:143
bool CanCollideWith(const RigidBody &other) const
Returns true if this body should be checked for collisions with the other body.
Definition: rigid_body.cc:215
const RigidBody * get_parent() const
Returns a const pointer to this rigid body&#39;s parent rigid body.
Definition: rigid_body.cc:85
The term rigid body implies that the deformations of the body under consideration are so small that t...
Definition: fixed_offset_frame.h:16
drake::eigen_aligned_std_vector< VisualElement > VectorOfVisualElements
Definition: visual_element.h:35
void AddCollisionElement(const std::string &group_name, drake::multibody::collision::Element *element)
Adds the given collision element to the body with the given group name.
Definition: rigid_body.cc:132
Definition: visual_element.h:9
const Eigen::Matrix3Xd & get_contact_points() const
Returns the points on this rigid body that should be checked for collision with the environment...
Definition: rigid_body.cc:258
Eigen::Matrix< Scalar, kTwistSize, kTwistSize > SquareTwistMatrix
A six-by-six matrix.
Definition: eigen_types.h:125
void set_body_index(int body_index)
Sets the "body index" of this RigidBody.
Definition: rigid_body.cc:95
void AddVisualElement(const DrakeShapes::VisualElement &elements)
Definition: rigid_body.cc:121
const std::map< std::string, std::vector< drake::multibody::collision::ElementId > > & get_group_to_collision_ids_map() const
Returns a map of collision element group names to vectors of collision element IDs.
Definition: rigid_body.cc:156
void set_spatial_inertia(const drake::SquareTwistMatrix< double > &inertia_matrix)
Sets the spatial inertia of this rigid body.
Definition: rigid_body.cc:288
void set_model_name(const std::string &name)
Sets the name of the model defining this rigid body.
Definition: rigid_body.cc:53
int get_num_collision_elements() const
Reports the total number of registered collision elements attached to this body.
Definition: rigid_body.h:418
void set_parent(RigidBody *parent)
Sets the parent rigid body.
Definition: rigid_body.cc:82
void setJoint(std::unique_ptr< DrakeJoint > joint)
Sets the parent joint through which this rigid body connects to its parent rigid body.
Definition: rigid_body.cc:66
const DrakeShapes::VectorOfVisualElements & get_visual_elements() const
Definition: rigid_body.cc:126
The underyling primitive class used for collision analysis.
Definition: element.h:42
friend std::ostream & operator<<(std::ostream &out, const RigidBody< double > &b)
Definition: rigid_body.cc:299