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);
71 
73  public: void LoadJoints();
74 
76  public: virtual void Init();
77 
79  public: void Update();
80 
82  public: virtual void Fini();
83 
86  public: virtual void UpdateParameters(sdf::ElementPtr _sdf);
87 
90  public: virtual const sdf::ElementPtr GetSDF();
91 
97  public: virtual const sdf::ElementPtr UnscaledSDF();
98 
101  public: virtual void RemoveChild(EntityPtr _child);
102  using Base::RemoveChild;
103 
105  public: void Reset();
106  using Entity::Reset;
107 
110  public: void ResetPhysicsStates();
111 
114  public: void SetLinearVel(const ignition::math::Vector3d &_vel);
115 
118  public: void SetAngularVel(const ignition::math::Vector3d &_vel);
119 
123  public: virtual ignition::math::Vector3d RelativeLinearVel() const;
124 
128  public: virtual ignition::math::Vector3d WorldLinearVel() const;
129 
133  public: virtual ignition::math::Vector3d RelativeAngularVel() const;
134 
138  public: virtual ignition::math::Vector3d WorldAngularVel() const;
139 
143  public: virtual ignition::math::Vector3d RelativeLinearAccel() const;
144 
148  public: virtual ignition::math::Vector3d WorldLinearAccel() const;
149 
153  public: virtual ignition::math::Vector3d RelativeAngularAccel() const;
154 
158  public: virtual ignition::math::Vector3d WorldAngularAccel() const;
159 
162  public: virtual ignition::math::Box BoundingBox() const;
163 
166  public: unsigned int GetJointCount() const;
167 
171  public: ModelPtr NestedModel(const std::string &_name) const;
172 
175  public: const Model_V &NestedModels() const;
176 
180  public: const Link_V &GetLinks() const;
181 
184  public: const Joint_V &GetJoints() const;
185 
189  public: JointPtr GetJoint(const std::string &name);
190 
195  public: LinkPtr GetLinkById(unsigned int _id) const;
197 
201  public: LinkPtr GetLink(const std::string &_name ="canonical") const;
202 
210  public: virtual bool GetSelfCollide() const;
211 
215  public: virtual void SetSelfCollide(bool _self_collide);
216 
219  public: void SetGravityMode(const bool &_value);
220 
225  public: void SetCollideMode(const std::string &_mode);
226 
229  public: void SetLaserRetro(const float _retro);
230 
233  public: virtual void FillMsg(msgs::Model &_msg);
234 
237  public: void ProcessMsg(const msgs::Model &_msg);
238 
243  public: void SetJointPosition(const std::string &_jointName,
244  double _position, int _index = 0);
245 
249  public: void SetJointPositions(
250  const std::map<std::string, double> &_jointPositions);
251 
256  public: void SetJointAnimation(
257  const std::map<std::string, common::NumericAnimationPtr> &_anims,
258  boost::function<void()> _onComplete = NULL);
259 
261  public: virtual void StopAnimation();
262 
277  public: void AttachStaticModel(ModelPtr &_model,
278  ignition::math::Pose3d _offset);
279 
283  public: void DetachStaticModel(const std::string &_model);
284 
287  public: void SetState(const ModelState &_state);
288 
294  public: void SetScale(const ignition::math::Vector3d &_scale,
295  const bool _publish = false);
296 
301  public: ignition::math::Vector3d Scale() const;
302 
305  public: void SetEnabled(bool _enabled);
306 
313  public: void SetLinkWorldPose(const ignition::math::Pose3d &_pose,
314  std::string _linkName);
315 
322  public: void SetLinkWorldPose(const ignition::math::Pose3d &_pose,
323  const LinkPtr &_link);
324 
328  public: void SetAutoDisable(bool _disable);
329 
332  public: bool GetAutoDisable() const;
333 
337  public: void LoadPlugins();
338 
341  public: unsigned int GetPluginCount() const;
342 
346  public: unsigned int GetSensorCount() const;
347 
360  public: std::vector<std::string> SensorScopedName(
361  const std::string &_name) const;
362 
366 
369  public: GripperPtr GetGripper(size_t _index) const;
370 
374  public: size_t GetGripperCount() const;
375 
379  public: double GetWorldEnergyPotential() const;
380 
385  public: double GetWorldEnergyKinetic() const;
386 
391  public: double GetWorldEnergy() const;
392 
406  const std::string &_name, const std::string &_type,
407  physics::LinkPtr _parent, physics::LinkPtr _child);
408 
419  sdf::ElementPtr _sdf);
420 
424  public: virtual bool RemoveJoint(const std::string &_name);
425 
428  public: virtual void SetWindMode(const bool _mode);
429 
432  public: virtual bool WindMode() const;
433 
436  public: boost::shared_ptr<Model> shared_from_this();
437 
442  public: LinkPtr CreateLink(const std::string &_name);
443 
461  public: void PluginInfo(const common::URI &_pluginUri,
462  ignition::msgs::Plugin_V &_plugins, bool &_success);
463 
465  protected: virtual void OnPoseChange();
466 
468  protected: virtual void RegisterIntrospectionItems();
469 
471  private: void LoadLinks();
472 
474  private: void LoadModels();
475 
478  private: void LoadJoint(sdf::ElementPtr _sdf);
479 
482  private: void LoadPlugin(sdf::ElementPtr _sdf);
483 
486  private: void LoadGripper(sdf::ElementPtr _sdf);
487 
491  private: void RemoveLink(const std::string &_name);
492 
494  private: virtual void PublishScale();
495 
497  protected: std::vector<ModelPtr> attachedModels;
498 
500  protected: std::vector<ignition::math::Pose3d> attachedModelsOffset;
501 
504 
506  private: LinkPtr canonicalLink;
507 
509  private: Joint_V joints;
510 
512  private: Link_V links;
513 
515  private: Model_V models;
516 
518  private: std::vector<GripperPtr> grippers;
519 
521  private: std::vector<ModelPluginPtr> plugins;
522 
524  private: std::map<std::string, common::NumericAnimationPtr>
525  jointAnimations;
526 
528  private: boost::function<void()> onJointAnimationComplete;
529 
531  private: JointControllerPtr jointController;
532 
534  private: mutable boost::recursive_mutex updateMutex;
535 
537  private: std::mutex receiveMutex;
538  };
540  }
541 }
542 #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
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.
virtual ignition::math::Vector3d RelativeAngularVel() const
Get the angular velocity of the entity.
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:500
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 Fini()
Finalize the model.
void Reset()
Reset the model.
virtual void Init()
Initialize the model.
virtual void RemoveChild(EntityPtr _child)
Remove a child.
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 const sdf::ElementPtr GetSDF()
Get the SDF values for the model.
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.
virtual void RegisterIntrospectionItems()
Register items in the introspection service.
void SetJointAnimation(const std::map< std::string, common::NumericAnimationPtr > &_anims, boost::function< void()> _onComplete=NULL)
Joint Animation.
virtual ignition::math::Box BoundingBox() const
Get the size of the bounding box.
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 WorldAngularVel() const
Get the angular velocity of the entity in the world frame.
JointPtr GetJoint(const std::string &name)
Get a joint.
virtual ignition::math::Vector3d RelativeLinearVel() const
Get the linear velocity of the entity.
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...
transport::PublisherPtr jointPub
Publisher for joint info.
Definition: Model.hh:503
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.
virtual ignition::math::Vector3d RelativeAngularAccel() const
Get the angular acceleration of the entity.
const Joint_V & GetJoints() const
Get the joints.
virtual void StopAnimation()
Stop the current animations.
void SetLinearVel(const ignition::math::Vector3d &_vel)
Set the linear velocity of the model, and all its links.
virtual ~Model()
Destructor.
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 ignition::math::Vector3d RelativeLinearAccel() const
Get the linear acceleration of the entity.
virtual void FillMsg(msgs::Model &_msg)
Fill a model message.
virtual ignition::math::Vector3d WorldLinearAccel() const
Get the linear acceleration of the entity in the world frame.
void SetJointPosition(const std::string &_jointName, double _position, int _index=0)
Set the positions of a Joint by name.
std::vector< std::string > SensorScopedName(const std::string &_name) const
Get scoped sensor name(s) in the model that matches sensor name.
bool GetAutoDisable() const
Return the value of the SDF <allow_auto_disable> element.
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 Load(sdf::ElementPtr _sdf)
Load the model.
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:497
virtual void UpdateParameters(sdf::ElementPtr _sdf)
Update the parameters using new sdf values.
void AttachStaticModel(ModelPtr &_model, ignition::math::Pose3d _offset)
Attach a static model to this model.
ModelPtr NestedModel(const std::string &_name) const
Get a nested model that is a direct child of this model.
virtual void OnPoseChange()
Callback when the pose of the model has been changed.
ignition::math::Vector3d Scale() const
Get the scale of model.
virtual ignition::math::Vector3d WorldAngularAccel() const
Get the angular acceleration of the entity in the world frame.
void SetCollideMode(const std::string &_mode)
\TODO This is not implemented in Link, which means this function doesn't do anything.
void Update()
Update the model.
unsigned int GetPluginCount() const
Get the number of plugins this model has.
void LoadJoints()
Load all the joints.
virtual ignition::math::Vector3d WorldLinearVel() const
Get the linear velocity of the entity in the world frame.
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 LoadPlugins()
Load all plugins.
Definition: JointMaker.hh:45
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:205
boost::shared_ptr< JointController > JointControllerPtr
Definition: PhysicsTypes.hh:121
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
boost::shared_ptr< Gripper > GripperPtr
Definition: PhysicsTypes.hh:197
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:77
std::vector< JointPtr > Joint_V
Definition: PhysicsTypes.hh:213
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:85
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:225
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:117
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
Forward declarations for the common classes.
Definition: Animation.hh:27
Definition: Model.hh:41