Go to the documentation of this file.
17 #ifndef GAZEBO_PHYSICS_MODEL_HH_
18 #define GAZEBO_PHYSICS_MODEL_HH_
24 #include <boost/function.hpp>
25 #include <boost/thread/recursive_mutex.hpp>
36 class recursive_mutex;
66 public:
virtual ~
Model();
70 public:
void Load(sdf::ElementPtr _sdf)
override;
73 public:
void LoadJoints();
76 public:
virtual void Init()
override;
79 public:
void Update()
override;
82 public:
virtual void Fini()
override;
86 public:
virtual void UpdateParameters(sdf::ElementPtr _sdf)
override;
90 public:
virtual const sdf::ElementPtr GetSDF()
override;
94 public:
const sdf::Model *GetSDFDom()
const;
101 public:
virtual const sdf::ElementPtr UnscaledSDF();
105 public:
virtual void RemoveChild(
EntityPtr _child);
109 public:
void Reset()
override;
114 public:
void ResetPhysicsStates();
118 public:
void SetLinearVel(
const ignition::math::Vector3d &_vel);
122 public:
void SetAngularVel(
const ignition::math::Vector3d &_vel);
127 public:
virtual ignition::math::Vector3d RelativeLinearVel()
const
133 public:
virtual ignition::math::Vector3d WorldLinearVel()
const
139 public:
virtual ignition::math::Vector3d RelativeAngularVel()
const
145 public:
virtual ignition::math::Vector3d WorldAngularVel()
const
151 public:
virtual ignition::math::Vector3d RelativeLinearAccel()
const
157 public:
virtual ignition::math::Vector3d WorldLinearAccel()
const
163 public:
virtual ignition::math::Vector3d RelativeAngularAccel()
const
169 public:
virtual ignition::math::Vector3d WorldAngularAccel()
const
174 public:
virtual ignition::math::AxisAlignedBox BoundingBox()
const
179 public:
unsigned int GetJointCount()
const;
184 public:
ModelPtr NestedModel(
const std::string &_name)
const;
188 public:
const Model_V &NestedModels()
const;
193 public:
const Link_V &GetLinks()
const;
197 public:
const Joint_V &GetJoints()
const;
202 public:
JointPtr GetJoint(
const std::string &name);
208 public:
LinkPtr GetLinkById(
unsigned int _id)
const;
214 public:
LinkPtr GetLink(
const std::string &_name =
"canonical")
const;
223 public:
virtual bool GetSelfCollide()
const;
228 public:
virtual void SetSelfCollide(
bool _self_collide);
232 public:
void SetGravityMode(
const bool &_value);
238 public:
void SetCollideMode(
const std::string &_mode);
242 public:
void SetLaserRetro(
const float _retro);
246 public:
virtual void FillMsg(msgs::Model &_msg);
250 public:
void ProcessMsg(
const msgs::Model &_msg);
256 public:
void SetJointPosition(
const std::string &_jointName,
257 double _position,
int _index = 0);
262 public:
void SetJointPositions(
263 const std::map<std::string, double> &_jointPositions);
269 public:
void SetJointAnimation(
270 const std::map<std::string, common::NumericAnimationPtr> &_anims,
271 boost::function<
void()> _onComplete =
NULL);
274 public:
virtual void StopAnimation()
override;
290 public:
void AttachStaticModel(
ModelPtr &_model,
291 ignition::math::Pose3d _offset);
296 public:
void DetachStaticModel(
const std::string &_model);
300 public:
void SetState(
const ModelState &_state);
307 public:
void SetScale(
const ignition::math::Vector3d &_scale,
308 const bool _publish =
false);
314 public: ignition::math::Vector3d Scale()
const;
318 public:
void SetEnabled(
bool _enabled);
326 public:
void SetLinkWorldPose(
const ignition::math::Pose3d &_pose,
327 std::string _linkName);
335 public:
void SetLinkWorldPose(
const ignition::math::Pose3d &_pose,
341 public:
void SetAutoDisable(
bool _disable);
345 public:
bool GetAutoDisable()
const;
350 public:
void LoadPlugins();
354 public:
unsigned int GetPluginCount()
const;
359 public:
unsigned int GetSensorCount()
const;
373 public: std::vector<std::string> SensorScopedName(
374 const std::string &_name)
const;
382 public:
GripperPtr GetGripper(
size_t _index)
const;
387 public:
size_t GetGripperCount()
const;
392 public:
double GetWorldEnergyPotential()
const;
398 public:
double GetWorldEnergyKinetic()
const;
404 public:
double GetWorldEnergy()
const;
419 const std::string &_name,
const std::string &_type,
432 sdf::ElementPtr _sdf);
437 public:
virtual bool RemoveJoint(
const std::string &_name);
441 public:
virtual void SetWindMode(
const bool _mode);
445 public:
virtual bool WindMode()
const;
449 public: boost::shared_ptr<Model> shared_from_this();
455 public:
LinkPtr CreateLink(
const std::string &_name);
474 public:
void PluginInfo(
const common::URI &_pluginUri,
475 ignition::msgs::Plugin_V &_plugins,
bool &_success);
478 public: std::optional<sdf::SemanticPose> SDFSemanticPose()
const override;
481 protected:
virtual void OnPoseChange()
override;
484 protected:
virtual void RegisterIntrospectionItems()
override;
487 private:
void LoadLinks();
490 private:
void LoadModels();
494 private:
void LoadJoint(sdf::ElementPtr _sdf);
498 private:
void LoadPlugin(sdf::ElementPtr _sdf);
502 private:
void LoadGripper(sdf::ElementPtr _sdf);
507 private:
void RemoveLink(
const std::string &_name);
510 private:
virtual void PublishScale();
522 private:
LinkPtr canonicalLink;
534 private: std::vector<GripperPtr> grippers;
537 private: std::vector<ModelPluginPtr> plugins;
540 private: std::map<std::string, common::NumericAnimationPtr>
544 private: boost::function<void()> onJointAnimationComplete;
550 private:
mutable boost::recursive_mutex updateMutex;
553 private: std::mutex receiveMutex;
558 private: std::unique_ptr<sdf::Model> modelSDFDomIsolated;
561 private:
const sdf::Model *modelSDFDom =
nullptr;
std::vector< JointPtr > Joint_V
Definition: PhysicsTypes.hh:214
virtual ignition::math::Vector3d WorldLinearVel() const override
Get the linear velocity of the entity in the world frame.
void Load(sdf::ElementPtr _sdf) override
Load the model.
A complete URI.
Definition: URI.hh:176
void LoadJoints()
Load all the joints.
double GetWorldEnergyPotential() const
Returns the potential energy of all links and joint springs in the model.
virtual ignition::math::Vector3d WorldAngularVel() const override
Get the angular velocity of the entity in the world frame.
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.
void AttachStaticModel(ModelPtr &_model, ignition::math::Pose3d _offset)
Attach a static model to this model.
void Reset() override
Reset the model.
virtual ~Model()
Destructor.
Forward declarations for the common classes.
Definition: Animation.hh:26
virtual ignition::math::Vector3d WorldAngularAccel() const override
Get the angular acceleration of the entity in the world frame.
virtual void OnPoseChange() override
Callback when the pose of the model has been changed.
boost::shared_ptr< JointController > JointControllerPtr
Definition: PhysicsTypes.hh:122
virtual bool GetSelfCollide() const
If true, all links within the model will collide by default.
Model(BasePtr _parent)
Constructor.
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
#define NULL
Definition: CommonTypes.hh:31
std::vector< ModelPtr > Model_V
Definition: PhysicsTypes.hh:206
void SetCollideMode(const std::string &_mode)
\TODO This is not implemented in Link, which means this function doesn't do anything.
virtual void StopAnimation() override
Stop the current animations.
boost::shared_ptr< Publisher > PublisherPtr
Definition: TransportTypes.hh:49
virtual void Init() override
Initialize the model.
virtual bool WindMode() const
Get the wind mode.
virtual ignition::math::Vector3d RelativeAngularVel() const override
Get the angular velocity of the entity.
virtual void RegisterIntrospectionItems() override
Register items in the introspection service.
Forward declarations for transport.
void SetJointAnimation(const std::map< std::string, common::NumericAnimationPtr > &_anims, boost::function< void()> _onComplete=NULL)
Joint Animation.
Definition: JointMaker.hh:44
size_t GetGripperCount() const
Get the number of grippers in this model.
virtual void SetWindMode(const bool _mode)
Set whether wind affects this body.
void SetLaserRetro(const float _retro)
Set the laser retro reflectiveness of the model.
double GetWorldEnergyKinetic() const
Returns sum of the kinetic energies of all links in this model.
unsigned int GetPluginCount() const
Get the number of plugins this model has.
virtual void Reset()
Reset the entity.
default namespace for gazebo
std::optional< sdf::SemanticPose > SDFSemanticPose() const override
Get the SDF SemanticPose object associated with the pose of this object.
virtual void UpdateParameters(sdf::ElementPtr _sdf) override
Update the parameters using new sdf values.
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.
JointPtr GetJoint(const std::string &name)
Get a joint.
double GetWorldEnergy() const
Returns this model's total energy, or sum of Model::GetWorldEnergyPotential() and Model::GetWorldEner...
void SetJointPositions(const std::map< std::string, double > &_jointPositions)
Set the positions of a set of joints.
void SetAngularVel(const ignition::math::Vector3d &_vel)
Set the angular velocity of the model, and all its links.
std::vector< std::string > SensorScopedName(const std::string &_name) const
Get scoped sensor name(s) in the model that matches sensor name.
GripperPtr GetGripper(size_t _index) const
Get a gripper based on an index.
virtual ignition::math::Vector3d WorldLinearAccel() const override
Get the linear acceleration of the entity in the world frame.
boost::shared_ptr< Model > shared_from_this()
Allow Model class to share itself as a boost shared_ptr.
JointControllerPtr GetJointController()
Get a handle to the Controller for the joints in this model.
virtual void RemoveChild(unsigned int _id)
Remove a child from this entity.
virtual void FillMsg(msgs::Model &_msg)
Fill a model message.
void ResetPhysicsStates()
Reset the velocity, acceleration, force and torque of all child links.
A model is a collection of links, joints, and plugins.
Definition: Model.hh:59
Store state information of a physics::Model object.
Definition: ModelState.hh:48
unsigned int GetJointCount() const
Get the number of joints.
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.
boost::shared_ptr< Base > BasePtr
Definition: PhysicsTypes.hh:78
LinkPtr CreateLink(const std::string &_name)
Create a new link for this model.
void SetGravityMode(const bool &_value)
Set the gravity mode of the model.
void Update() override
Update the model.
GAZEBO_VISIBLE void Init(google::protobuf::Message &_message, const std::string &_id="")
Initialize a message.
virtual const sdf::ElementPtr GetSDF() override
Get the SDF values for the model.
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:110
virtual ignition::math::Vector3d RelativeLinearVel() const override
Get the linear velocity of the entity.
void SetAutoDisable(bool _disable)
Allow the model the auto disable.
ignition::math::Vector3d Scale() const
Get the scale of model.
virtual bool RemoveJoint(const std::string &_name)
Remove a joint for this model.
std::vector< ModelPtr > attachedModels
used by Model::AttachStaticModel
Definition: Model.hh:513
ModelPtr NestedModel(const std::string &_name) const
Get a nested model that is a direct child of this model.
void LoadPlugins()
Load all plugins.
void SetJointPosition(const std::string &_jointName, double _position, int _index=0)
Set the positions of a Joint by name.
boost::shared_ptr< Entity > EntityPtr
Definition: PhysicsTypes.hh:86
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:118
Base class for all physics objects in Gazebo.
Definition: Entity.hh:52
bool GetAutoDisable() const
Return the value of the SDF <allow_auto_disable> element.
void SetState(const ModelState &_state)
Set the current model state.
virtual ignition::math::Vector3d RelativeLinearAccel() const override
Get the linear acceleration of the entity.
std::vector< ignition::math::Pose3d > attachedModelsOffset
used by Model::AttachStaticModel
Definition: Model.hh:516
std::vector< LinkPtr > Link_V
Definition: PhysicsTypes.hh:226
virtual ignition::math::Vector3d RelativeAngularAccel() const override
Get the angular acceleration of the entity.
virtual const sdf::ElementPtr UnscaledSDF()
virtual void Fini() override
Finalize the model.
virtual ignition::math::AxisAlignedBox BoundingBox() const override
Get the size of the bounding box.
void SetEnabled(bool _enabled)
Enable all the links in all the models.
virtual void SetSelfCollide(bool _self_collide)
Set this model's self_collide property.
boost::shared_ptr< Gripper > GripperPtr
Definition: PhysicsTypes.hh:198
virtual void RemoveChild(EntityPtr _child)
Remove a child.
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...
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.
const sdf::Model * GetSDFDom() const
Get the SDF DOM for the model.
void SetScale(const ignition::math::Vector3d &_scale, const bool _publish=false)
Set the scale of model.
void DetachStaticModel(const std::string &_model)
Detach a static model from this model.
transport::PublisherPtr jointPub
Publisher for joint info.
Definition: Model.hh:519
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...
const Model_V & NestedModels() const
Get all the nested models.
void ProcessMsg(const msgs::Model &_msg)
Update parameters from a model message.