Model.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GAZEBO_PHYSICS_MODEL_HH_
18 #define GAZEBO_PHYSICS_MODEL_HH_
19 
20 #include <string>
21 #include <map>
22 #include <mutex>
23 #include <vector>
24 #include <boost/function.hpp>
25 #include <boost/thread/recursive_mutex.hpp>
26 
30 #include "gazebo/physics/Entity.hh"
32 #include "gazebo/util/system.hh"
33 
34 namespace boost
35 {
36  class recursive_mutex;
37 }
38 
39 // Forward declare reference and pointer parameters
40 namespace ignition
41 {
42  namespace msgs
43  {
44  class Plugin_V;
45  }
46 }
47 
48 namespace gazebo
49 {
50  namespace physics
51  {
52  class Gripper;
53 
56 
59  class GZ_PHYSICS_VISIBLE Model : public Entity
60  {
63  public: explicit Model(BasePtr _parent);
64 
66  public: virtual ~Model();
67 
70  public: void Load(sdf::ElementPtr _sdf) override;
71 
73  public: void LoadJoints();
74 
76  public: virtual void Init() override;
77 
79  public: void Update() override;
80 
82  public: virtual void Fini() override;
83 
86  public: virtual void UpdateParameters(sdf::ElementPtr _sdf) override;
87 
90  public: virtual const sdf::ElementPtr GetSDF() override;
91 
94  public: const sdf::Model *GetSDFDom() const;
95 
101  public: virtual const sdf::ElementPtr UnscaledSDF();
102 
105  public: virtual void RemoveChild(EntityPtr _child);
106  using Base::RemoveChild;
107 
109  public: void Reset() override;
110  using Entity::Reset;
111 
114  public: void ResetPhysicsStates();
115 
118  public: void SetLinearVel(const ignition::math::Vector3d &_vel);
119 
122  public: void SetAngularVel(const ignition::math::Vector3d &_vel);
123 
127  public: virtual ignition::math::Vector3d RelativeLinearVel() const
128  override;
129 
133  public: virtual ignition::math::Vector3d WorldLinearVel() const
134  override;
135 
139  public: virtual ignition::math::Vector3d RelativeAngularVel() const
140  override;
141 
145  public: virtual ignition::math::Vector3d WorldAngularVel() const
146  override;
147 
151  public: virtual ignition::math::Vector3d RelativeLinearAccel() const
152  override;
153 
157  public: virtual ignition::math::Vector3d WorldLinearAccel() const
158  override;
159 
163  public: virtual ignition::math::Vector3d RelativeAngularAccel() const
164  override;
165 
169  public: virtual ignition::math::Vector3d WorldAngularAccel() const
170  override;
171 
174  public: virtual ignition::math::AxisAlignedBox BoundingBox() const
175  override;
176 
179  public: unsigned int GetJointCount() const;
180 
184  public: ModelPtr NestedModel(const std::string &_name) const;
185 
188  public: const Model_V &NestedModels() const;
189 
193  public: const Link_V &GetLinks() const;
194 
197  public: const Joint_V &GetJoints() const;
198 
202  public: JointPtr GetJoint(const std::string &name);
203 
208  public: LinkPtr GetLinkById(unsigned int _id) const;
210 
214  public: LinkPtr GetLink(const std::string &_name ="canonical") const;
215 
223  public: virtual bool GetSelfCollide() const;
224 
228  public: virtual void SetSelfCollide(bool _self_collide);
229 
232  public: void SetGravityMode(const bool &_value);
233 
238  public: void SetCollideMode(const std::string &_mode);
239 
242  public: void SetLaserRetro(const float _retro);
243 
246  public: virtual void FillMsg(msgs::Model &_msg);
247 
250  public: void ProcessMsg(const msgs::Model &_msg);
251 
256  public: void SetJointPosition(const std::string &_jointName,
257  double _position, int _index = 0);
258 
262  public: void SetJointPositions(
263  const std::map<std::string, double> &_jointPositions);
264 
269  public: void SetJointAnimation(
270  const std::map<std::string, common::NumericAnimationPtr> &_anims,
271  boost::function<void()> _onComplete = NULL);
272 
274  public: virtual void StopAnimation() override;
275 
290  public: void AttachStaticModel(ModelPtr &_model,
291  ignition::math::Pose3d _offset);
292 
296  public: void DetachStaticModel(const std::string &_model);
297 
300  public: void SetState(const ModelState &_state);
301 
307  public: void SetScale(const ignition::math::Vector3d &_scale,
308  const bool _publish = false);
309 
314  public: ignition::math::Vector3d Scale() const;
315 
318  public: void SetEnabled(bool _enabled);
319 
326  public: void SetLinkWorldPose(const ignition::math::Pose3d &_pose,
327  std::string _linkName);
328 
335  public: void SetLinkWorldPose(const ignition::math::Pose3d &_pose,
336  const LinkPtr &_link);
337 
341  public: void SetAutoDisable(bool _disable);
342 
345  public: bool GetAutoDisable() const;
346 
350  public: void LoadPlugins();
351 
354  public: unsigned int GetPluginCount() const;
355 
359  public: unsigned int GetSensorCount() const;
360 
373  public: std::vector<std::string> SensorScopedName(
374  const std::string &_name) const;
375 
379 
382  public: GripperPtr GetGripper(size_t _index) const;
383 
387  public: size_t GetGripperCount() const;
388 
392  public: double GetWorldEnergyPotential() const;
393 
398  public: double GetWorldEnergyKinetic() const;
399 
404  public: double GetWorldEnergy() const;
405 
419  const std::string &_name, const std::string &_type,
420  physics::LinkPtr _parent, physics::LinkPtr _child);
421 
432  sdf::ElementPtr _sdf);
433 
437  public: virtual bool RemoveJoint(const std::string &_name);
438 
441  public: virtual void SetWindMode(const bool _mode);
442 
445  public: virtual bool WindMode() const;
446 
449  public: boost::shared_ptr<Model> shared_from_this();
450 
455  public: LinkPtr CreateLink(const std::string &_name);
456 
474  public: void PluginInfo(const common::URI &_pluginUri,
475  ignition::msgs::Plugin_V &_plugins, bool &_success);
476 
477  // Documentation inherited.
478  public: std::optional<sdf::SemanticPose> SDFSemanticPose() const override;
479 
481  protected: virtual void OnPoseChange() override;
482 
484  protected: virtual void RegisterIntrospectionItems() override;
485 
487  private: void LoadLinks();
488 
490  private: void LoadModels();
491 
494  private: void LoadJoint(sdf::ElementPtr _sdf);
495 
498  private: void LoadPlugin(sdf::ElementPtr _sdf);
499 
502  private: void LoadGripper(sdf::ElementPtr _sdf);
503 
507  private: void RemoveLink(const std::string &_name);
508 
510  private: virtual void PublishScale();
511 
513  protected: std::vector<ModelPtr> attachedModels;
514 
516  protected: std::vector<ignition::math::Pose3d> attachedModelsOffset;
517 
520 
522  private: LinkPtr canonicalLink;
523 
525  private: Joint_V joints;
526 
528  private: Link_V links;
529 
531  private: Model_V models;
532 
534  private: std::vector<GripperPtr> grippers;
535 
537  private: std::vector<ModelPluginPtr> plugins;
538 
540  private: std::map<std::string, common::NumericAnimationPtr>
541  jointAnimations;
542 
544  private: boost::function<void()> onJointAnimationComplete;
545 
547  private: JointControllerPtr jointController;
548 
550  private: mutable boost::recursive_mutex updateMutex;
551 
553  private: std::mutex receiveMutex;
554 
558  private: std::unique_ptr<sdf::Model> modelSDFDomIsolated;
559 
561  private: const sdf::Model *modelSDFDom = nullptr;
562  };
564  }
565 }
566 #endif
#define NULL
Definition: CommonTypes.hh:31
default namespace for gazebo
Forward declarations for transport.
A complete URI.
Definition: URI.hh:177
virtual void RemoveChild(unsigned int _id)
Remove a child from this entity.
Base class for all physics objects in Gazebo.
Definition: Entity.hh:53
virtual void Reset()
Reset the entity.
Store state information of a physics::Model object.
Definition: ModelState.hh:49
A model is a collection of links, joints, and plugins.
Definition: Model.hh:60
virtual ignition::math::Vector3d WorldLinearAccel() const override
Get the linear acceleration of the entity in the world frame.
void SetAngularVel(const ignition::math::Vector3d &_vel)
Set the angular velocity of the model, and all its links.
void SetScale(const ignition::math::Vector3d &_scale, const bool _publish=false)
Set the scale of model.
JointControllerPtr GetJointController()
Get a handle to the Controller for the joints in this model.
void DetachStaticModel(const std::string &_model)
Detach a static model from this model.
virtual gazebo::physics::JointPtr CreateJoint(sdf::ElementPtr _sdf)
Create a joint for this model.
virtual bool GetSelfCollide() const
If true, all links within the model will collide by default.
const Model_V & NestedModels() const
Get all the nested models.
void SetLaserRetro(const float _retro)
Set the laser retro reflectiveness of the model.
std::vector< ignition::math::Pose3d > attachedModelsOffset
used by Model::AttachStaticModel
Definition: Model.hh:516
virtual ignition::math::Vector3d RelativeAngularVel() const override
Get the angular velocity of the entity.
virtual void SetWindMode(const bool _mode)
Set whether wind affects this body.
void PluginInfo(const common::URI &_pluginUri, ignition::msgs::Plugin_V &_plugins, bool &_success)
Get information about plugins in this model or one of its children, according to the given _pluginUri...
virtual void OnPoseChange() override
Callback when the pose of the model has been changed.
virtual ignition::math::Vector3d WorldAngularAccel() const override
Get the angular acceleration of the entity in the world frame.
virtual void RemoveChild(EntityPtr _child)
Remove a child.
void Load(sdf::ElementPtr _sdf) override
Load the model.
void SetAutoDisable(bool _disable)
Allow the model the auto disable.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of all child links.
const Link_V & GetLinks() const
Construct and return a vector of Link's in this model Note this constructs the vector of Link's on th...
virtual ignition::math::AxisAlignedBox BoundingBox() const override
Get the size of the bounding box.
virtual ignition::math::Vector3d RelativeLinearAccel() const override
Get the linear acceleration of the entity.
virtual ignition::math::Vector3d RelativeAngularAccel() const override
Get the angular acceleration of the entity.
unsigned int GetJointCount() const
Get the number of joints.
LinkPtr CreateLink(const std::string &_name)
Create a new link for this model.
void ProcessMsg(const msgs::Model &_msg)
Update parameters from a model message.
Model(BasePtr _parent)
Constructor.
void SetJointAnimation(const std::map< std::string, common::NumericAnimationPtr > &_anims, boost::function< void()> _onComplete=NULL)
Joint Animation.
unsigned int GetSensorCount() const
Get the number of sensors attached to this model.
LinkPtr GetLink(const std::string &_name="canonical") const
Get a link by name.
double GetWorldEnergyKinetic() const
Returns sum of the kinetic energies of all links in this model.
virtual bool RemoveJoint(const std::string &_name)
Remove a joint for this model.
virtual ignition::math::Vector3d WorldLinearVel() const override
Get the linear velocity of the entity in the world frame.
virtual void StopAnimation() override
Stop the current animations.
JointPtr GetJoint(const std::string &name)
Get a joint.
void SetEnabled(bool _enabled)
Enable all the links in all the models.
virtual const sdf::ElementPtr UnscaledSDF()
virtual bool WindMode() const
Get the wind mode.
double GetWorldEnergy() const
Returns this model's total energy, or sum of Model::GetWorldEnergyPotential() and Model::GetWorldEner...
virtual void RegisterIntrospectionItems() override
Register items in the introspection service.
transport::PublisherPtr jointPub
Publisher for joint info.
Definition: Model.hh:519
void SetLinkWorldPose(const ignition::math::Pose3d &_pose, std::string _linkName)
Set the Pose of the entire Model by specifying desired Pose of a Link within the Model.
GripperPtr GetGripper(size_t _index) const
Get a gripper based on an index.
const Joint_V & GetJoints() const
Get the joints.
void SetLinearVel(const ignition::math::Vector3d &_vel)
Set the linear velocity of the model, and all its links.
virtual ~Model()
Destructor.
std::optional< sdf::SemanticPose > SDFSemanticPose() const override
Get the SDF SemanticPose object associated with the pose of this object.
double GetWorldEnergyPotential() const
Returns the potential energy of all links and joint springs in the model.
void SetState(const ModelState &_state)
Set the current model state.
virtual void SetSelfCollide(bool _self_collide)
Set this model's self_collide property.
virtual gazebo::physics::JointPtr CreateJoint(const std::string &_name, const std::string &_type, physics::LinkPtr _parent, physics::LinkPtr _child)
Create a joint for this model.
virtual void FillMsg(msgs::Model &_msg)
Fill a model message.
void Update() override
Update the model.
void SetJointPosition(const std::string &_jointName, double _position, int _index=0)
Set the positions of a Joint by name.
virtual void UpdateParameters(sdf::ElementPtr _sdf) override
Update the parameters using new sdf values.
virtual void Fini() override
Finalize the model.
std::vector< std::string > SensorScopedName(const std::string &_name) const
Get scoped sensor name(s) in the model that matches sensor name.
virtual const sdf::ElementPtr GetSDF() override
Get the SDF values for the model.
bool GetAutoDisable() const
Return the value of the SDF <allow_auto_disable> element.
const sdf::Model * GetSDFDom() const
Get the SDF DOM for the model.
void SetGravityMode(const bool &_value)
Set the gravity mode of the model.
size_t GetGripperCount() const
Get the number of grippers in this model.
boost::shared_ptr< Model > shared_from_this()
Allow Model class to share itself as a boost shared_ptr.
void SetJointPositions(const std::map< std::string, double > &_jointPositions)
Set the positions of a set of joints.
std::vector< ModelPtr > attachedModels
used by Model::AttachStaticModel
Definition: Model.hh:513
void AttachStaticModel(ModelPtr &_model, ignition::math::Pose3d _offset)
Attach a static model to this model.
virtual void Init() override
Initialize the model.
ModelPtr NestedModel(const std::string &_name) const
Get a nested model that is a direct child of this model.
ignition::math::Vector3d Scale() const
Get the scale of model.
virtual ignition::math::Vector3d WorldAngularVel() const override
Get the angular velocity of the entity in the world frame.
virtual ignition::math::Vector3d RelativeLinearVel() const override
Get the linear velocity of the entity.
void SetCollideMode(const std::string &_mode)
\TODO This is not implemented in Link, which means this function doesn't do anything.
unsigned int GetPluginCount() const
Get the number of plugins this model has.
void LoadJoints()
Load all the joints.
void SetLinkWorldPose(const ignition::math::Pose3d &_pose, const LinkPtr &_link)
Set the Pose of the entire Model by specifying desired Pose of a Link within the Model.
void Reset() override
Reset the model.
void LoadPlugins()
Load all plugins.
Definition: JointMaker.hh:45
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:206
boost::shared_ptr< JointController > JointControllerPtr
Definition: PhysicsTypes.hh:122
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:110
boost::shared_ptr< Gripper > GripperPtr
Definition: PhysicsTypes.hh:198
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
std::vector< JointPtr > Joint_V
Definition: PhysicsTypes.hh:214
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:86
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:226
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:118
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
Forward declarations for the common classes.
Definition: Animation.hh:27
Definition: Model.hh:41