Gazebo

namespace scenario::gazebo

Enums

enum PhysicsEngine

Supported physics engines.

Values:

enumerator Dart

The physics engine included in the Dynamic Animation and Robotics Toolkit.

enumerator TPE

The Trivial Physics Engine, a kinematics-only physics engine developed by Open Robotics.

enumerator Dart

The physics engine included in the Dynamic Animation and Robotics Toolkit.

enumerator TPE

The Trivial Physics Engine, a kinematics-only physics engine developed by Open Robotics.

enum PhysicsEngine

Supported physics engines.

Values:

enumerator Dart

The physics engine included in the Dynamic Animation and Robotics Toolkit.

enumerator TPE

The Trivial Physics Engine, a kinematics-only physics engine developed by Open Robotics.

enumerator Dart

The physics engine included in the Dynamic Animation and Robotics Toolkit.

enumerator TPE

The Trivial Physics Engine, a kinematics-only physics engine developed by Open Robotics.

class GazeboEntity

Subclassed by scenario::gazebo::Joint, scenario::gazebo::Link, scenario::gazebo::Model, scenario::gazebo::World

Public Functions

virtual uint64_t id() const = 0

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity linkEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) = 0

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() = 0

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

inline ignition::gazebo::Entity entity() const

Return the entity of this object.

Returns

The entity that corresponds to this object.

inline ignition::gazebo::EventManager *eventManager() const

Return the pointer to the event manager.

Returns

The pointer to the event manager.

inline ignition::gazebo::EntityComponentManager *ecm() const

Return the pointer to the Entity Component Manager.

Returns

The pointer to the Entity Component Manager.

inline bool validEntity() const

Checks if the GazeboEntity is valid.

Returns

True if the GazeboEntity is valid, false otherwise.

virtual uint64_t id() const = 0

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity linkEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) = 0

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() = 0

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

inline ignition::gazebo::Entity entity() const

Return the entity of this object.

Returns

The entity that corresponds to this object.

inline ignition::gazebo::EventManager *eventManager() const

Return the pointer to the event manager.

Returns

The pointer to the event manager.

inline ignition::gazebo::EntityComponentManager *ecm() const

Return the pointer to the Entity Component Manager.

Returns

The pointer to the Entity Component Manager.

inline bool validEntity() const

Checks if the GazeboEntity is valid.

Returns

True if the GazeboEntity is valid, false otherwise.

class GazeboSimulator

Public Functions

GazeboSimulator(const double stepSize = 0.001, const double rtf = 1.0, const size_t stepsPerRun = 1)

Class wrapping the Ignition Gazebo simulator.

The simulator can execute either unpaused or paused runs. Paused runs update the internal state of the simulator without stepping the physics. This is useful to process e.g. model insertions or removals. Every simulator run can execute one or more physics steps, depending on how it is configured when constructed.

Parameters
  • stepSize – The size of the physics step.

  • rtf – The desired real-time factor.

  • stepsPerRun – Number of steps to execute at each simulator run.

double stepSize() const

Get the size of a simulator step.

Returns

The simulator step size in seconds.

double realTimeFactor() const

Get the desired real-time factor of the simulator.

Returns

The desired real-time factor.

size_t stepsPerRun() const

Get the number or steps to execute every simulator run.

Returns

The configured number of steps per run.

bool initialize()

Initialize the simulator.

Returns

True for success, false otherwise.

bool initialized() const

Check if the simulator has been initialized.

Returns

True if the simulator was initialized, false otherwise.

bool run(const bool paused = false)

Run the simulator.

Parameters

paused – True to perform paused steps that do not affect the physics, false for normal steps. The number of steps configured during construction are executed.

Returns

True for success, false otherwise.

bool gui(const int verbosity = -1)

Open the Ignition Gazebo GUI.

Parameters

verbosity – Configure the verbosity of the GUI (0-4)

Returns

True for success, false otherwise.

bool close()

Close the simulator and the GUI.

Returns

True for success, false otherwise.

bool pause()

Pause the simulator.

Note

This method is useful in non-deterministic mode, which is not currently supported.

Returns

True for success, false otherwise.

bool running() const

Check if the simulator is running.

Note

This method is useful in non-deterministic mode, which is not currently supported.

Returns

True for success, false otherwise.

bool insertWorldFromSDF(const std::string &worldFile = "", const std::string &worldName = "")

Load a SDF world file.

Note

If the world file is not passed, the default empty world is inserted. The default empty world does not have the ground plane nor any physics. Both can be added by operating on the World object.

Note

This function can only be used while the simulator object is uninitialized.

Parameters
  • worldFile – The path to the SDF world file.

  • worldName – Optionally override the name of the world defined in the SDF world file.

Returns

True for success, false otherwise.

bool insertWorldsFromSDF(const std::string &worldFile, const std::vector<std::string> &worldNames = {})

Load a SDF world file containing multiple worlds.

Warning

This is an experimental feature. Multiworld simulations are only partially supported by Ignition Gazebo.

Parameters
  • worldFile – The path to the SDF world file.

  • worldNames – Optionally override the names of the worlds defined in the SDF world file.

Returns

True for success, false otherwise.

std::vector<std::string> worldNames() const

Get the list if the world names part of the simulation.

Returns

The world names.

std::shared_ptr<scenario::gazebo::World> getWorld(const std::string &worldName = "") const

Get a simulated world.

Parameters

worldName – The name of the desired world.

Returns

The world object.

GazeboSimulator(const double stepSize = 0.001, const double rtf = 1.0, const size_t stepsPerRun = 1)

Class wrapping the Ignition Gazebo simulator.

The simulator can execute either unpaused or paused runs. Paused runs update the internal state of the simulator without stepping the physics. This is useful to process e.g. model insertions or removals. Every simulator run can execute one or more physics steps, depending on how it is configured when constructed.

Parameters
  • stepSize – The size of the physics step.

  • rtf – The desired real-time factor.

  • stepsPerRun – Number of steps to execute at each simulator run.

double stepSize() const

Get the size of a simulator step.

Returns

The simulator step size in seconds.

double realTimeFactor() const

Get the desired real-time factor of the simulator.

Returns

The desired real-time factor.

size_t stepsPerRun() const

Get the number or steps to execute every simulator run.

Returns

The configured number of steps per run.

bool initialize()

Initialize the simulator.

Returns

True for success, false otherwise.

bool initialized() const

Check if the simulator has been initialized.

Returns

True if the simulator was initialized, false otherwise.

bool run(const bool paused = false)

Run the simulator.

Parameters

paused – True to perform paused steps that do not affect the physics, false for normal steps. The number of steps configured during construction are executed.

Returns

True for success, false otherwise.

bool gui(const int verbosity = -1)

Open the Ignition Gazebo GUI.

Parameters

verbosity – Configure the verbosity of the GUI (0-4)

Returns

True for success, false otherwise.

bool close()

Close the simulator and the GUI.

Returns

True for success, false otherwise.

bool pause()

Pause the simulator.

Note

This method is useful in non-deterministic mode, which is not currently supported.

Returns

True for success, false otherwise.

bool running() const

Check if the simulator is running.

Note

This method is useful in non-deterministic mode, which is not currently supported.

Returns

True for success, false otherwise.

bool insertWorldFromSDF(const std::string &worldFile = "", const std::string &worldName = "")

Load a SDF world file.

Note

If the world file is not passed, the default empty world is inserted. The default empty world does not have the ground plane nor any physics. Both can be added by operating on the World object.

Note

This function can only be used while the simulator object is uninitialized.

Parameters
  • worldFile – The path to the SDF world file.

  • worldName – Optionally override the name of the world defined in the SDF world file.

Returns

True for success, false otherwise.

bool insertWorldsFromSDF(const std::string &worldFile, const std::vector<std::string> &worldNames = {})

Load a SDF world file containing multiple worlds.

Warning

This is an experimental feature. Multiworld simulations are only partially supported by Ignition Gazebo.

Parameters
  • worldFile – The path to the SDF world file.

  • worldNames – Optionally override the names of the worlds defined in the SDF world file.

Returns

True for success, false otherwise.

std::vector<std::string> worldNames() const

Get the list if the world names part of the simulation.

Returns

The world names.

std::shared_ptr<scenario::gazebo::World> getWorld(const std::string &worldName = "") const

Get a simulated world.

Parameters

worldName – The name of the desired world.

Returns

The world object.

class Joint : public scenario::core::Joint, public scenario::gazebo::GazeboEntity, public std::enable_shared_from_this<scenario::gazebo::Joint>

Public Functions

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity jointEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertJointPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the joint.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool resetPosition(const double position = 0, const size_t dof = 0)

Reset the position of a joint DOF.

Parameters
  • position – The desired position.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool resetVelocity(const double velocity = 0, const size_t dof = 0)

Reset the velocity of a joint DOF.

Parameters
  • velocity – The desired velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool reset(const double position = 0, const double velocity = 0, const size_t dof = 0)

Reset the state of a joint DOF.

This method also resets the PID state of the joint.

Parameters
  • position – The desired position.

  • velocity – The desired velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool resetJointPosition(const std::vector<double> &position)

Reset the position of the joint.

This method also resets the PID state of the joint.

Parameters

position – The desired position.

Returns

True for success, false otherwise.

bool resetJointVelocity(const std::vector<double> &velocity)

Reset the velocity of the joint.

This method also resets the PID state of the joint.

Parameters

velocity – The desired velocity.

Returns

True for success, false otherwise.

bool resetJoint(const std::vector<double> &position, const std::vector<double> &velocity)

Reset the state of the joint.

This method also resets the PID state of the joint.

Parameters
  • position – The desired position.

  • velocity – The desired velocity.

Returns

True for success, false otherwise.

bool setCoulombFriction(const double value)

Set the Coulomb friction parameter of the joint.

Note

Friction can be changed only before the first simulated step after model insertion.

Parameters

value – The new Coulomb friction value.

Returns

True for success, false otherwise.

bool setViscousFriction(const double value)

Set the viscous friction parameter of the joint.

Note

Friction can be changed only before the first simulated step after model insertion.

Parameters

value – The new viscous friction value.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the joint is valid.

Returns

True if the joint is valid, false otherwise.

virtual size_t dofs() const override

Get the number of degrees of freedom of the joint.

Returns

The number of DOFs of the joint.

virtual std::string name(const bool scoped = false) const override

Get the name of the joint.

Parameters

scoped – If true, the scoped name of the joint is returned.

Returns

The name of the joint.

virtual core::JointType type() const override

Get the type of the joint.

Returns

The type of the joint.

virtual core::JointControlMode controlMode() const override

Get the active joint control mode.

Returns

The active joint control mode.

virtual bool setControlMode(const core::JointControlMode mode) override

Set the joint control mode.

Parameters

mode – The desired control mode.

Returns

True for success, false otherwise.

virtual double controllerPeriod() const override

Get the period of the controller, if any.

The controller period is a model quantity. If no controller is active, infinity is returned.

Returns

The the controller period.

virtual core::PID pid() const override

Get the PID parameters of the joint.

If no PID parameters have been set, the default parameters are returned.

Returns

The joint PID parameters.

virtual bool setPID(const core::PID &pid) override

Set the PID parameters of the joint.

Parameters

pid – The desired PID parameters.

Returns

True for success, false otherwise.

virtual bool historyOfAppliedJointForcesEnabled() const override

Check if the history of applied joint forces is enabled.

Returns

True if the history is enabled, false otherwise.

virtual bool enableHistoryOfAppliedJointForces(const bool enable = true, const size_t maxHistorySize = 100) override

Enable the history of joint forces.

Parameters
  • enable – True to enable, false to disable.

  • maxHistorySize – The size of the history window.

Returns

True for success, false otherwise.

virtual std::vector<double> historyOfAppliedJointForces() const override

Get the history of applied joint forces.

The vector is populated with #DoFs values at each physics step.

Returns

The vector containing the history of joint forces.

virtual double coulombFriction() const override

Get the Coulomb friction of the joint.

If \( K_c \) is the Coulomb friction parameter, and \( \dot{q} \) the joint velocity, the corresponding torque is often modelled as:

\( \tau_{static} = sign(\dot{q}) K_c \)

Returns

The Coulomb friction parameter of the joint.

virtual double viscousFriction() const override

Get the viscous friction of the joint.

If \( K_v \) is the viscous friction parameter, and \( \dot{q} \) the joint velocity, the corresponding torque is often modelled as:

\( \tau_{static} = K_v \dot{q} \)

Returns

The viscous friction parameter of the joint.

virtual core::Limit positionLimit(const size_t dof = 0) const override

Get the position limits of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The position limits of the joint DOF.

virtual core::Limit velocityLimit(const size_t dof = 0) const override

Get the velocity limit of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The velocity limit of the joint DOF.

virtual bool setVelocityLimit(const double maxVelocity, const size_t dof = 0) override

Set the maximum velocity of a joint DOF.

This limit can be used to clip the velocity applied by joint controllers.

Parameters
  • maxVelocity – The maximum velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double maxGeneralizedForce(const size_t dof = 0) const override

Get the maximum generalized force that could be applied to a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The maximum generalized force of the joint DOF.

virtual bool setMaxGeneralizedForce(const double maxForce, const size_t dof = 0) override

Set the maximum generalized force that can be applied to a joint DOF.

This limit can be used to clip the force applied by joint controllers.

Parameters
  • maxForce – The maximum generalized force.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double position(const size_t dof = 0) const override

Get the position of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The position of the joint DOF.

virtual double velocity(const size_t dof = 0) const override

Get the velocity of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The velocity of the joint DOF.

virtual double acceleration(const size_t dof = 0) const override

Get the acceleration of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The acceleration of the joint DOF.

virtual double generalizedForce(const size_t dof = 0) const override

Get the generalized force of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The generalized force of the joint DOF.

virtual bool setPositionTarget(const double position, const size_t dof = 0) override

Set the position target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • position – The position target of the joint DOF.

  • dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

True for success, false otherwise.

virtual bool setVelocityTarget(const double velocity, const size_t dof = 0) override

Set the velocity target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • velocity – The velocity target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual bool setAccelerationTarget(const double acceleration, const size_t dof = 0) override

Set the acceleration target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • acceleration – The acceleration target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual bool setGeneralizedForceTarget(const double force, const size_t dof = 0) override

Set the generalized force target of a joint DOF.

The force is applied to the desired DOF. Note that if there’s friction or other loss components, the real joint force will differ.

Parameters
  • force – The generalized force target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double positionTarget(const size_t dof = 0) const override

Get the active position target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no position target was set.

Returns

The position target of the joint DOF.

virtual double velocityTarget(const size_t dof = 0) const override

Get the active velocity target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no velocity target was set.

Returns

The velocity target of the joint DOF.

virtual double accelerationTarget(const size_t dof = 0) const override

Get the active acceleration target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no acceleration target was set.

Returns

The acceleration target of the joint DOF.

virtual double generalizedForceTarget(const size_t dof = 0) const override

Get the active generalized force target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no generalized force target was set.

Returns

The generalized force target of the joint DOF.

virtual core::JointLimit jointPositionLimit() const override

Get the position limits of the joint.

Returns

The position limits of the joint.

virtual core::JointLimit jointVelocityLimit() const override

Get the velocity limits of the joint.

Returns

The velocity limits of the joint.

virtual bool setJointVelocityLimit(const std::vector<double> &maxVelocity) override

Set the maximum velocity of the joint.

This limit can be used to clip the velocity applied by joint controllers.

Parameters

maxVelocity – A vector with the maximum velocity of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointMaxGeneralizedForce() const override

Get the maximum generalized force that could be applied to the joint.

Returns

The maximum generalized force of the joint.

virtual bool setJointMaxGeneralizedForce(const std::vector<double> &maxForce) override

Set the maximum generalized force that can be applied to the joint.

This limit can be used to clip the force applied by joint controllers.

Parameters

maxForce – A vector with the maximum generalized forces of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPosition() const override

Get the position of the joint.

Returns

The position of the joint.

virtual std::vector<double> jointVelocity() const override

Get the velocity of the joint.

Returns

The velocity of the joint.

virtual std::vector<double> jointAcceleration() const override

Get the acceleration of the joint.

Returns

The acceleration of the joint.

virtual std::vector<double> jointGeneralizedForce() const override

Get the generalized force of the joint.

Returns

The generalized force of the joint.

virtual bool setJointPositionTarget(const std::vector<double> &position) override

Set the position target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

position – A vector with the position targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointVelocityTarget(const std::vector<double> &velocity) override

Set the velocity target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

velocity – A vector with the velocity targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointAccelerationTarget(const std::vector<double> &acceleration) override

Set the acceleration target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

acceleration – A vector with the acceleration targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointGeneralizedForceTarget(const std::vector<double> &force) override

Set the generalized force target of the joint.

Note that if there’s friction or other loss components, the real joint force will differ.

Parameters

force – A vector with the generalized force targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPositionTarget() const override

Get the active position target.

Returns

The position target of the joint.

virtual std::vector<double> jointVelocityTarget() const override

Get the active velocity target.

Returns

The velocity target of the joint.

virtual std::vector<double> jointAccelerationTarget() const override

Get the active acceleration target.

Returns

The acceleration target of the joint.

virtual std::vector<double> jointGeneralizedForceTarget() const override

Get the active generalized force target.

Returns

The generalized force target of the joint.

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity jointEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertJointPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the joint.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool resetPosition(const double position = 0, const size_t dof = 0)

Reset the position of a joint DOF.

Parameters
  • position – The desired position.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool resetVelocity(const double velocity = 0, const size_t dof = 0)

Reset the velocity of a joint DOF.

Parameters
  • velocity – The desired velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool reset(const double position = 0, const double velocity = 0, const size_t dof = 0)

Reset the state of a joint DOF.

This method also resets the PID state of the joint.

Parameters
  • position – The desired position.

  • velocity – The desired velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

bool resetJointPosition(const std::vector<double> &position)

Reset the position of the joint.

This method also resets the PID state of the joint.

Parameters

position – The desired position.

Returns

True for success, false otherwise.

bool resetJointVelocity(const std::vector<double> &velocity)

Reset the velocity of the joint.

This method also resets the PID state of the joint.

Parameters

velocity – The desired velocity.

Returns

True for success, false otherwise.

bool resetJoint(const std::vector<double> &position, const std::vector<double> &velocity)

Reset the state of the joint.

This method also resets the PID state of the joint.

Parameters
  • position – The desired position.

  • velocity – The desired velocity.

Returns

True for success, false otherwise.

bool setCoulombFriction(const double value)

Set the Coulomb friction parameter of the joint.

Note

Friction can be changed only before the first simulated step after model insertion.

Parameters

value – The new Coulomb friction value.

Returns

True for success, false otherwise.

bool setViscousFriction(const double value)

Set the viscous friction parameter of the joint.

Note

Friction can be changed only before the first simulated step after model insertion.

Parameters

value – The new viscous friction value.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the joint is valid.

Returns

True if the joint is valid, false otherwise.

virtual size_t dofs() const override

Get the number of degrees of freedom of the joint.

Returns

The number of DOFs of the joint.

virtual std::string name(const bool scoped = false) const override

Get the name of the joint.

Parameters

scoped – If true, the scoped name of the joint is returned.

Returns

The name of the joint.

virtual core::JointType type() const override

Get the type of the joint.

Returns

The type of the joint.

virtual core::JointControlMode controlMode() const override

Get the active joint control mode.

Returns

The active joint control mode.

virtual bool setControlMode(const core::JointControlMode mode) override

Set the joint control mode.

Parameters

mode – The desired control mode.

Returns

True for success, false otherwise.

virtual double controllerPeriod() const override

Get the period of the controller, if any.

The controller period is a model quantity. If no controller is active, infinity is returned.

Returns

The the controller period.

virtual core::PID pid() const override

Get the PID parameters of the joint.

If no PID parameters have been set, the default parameters are returned.

Returns

The joint PID parameters.

virtual bool setPID(const core::PID &pid) override

Set the PID parameters of the joint.

Parameters

pid – The desired PID parameters.

Returns

True for success, false otherwise.

virtual bool historyOfAppliedJointForcesEnabled() const override

Check if the history of applied joint forces is enabled.

Returns

True if the history is enabled, false otherwise.

virtual bool enableHistoryOfAppliedJointForces(const bool enable = true, const size_t maxHistorySize = 100) override

Enable the history of joint forces.

Parameters
  • enable – True to enable, false to disable.

  • maxHistorySize – The size of the history window.

Returns

True for success, false otherwise.

virtual std::vector<double> historyOfAppliedJointForces() const override

Get the history of applied joint forces.

The vector is populated with #DoFs values at each physics step.

Returns

The vector containing the history of joint forces.

virtual double coulombFriction() const override

Get the Coulomb friction of the joint.

If \( K_c \) is the Coulomb friction parameter, and \( \dot{q} \) the joint velocity, the corresponding torque is often modelled as:

\( \tau_{static} = sign(\dot{q}) K_c \)

Returns

The Coulomb friction parameter of the joint.

virtual double viscousFriction() const override

Get the viscous friction of the joint.

If \( K_v \) is the viscous friction parameter, and \( \dot{q} \) the joint velocity, the corresponding torque is often modelled as:

\( \tau_{static} = K_v \dot{q} \)

Returns

The viscous friction parameter of the joint.

virtual core::Limit positionLimit(const size_t dof = 0) const override

Get the position limits of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The position limits of the joint DOF.

virtual core::Limit velocityLimit(const size_t dof = 0) const override

Get the velocity limit of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The velocity limit of the joint DOF.

virtual bool setVelocityLimit(const double maxVelocity, const size_t dof = 0) override

Set the maximum velocity of a joint DOF.

This limit can be used to clip the velocity applied by joint controllers.

Parameters
  • maxVelocity – The maximum velocity.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double maxGeneralizedForce(const size_t dof = 0) const override

Get the maximum generalized force that could be applied to a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The maximum generalized force of the joint DOF.

virtual bool setMaxGeneralizedForce(const double maxForce, const size_t dof = 0) override

Set the maximum generalized force that can be applied to a joint DOF.

This limit can be used to clip the force applied by joint controllers.

Parameters
  • maxForce – The maximum generalized force.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double position(const size_t dof = 0) const override

Get the position of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The position of the joint DOF.

virtual double velocity(const size_t dof = 0) const override

Get the velocity of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The velocity of the joint DOF.

virtual double acceleration(const size_t dof = 0) const override

Get the acceleration of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The acceleration of the joint DOF.

virtual double generalizedForce(const size_t dof = 0) const override

Get the generalized force of a joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

The generalized force of the joint DOF.

virtual bool setPositionTarget(const double position, const size_t dof = 0) override

Set the position target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • position – The position target of the joint DOF.

  • dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid.

Returns

True for success, false otherwise.

virtual bool setVelocityTarget(const double velocity, const size_t dof = 0) override

Set the velocity target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • velocity – The velocity target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual bool setAccelerationTarget(const double acceleration, const size_t dof = 0) override

Set the acceleration target of a joint DOF.

The target is processed by a joint controller, if enabled.

Parameters
  • acceleration – The acceleration target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual bool setGeneralizedForceTarget(const double force, const size_t dof = 0) override

Set the generalized force target of a joint DOF.

The force is applied to the desired DOF. Note that if there’s friction or other loss components, the real joint force will differ.

Parameters
  • force – The generalized force target of the joint DOF.

  • dof – The index of the DOF.

Returns

True for success, false otherwise.

virtual double positionTarget(const size_t dof = 0) const override

Get the active position target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no position target was set.

Returns

The position target of the joint DOF.

virtual double velocityTarget(const size_t dof = 0) const override

Get the active velocity target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no velocity target was set.

Returns

The velocity target of the joint DOF.

virtual double accelerationTarget(const size_t dof = 0) const override

Get the active acceleration target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no acceleration target was set.

Returns

The acceleration target of the joint DOF.

virtual double generalizedForceTarget(const size_t dof = 0) const override

Get the active generalized force target of the joint DOF.

Parameters

dof – The index of the DOF.

Throws

std::runtime_error – if the DOF is not valid or if no generalized force target was set.

Returns

The generalized force target of the joint DOF.

virtual core::JointLimit jointPositionLimit() const override

Get the position limits of the joint.

Returns

The position limits of the joint.

virtual core::JointLimit jointVelocityLimit() const override

Get the velocity limits of the joint.

Returns

The velocity limits of the joint.

virtual bool setJointVelocityLimit(const std::vector<double> &maxVelocity) override

Set the maximum velocity of the joint.

This limit can be used to clip the velocity applied by joint controllers.

Parameters

maxVelocity – A vector with the maximum velocity of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointMaxGeneralizedForce() const override

Get the maximum generalized force that could be applied to the joint.

Returns

The maximum generalized force of the joint.

virtual bool setJointMaxGeneralizedForce(const std::vector<double> &maxForce) override

Set the maximum generalized force that can be applied to the joint.

This limit can be used to clip the force applied by joint controllers.

Parameters

maxForce – A vector with the maximum generalized forces of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPosition() const override

Get the position of the joint.

Returns

The position of the joint.

virtual std::vector<double> jointVelocity() const override

Get the velocity of the joint.

Returns

The velocity of the joint.

virtual std::vector<double> jointAcceleration() const override

Get the acceleration of the joint.

Returns

The acceleration of the joint.

virtual std::vector<double> jointGeneralizedForce() const override

Get the generalized force of the joint.

Returns

The generalized force of the joint.

virtual bool setJointPositionTarget(const std::vector<double> &position) override

Set the position target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

position – A vector with the position targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointVelocityTarget(const std::vector<double> &velocity) override

Set the velocity target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

velocity – A vector with the velocity targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointAccelerationTarget(const std::vector<double> &acceleration) override

Set the acceleration target of the joint.

The target is processed by a joint controller, if enabled.

Parameters

acceleration – A vector with the acceleration targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual bool setJointGeneralizedForceTarget(const std::vector<double> &force) override

Set the generalized force target of the joint.

Note that if there’s friction or other loss components, the real joint force will differ.

Parameters

force – A vector with the generalized force targets of the joint DOFs.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPositionTarget() const override

Get the active position target.

Returns

The position target of the joint.

virtual std::vector<double> jointVelocityTarget() const override

Get the active velocity target.

Returns

The velocity target of the joint.

virtual std::vector<double> jointAccelerationTarget() const override

Get the active acceleration target.

Returns

The acceleration target of the joint.

virtual std::vector<double> jointGeneralizedForceTarget() const override

Get the active generalized force target.

Returns

The generalized force target of the joint.

class Link : public scenario::core::Link, public scenario::gazebo::GazeboEntity, public std::enable_shared_from_this<scenario::gazebo::Link>

Public Functions

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity linkEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertLinkPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the link.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the link is valid.

Returns

True if the link is valid, false otherwise.

virtual std::string name(const bool scoped = false) const override

Get the name of the link.

Parameters

scoped – If true, the scoped name of the link is returned.

Returns

The name of the link.

virtual double mass() const override

Get the mass of the link.

Returns

The mass of the link.

virtual std::array<double, 3> position() const override

Get the position of the link.

The returned position is the position of the link frame, as it was defined in the model file, in world coordinates.

Returns

The cartesian position of the link frame in world coordinates.

virtual std::array<double, 4> orientation() const override

Get the orientation of the link.

The orientation is returned as a quaternion, which defines the rotation between the world frame and the link frame.

Returns

The wxyz quaternion defining the orientation if the link wrt the world frame.

virtual std::array<double, 3> worldLinearVelocity() const override

Get the linear mixed velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed velocity of the link.

virtual std::array<double, 3> worldAngularVelocity() const override

Get the angular mixed velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed velocity of the link.

virtual std::array<double, 3> bodyLinearVelocity() const override

Get the linear body velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body velocity of the link.

virtual std::array<double, 3> bodyAngularVelocity() const override

Get the angular body velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body velocity of the link.

virtual std::array<double, 3> worldLinearAcceleration() const override

Get the linear mixed acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed acceleration of the link.

virtual std::array<double, 3> worldAngularAcceleration() const override

Get the angular mixed acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed acceleration of the link.

virtual std::array<double, 3> bodyLinearAcceleration() const override

Get the linear body acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body acceleration of the link.

virtual std::array<double, 3> bodyAngularAcceleration() const override

Get the angular body acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body acceleration of the link.

virtual bool contactsEnabled() const override

Check if the contact detection is enabled.

Returns

True if the contact detection is enabled, false otherwise.

virtual bool enableContactDetection(const bool enable) override

Enable the contact detection.

Parameters

enable – True to enable the contact detection, false to disable.

Returns

True for success, false otherwise.

virtual bool inContact() const override

Check if the link has active contacts.

Returns

True if the link has at least one contact and contacts are enabled, false otherwise.

virtual std::vector<core::Contact> contacts() const override

Get the active contacts of the link.

Returns

The vector of active contacts.

virtual std::array<double, 6> contactWrench() const override

Get the total wrench generated by the active contacts.

All the contact wrenches are composed to an equivalent wrench applied to the origin of the link frame and expressed in world coordinates.

Returns

The total wrench of the active contacts.

bool applyWorldForce(const std::array<double, 3> &force, const double duration = 0.0)

Apply a force to the link.

The force is applied to the origin of the link frame.

Parameters
  • force – The force to apply expressed in world coordinates.

  • duration – The duration of the application of the force. By default the force is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldTorque(const std::array<double, 3> &torque, const double duration = 0.0)

Apply a torque to the link.

The force is applied to the origin of the link frame.

Parameters
  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the torque. By default the torque is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldWrench(const std::array<double, 3> &force, const std::array<double, 3> &torque, const double duration = 0.0)

Apply a wrench to the link.

The force is applied to the origin of the link frame.

Parameters
  • force – The force to apply expressed in world coordinates.

  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the wrench. By default the wrench is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldWrenchToCoM(const std::array<double, 3> &force, const std::array<double, 3> &torque, const double duration = 0.0)

Apply a wrench to the CoM of the link.

Note

This method considers the CoM being positioned in the origin of the inertial frame as it is defined in the SDF description of the model. Note that if not explicitly specified, inertial and link frames match. In this case, applyWorldWrench and applyWorldWrenchToCoM effects will be the same.

Parameters
  • force – The force to apply expressed in world coordinates.

  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the wrench. By default the wrench is applied for a single physics step.

Returns

True for success, false otherwise.

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity linkEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertLinkPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the link.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the link is valid.

Returns

True if the link is valid, false otherwise.

virtual std::string name(const bool scoped = false) const override

Get the name of the link.

Parameters

scoped – If true, the scoped name of the link is returned.

Returns

The name of the link.

virtual double mass() const override

Get the mass of the link.

Returns

The mass of the link.

virtual std::array<double, 3> position() const override

Get the position of the link.

The returned position is the position of the link frame, as it was defined in the model file, in world coordinates.

Returns

The cartesian position of the link frame in world coordinates.

virtual std::array<double, 4> orientation() const override

Get the orientation of the link.

The orientation is returned as a quaternion, which defines the rotation between the world frame and the link frame.

Returns

The wxyz quaternion defining the orientation if the link wrt the world frame.

virtual std::array<double, 3> worldLinearVelocity() const override

Get the linear mixed velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed velocity of the link.

virtual std::array<double, 3> worldAngularVelocity() const override

Get the angular mixed velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed velocity of the link.

virtual std::array<double, 3> bodyLinearVelocity() const override

Get the linear body velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body velocity of the link.

virtual std::array<double, 3> bodyAngularVelocity() const override

Get the angular body velocity of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body velocity of the link.

virtual std::array<double, 3> worldLinearAcceleration() const override

Get the linear mixed acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed acceleration of the link.

virtual std::array<double, 3> worldAngularAcceleration() const override

Get the angular mixed acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed acceleration of the link.

virtual std::array<double, 3> bodyLinearAcceleration() const override

Get the linear body acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body acceleration of the link.

virtual std::array<double, 3> bodyAngularAcceleration() const override

Get the angular body acceleration of the link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body acceleration of the link.

virtual bool contactsEnabled() const override

Check if the contact detection is enabled.

Returns

True if the contact detection is enabled, false otherwise.

virtual bool enableContactDetection(const bool enable) override

Enable the contact detection.

Parameters

enable – True to enable the contact detection, false to disable.

Returns

True for success, false otherwise.

virtual bool inContact() const override

Check if the link has active contacts.

Returns

True if the link has at least one contact and contacts are enabled, false otherwise.

virtual std::vector<core::Contact> contacts() const override

Get the active contacts of the link.

Returns

The vector of active contacts.

virtual std::array<double, 6> contactWrench() const override

Get the total wrench generated by the active contacts.

All the contact wrenches are composed to an equivalent wrench applied to the origin of the link frame and expressed in world coordinates.

Returns

The total wrench of the active contacts.

bool applyWorldForce(const std::array<double, 3> &force, const double duration = 0.0)

Apply a force to the link.

The force is applied to the origin of the link frame.

Parameters
  • force – The force to apply expressed in world coordinates.

  • duration – The duration of the application of the force. By default the force is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldTorque(const std::array<double, 3> &torque, const double duration = 0.0)

Apply a torque to the link.

The force is applied to the origin of the link frame.

Parameters
  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the torque. By default the torque is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldWrench(const std::array<double, 3> &force, const std::array<double, 3> &torque, const double duration = 0.0)

Apply a wrench to the link.

The force is applied to the origin of the link frame.

Parameters
  • force – The force to apply expressed in world coordinates.

  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the wrench. By default the wrench is applied for a single physics step.

Returns

True for success, false otherwise.

bool applyWorldWrenchToCoM(const std::array<double, 3> &force, const std::array<double, 3> &torque, const double duration = 0.0)

Apply a wrench to the CoM of the link.

Note

This method considers the CoM being positioned in the origin of the inertial frame as it is defined in the SDF description of the model. Note that if not explicitly specified, inertial and link frames match. In this case, applyWorldWrench and applyWorldWrenchToCoM effects will be the same.

Parameters
  • force – The force to apply expressed in world coordinates.

  • torque – The torque to apply expressed in world coordinates.

  • duration – The duration of the application of the wrench. By default the wrench is applied for a single physics step.

Returns

True for success, false otherwise.

class Model : public scenario::core::Model, public scenario::gazebo::GazeboEntity, public std::enable_shared_from_this<scenario::gazebo::Model>

Public Functions

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity modelEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertModelPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the model.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool resetJointPositions(const std::vector<double> &positions, const std::vector<std::string> &jointNames = {})

Reset the positions of the joints.

Parameters
  • positions – The desired new joint positions.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

bool resetJointVelocities(const std::vector<double> &velocities, const std::vector<std::string> &jointNames = {})

Reset the velocities of the joints.

Parameters
  • velocities – The desired new velocities positions.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

bool resetBasePose(const std::array<double, 3> &position = {0, 0, 0}, const std::array<double, 4> &orientation = {0, 0, 0, 0})

Reset the pose of the base link.

Parameters
  • position – The desired position of the base link in world coordinates.

  • orientation – The wxyz quaternion defining the desired orientation of the base link wrt the world frame.

Returns

True for success, false otherwise.

bool resetBasePosition(const std::array<double, 3> &position = {0, 0, 0})

Reset the position of the base link.

Parameters

position – The desired position of the base link in world coordinates.

Returns

True for success, false otherwise.

bool resetBaseOrientation(const std::array<double, 4> &orientation = {0, 0, 0, 0})

Reset the orientation of the base link.

Parameters

orientation – The wxyz quaternion defining the desired orientation of the base link wrt the world frame.

Returns

True for success, false otherwise.

bool resetBaseWorldLinearVelocity(const std::array<double, 3> &linear = {0, 0, 0})

Reset the linear mixed velocity of the base link.

Parameters

linear – The desired linear mixed velocity of the base link.

Returns

True for success, false otherwise.

bool resetBaseWorldAngularVelocity(const std::array<double, 3> &angular = {0, 0, 0})

Reset the angular mixed velocity of the base link.

Parameters

angular – The desired angular mixed velocity of the base link.

Returns

True for success, false otherwise.

bool resetBaseWorldVelocity(const std::array<double, 3> &linear = {0, 0, 0}, const std::array<double, 3> &angular = {0, 0, 0})

Reset the mixed velocity of the base link.

Parameters
  • linear – The desired linear mixed velocity of the base link.

  • angular – The desired angular mixed velocity of the base link.

Returns

True for success, false otherwise.

bool selfCollisionsEnabled() const

Check if the detection of self-collisions is enabled.

Returns

True if self-collisions detection is enabled, false otherwise.

bool enableSelfCollisions(const bool enable = true)

Enable the detection of self-collisions.

It will enable contact detection if it was disabled.

Parameters

enable – True to enable the self-collision detection, false to disable.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the model is valid.

Returns

True if the model is valid, false otherwise.

virtual size_t dofs(const std::vector<std::string> &jointNames = {}) const override

Get the degrees of freedom of the model.

Parameters

jointNames – Optionally restrict the count to a subset of joints.

Returns

The number of degrees of freedom of the model.

virtual std::string name() const override

Get the name of the model.

Returns

The name of the model.

virtual size_t nrOfLinks() const override

Get the number of links of the model.

Returns

The number of links.

virtual size_t nrOfJoints() const override

Get the number of joints of the model.

Returns

The number of joints.

virtual double totalMass(const std::vector<std::string> &linkNames = {}) const override

Get the total mass of the model.

Parameters

linkNames – Optionally restrict the count to a subset of links.

Returns

The total mass of the model.

virtual core::LinkPtr getLink(const std::string &linkName) const override

Get a link belonging to the model.

Parameters

linkName – The name of the link.

Throws

std::runtime_error – if the link does not exist.

Returns

The desired link.

virtual core::JointPtr getJoint(const std::string &jointName) const override

Get a joint belonging to the model.

Parameters

jointName – The name of the joint.

Throws

std::runtime_error – if the joint does not exist.

Returns

The desired joint.

virtual std::vector<std::string> linkNames(const bool scoped = false) const override

Get the name of all the model’s links.

Parameters

scoped – Scope the link names with the model name (e.g. mymodel::link1).

Returns

The list of link names.

virtual std::vector<std::string> jointNames(const bool scoped = false) const override

Get the name of all the model’s joints.

Parameters

scoped – Scope the joint names with the model name, (e.g. mymodel::joint1).

Returns

The list of joint names.

virtual double controllerPeriod() const override

Get the controller period of the model.

If no controller has been enabled, infinite is returned.

Returns

The controller period of the model.

virtual bool setControllerPeriod(const double period) override

Set the controller period of the model.

This controller period is used by PIDs and custom controller. If it is smaller than the physics step, it is treated as 0.

Parameters

period – The desired controller period.

Returns

True for success, false otherwise.

virtual bool enableHistoryOfAppliedJointForces(const bool enable = true, const size_t maxHistorySizePerJoint = 100, const std::vector<std::string> &jointNames = {}) override

Enable logging the applied joint forces.

The output of joint controllers is often a torque. This method allows to log the force references that the controller sent to the joints. It is useful when the controller runs in its own thread at its own rate and the caller wants to extract the forces at a lower frequency.

Parameters
  • enable – True to enable logging, false to disable.

  • maxHistorySizePerJoint – Size of the logging window of each joint.

  • jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool historyOfAppliedJointForcesEnabled(const std::vector<std::string> &jointNames = {}) const override

Check if logging the applied joint force is enabled.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True if the log is enabled, false otherwise.

virtual std::vector<double> historyOfAppliedJointForces(const std::vector<std::string> &jointNames = {}) const override

Get the log of applied joint forces.

Note

Given a serialization, the window has DoFs * JointWindowSize elements. The elements are ordered per time steps, i.e. the first #DoFs elements refer to the oldest forces of the windows ordered with the active joint serialization.

Note

If a joint has multiple DoFs, they are serialized contiguously.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The entire window of applied joint forces.

virtual bool contactsEnabled() const override

Check if the contact detection is enabled model-wise.

Returns

True if the contact detection is enabled model-wise, false otherwise.

virtual bool enableContacts(const bool enable = true) override

Enable the contact detection model-wise.

Parameters

enable – True to enable the contact detection model-wise, false to disable.

Returns

True for success, false otherwise.

virtual std::vector<std::string> linksInContact() const override

Get the vector of links with active contacts with other bodies.

Returns

The vector of links in contact.

virtual std::vector<core::Contact> contacts(const std::vector<std::string> &linkNames = {}) const override

Get the active contacts of the model.

Parameters

linkNames – Optionally restrict the considered links.

Returns

A vector of contacts.

virtual std::vector<double> jointPositions(const std::vector<std::string> &jointNames = {}) const override

Get the joint positions.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint positions. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointVelocities(const std::vector<std::string> &jointNames = {}) const override

Get the joint velocities.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint velocities. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointAccelerations(const std::vector<std::string> &jointNames = {}) const override

Get the joint accelerations.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint accelerations. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointGeneralizedForces(const std::vector<std::string> &jointNames = {}) const override

Get the joint generalized forces.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint forces. The vector has as many elements as DoFs of the considered joints.

virtual core::JointLimit jointLimits(const std::vector<std::string> &jointNames = {}) const override

Get the joint limits of the model.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The joint limits of the model. The vectors of the limit object have as many elements as DoFs of the considered joints.

virtual bool setJointControlMode(const core::JointControlMode mode, const std::vector<std::string> &jointNames = {}) override

Set the control mode of model joints.

Parameters
  • mode – The desired joint control mode.

  • jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual std::vector<core::LinkPtr> links(const std::vector<std::string> &linkNames = {}) const override

Get the links of the model.

Parameters

linkNames – Optional vector of considered links. By default, Model::linkNames is used.

Returns

A vector of pointers to the link objects.

virtual std::vector<core::JointPtr> joints(const std::vector<std::string> &jointNames = {}) const override

Get the joints of the model.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

A vector of pointers to the joint objects.

virtual bool setJointPositionTargets(const std::vector<double> &positions, const std::vector<std::string> &jointNames = {}) override

Set the position targets of the joints.

Parameters
  • positions – The vector with the joint position targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointVelocityTargets(const std::vector<double> &velocities, const std::vector<std::string> &jointNames = {}) override

Set the velocity targets of the joints.

Parameters
  • velocities – The vector with the joint velocity targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointAccelerationTargets(const std::vector<double> &accelerations, const std::vector<std::string> &jointNames = {}) override

Set the acceleration targets of the joints.

Parameters
  • accelerations – The vector with the joint acceleration targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointGeneralizedForceTargets(const std::vector<double> &forces, const std::vector<std::string> &jointNames = {}) override

Set the generalized force targets of the joints.

Parameters
  • forces – The vector with the joint generalized force targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPositionTargets(const std::vector<std::string> &jointNames = {}) const override

Get the position targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The position targets of the joints.

virtual std::vector<double> jointVelocityTargets(const std::vector<std::string> &jointNames = {}) const override

Get the velocity targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The velocity targets of the joints.

virtual std::vector<double> jointAccelerationTargets(const std::vector<std::string> &jointNames = {}) const override

Get the acceleration targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The acceleration targets of the joints.

virtual std::vector<double> jointGeneralizedForceTargets(const std::vector<std::string> &jointNames = {}) const override

Get the generalized force targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The generalized force targets of the joints.

virtual std::string baseFrame() const override

Get the name of the model’s base frame.

By default, the base frame is typically the root of the kinematic tree of the model.

Returns

The name of the model’s base frame.

virtual std::array<double, 3> basePosition() const override

Get the position of the base link.

Returns

The position of the base link in world coordinates.

virtual std::array<double, 4> baseOrientation() const override

Get the orientation of the base link.

Returns

The wxyz quaternion defining the orientation of the base link wrt the world frame.

virtual std::array<double, 3> baseBodyLinearVelocity() const override

Get the linear body velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body velocity of the base link.

virtual std::array<double, 3> baseBodyAngularVelocity() const override

Get the angular body velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body velocity of the base link.

virtual std::array<double, 3> baseWorldLinearVelocity() const override

Get the linear mixed velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed velocity of the base link.

virtual std::array<double, 3> baseWorldAngularVelocity() const override

Get the angular mixed velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed velocity of the base link.

virtual bool setBasePoseTarget(const std::array<double, 3> &position, const std::array<double, 4> &orientation) override

Set the pose target of the base link.

Parameters
  • position – The position target of the base link in world coordinates.

  • orientation – The wxyz quaternion defining the orientation target of the base link wrt the world frame.

Returns

True for success, false otherwise.

virtual bool setBasePositionTarget(const std::array<double, 3> &position) override

Set the position target of the base link.

Parameters

position – The position target of the base link in world coordinates.

Returns

True for success, false otherwise.

virtual bool setBaseOrientationTarget(const std::array<double, 4> &orientation) override

Set the orientation target of the base link.

Parameters

orientation – The wxyz quaternion defining the orientation target of the base link wrt the world frame.

Returns

True for success, false otherwise.

virtual bool setBaseWorldVelocityTarget(const std::array<double, 3> &linear, const std::array<double, 3> &angular) override

Set the mixed velocity target of the base link.

Parameters
  • linear – The mixed linear velocity target of the base link.

  • angular – The mixed angular velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldLinearVelocityTarget(const std::array<double, 3> &linear) override

Set the mixed linear velocity target of the base link.

Parameters

linear – The mixed linear velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldAngularVelocityTarget(const std::array<double, 3> &angular) override

Set the mixed angular velocity target of the base link.

Parameters

angular – The mixed angular velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldLinearAccelerationTarget(const std::array<double, 3> &linear) override

Set the mixed linear acceleration target of the base link.

Parameters

linear – The mixed linear acceleration target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldAngularAccelerationTarget(const std::array<double, 3> &angular) override

Set the mixed angular acceleration target of the base link.

Parameters

angular – The mixed angular acceleration target of the base link.

Returns

True for success, false otherwise.

virtual std::array<double, 3> basePositionTarget() const override

Get the position target of the base link.

Returns

The position target of the base link.

virtual std::array<double, 4> baseOrientationTarget() const override

Get the orientation target of the base link.

Returns

The quaternion defining the orientation target of the base link.

virtual std::array<double, 3> baseWorldLinearVelocityTarget() const override

Get the mixed linear velocity target of the base link.

Returns

The mixed linear velocity target of the base link.

virtual std::array<double, 3> baseWorldAngularVelocityTarget() const override

Get the mixed angular velocity target of the base link.

Returns

The mixed angular velocity target of the base link.

virtual std::array<double, 3> baseWorldLinearAccelerationTarget() const override

Get the mixed linear acceleration target of the base link.

Returns

The mixed linear acceleration target of the base link.

virtual std::array<double, 3> baseWorldAngularAccelerationTarget() const override

Get the mixed angular acceleration target of the base link.

Returns

The mixed angular acceleration target of the base link.

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity modelEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertModelPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the model.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool resetJointPositions(const std::vector<double> &positions, const std::vector<std::string> &jointNames = {})

Reset the positions of the joints.

Parameters
  • positions – The desired new joint positions.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

bool resetJointVelocities(const std::vector<double> &velocities, const std::vector<std::string> &jointNames = {})

Reset the velocities of the joints.

Parameters
  • velocities – The desired new velocities positions.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

bool resetBasePose(const std::array<double, 3> &position = {0, 0, 0}, const std::array<double, 4> &orientation = {0, 0, 0, 0})

Reset the pose of the base link.

Parameters
  • position – The desired position of the base link in world coordinates.

  • orientation – The wxyz quaternion defining the desired orientation of the base link wrt the world frame.

Returns

True for success, false otherwise.

bool resetBasePosition(const std::array<double, 3> &position = {0, 0, 0})

Reset the position of the base link.

Parameters

position – The desired position of the base link in world coordinates.

Returns

True for success, false otherwise.

bool resetBaseOrientation(const std::array<double, 4> &orientation = {0, 0, 0, 0})

Reset the orientation of the base link.

Parameters

orientation – The wxyz quaternion defining the desired orientation of the base link wrt the world frame.

Returns

True for success, false otherwise.

bool resetBaseWorldLinearVelocity(const std::array<double, 3> &linear = {0, 0, 0})

Reset the linear mixed velocity of the base link.

Parameters

linear – The desired linear mixed velocity of the base link.

Returns

True for success, false otherwise.

bool resetBaseWorldAngularVelocity(const std::array<double, 3> &angular = {0, 0, 0})

Reset the angular mixed velocity of the base link.

Parameters

angular – The desired angular mixed velocity of the base link.

Returns

True for success, false otherwise.

bool resetBaseWorldVelocity(const std::array<double, 3> &linear = {0, 0, 0}, const std::array<double, 3> &angular = {0, 0, 0})

Reset the mixed velocity of the base link.

Parameters
  • linear – The desired linear mixed velocity of the base link.

  • angular – The desired angular mixed velocity of the base link.

Returns

True for success, false otherwise.

bool selfCollisionsEnabled() const

Check if the detection of self-collisions is enabled.

Returns

True if self-collisions detection is enabled, false otherwise.

bool enableSelfCollisions(const bool enable = true)

Enable the detection of self-collisions.

It will enable contact detection if it was disabled.

Parameters

enable – True to enable the self-collision detection, false to disable.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the model is valid.

Returns

True if the model is valid, false otherwise.

virtual size_t dofs(const std::vector<std::string> &jointNames = {}) const override

Get the degrees of freedom of the model.

Parameters

jointNames – Optionally restrict the count to a subset of joints.

Returns

The number of degrees of freedom of the model.

virtual std::string name() const override

Get the name of the model.

Returns

The name of the model.

virtual size_t nrOfLinks() const override

Get the number of links of the model.

Returns

The number of links.

virtual size_t nrOfJoints() const override

Get the number of joints of the model.

Returns

The number of joints.

virtual double totalMass(const std::vector<std::string> &linkNames = {}) const override

Get the total mass of the model.

Parameters

linkNames – Optionally restrict the count to a subset of links.

Returns

The total mass of the model.

virtual core::LinkPtr getLink(const std::string &linkName) const override

Get a link belonging to the model.

Parameters

linkName – The name of the link.

Throws

std::runtime_error – if the link does not exist.

Returns

The desired link.

virtual core::JointPtr getJoint(const std::string &jointName) const override

Get a joint belonging to the model.

Parameters

jointName – The name of the joint.

Throws

std::runtime_error – if the joint does not exist.

Returns

The desired joint.

virtual std::vector<std::string> linkNames(const bool scoped = false) const override

Get the name of all the model’s links.

Parameters

scoped – Scope the link names with the model name (e.g. mymodel::link1).

Returns

The list of link names.

virtual std::vector<std::string> jointNames(const bool scoped = false) const override

Get the name of all the model’s joints.

Parameters

scoped – Scope the joint names with the model name, (e.g. mymodel::joint1).

Returns

The list of joint names.

virtual double controllerPeriod() const override

Get the controller period of the model.

If no controller has been enabled, infinite is returned.

Returns

The controller period of the model.

virtual bool setControllerPeriod(const double period) override

Set the controller period of the model.

This controller period is used by PIDs and custom controller. If it is smaller than the physics step, it is treated as 0.

Parameters

period – The desired controller period.

Returns

True for success, false otherwise.

virtual bool enableHistoryOfAppliedJointForces(const bool enable = true, const size_t maxHistorySizePerJoint = 100, const std::vector<std::string> &jointNames = {}) override

Enable logging the applied joint forces.

The output of joint controllers is often a torque. This method allows to log the force references that the controller sent to the joints. It is useful when the controller runs in its own thread at its own rate and the caller wants to extract the forces at a lower frequency.

Parameters
  • enable – True to enable logging, false to disable.

  • maxHistorySizePerJoint – Size of the logging window of each joint.

  • jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool historyOfAppliedJointForcesEnabled(const std::vector<std::string> &jointNames = {}) const override

Check if logging the applied joint force is enabled.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True if the log is enabled, false otherwise.

virtual std::vector<double> historyOfAppliedJointForces(const std::vector<std::string> &jointNames = {}) const override

Get the log of applied joint forces.

Note

Given a serialization, the window has DoFs * JointWindowSize elements. The elements are ordered per time steps, i.e. the first #DoFs elements refer to the oldest forces of the windows ordered with the active joint serialization.

Note

If a joint has multiple DoFs, they are serialized contiguously.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The entire window of applied joint forces.

virtual bool contactsEnabled() const override

Check if the contact detection is enabled model-wise.

Returns

True if the contact detection is enabled model-wise, false otherwise.

virtual bool enableContacts(const bool enable = true) override

Enable the contact detection model-wise.

Parameters

enable – True to enable the contact detection model-wise, false to disable.

Returns

True for success, false otherwise.

virtual std::vector<std::string> linksInContact() const override

Get the vector of links with active contacts with other bodies.

Returns

The vector of links in contact.

virtual std::vector<core::Contact> contacts(const std::vector<std::string> &linkNames = {}) const override

Get the active contacts of the model.

Parameters

linkNames – Optionally restrict the considered links.

Returns

A vector of contacts.

virtual std::vector<double> jointPositions(const std::vector<std::string> &jointNames = {}) const override

Get the joint positions.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint positions. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointVelocities(const std::vector<std::string> &jointNames = {}) const override

Get the joint velocities.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint velocities. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointAccelerations(const std::vector<std::string> &jointNames = {}) const override

Get the joint accelerations.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint accelerations. The vector has as many elements as DoFs of the considered joints.

virtual std::vector<double> jointGeneralizedForces(const std::vector<std::string> &jointNames = {}) const override

Get the joint generalized forces.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The serialization of joint forces. The vector has as many elements as DoFs of the considered joints.

virtual core::JointLimit jointLimits(const std::vector<std::string> &jointNames = {}) const override

Get the joint limits of the model.

Parameters

jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

The joint limits of the model. The vectors of the limit object have as many elements as DoFs of the considered joints.

virtual bool setJointControlMode(const core::JointControlMode mode, const std::vector<std::string> &jointNames = {}) override

Set the control mode of model joints.

Parameters
  • mode – The desired joint control mode.

  • jointNames – Optional vector of considered joints that also defines the joint serialization. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual std::vector<core::LinkPtr> links(const std::vector<std::string> &linkNames = {}) const override

Get the links of the model.

Parameters

linkNames – Optional vector of considered links. By default, Model::linkNames is used.

Returns

A vector of pointers to the link objects.

virtual std::vector<core::JointPtr> joints(const std::vector<std::string> &jointNames = {}) const override

Get the joints of the model.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

A vector of pointers to the joint objects.

virtual bool setJointPositionTargets(const std::vector<double> &positions, const std::vector<std::string> &jointNames = {}) override

Set the position targets of the joints.

Parameters
  • positions – The vector with the joint position targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointVelocityTargets(const std::vector<double> &velocities, const std::vector<std::string> &jointNames = {}) override

Set the velocity targets of the joints.

Parameters
  • velocities – The vector with the joint velocity targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointAccelerationTargets(const std::vector<double> &accelerations, const std::vector<std::string> &jointNames = {}) override

Set the acceleration targets of the joints.

Parameters
  • accelerations – The vector with the joint acceleration targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual bool setJointGeneralizedForceTargets(const std::vector<double> &forces, const std::vector<std::string> &jointNames = {}) override

Set the generalized force targets of the joints.

Parameters
  • forces – The vector with the joint generalized force targets. It must have as many elements as the considered joint DoFs.

  • jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

True for success, false otherwise.

virtual std::vector<double> jointPositionTargets(const std::vector<std::string> &jointNames = {}) const override

Get the position targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The position targets of the joints.

virtual std::vector<double> jointVelocityTargets(const std::vector<std::string> &jointNames = {}) const override

Get the velocity targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The velocity targets of the joints.

virtual std::vector<double> jointAccelerationTargets(const std::vector<std::string> &jointNames = {}) const override

Get the acceleration targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The acceleration targets of the joints.

virtual std::vector<double> jointGeneralizedForceTargets(const std::vector<std::string> &jointNames = {}) const override

Get the generalized force targets of the joints.

Parameters

jointNames – Optional vector of considered joints. By default, Model::jointNames is used.

Returns

The generalized force targets of the joints.

virtual std::string baseFrame() const override

Get the name of the model’s base frame.

By default, the base frame is typically the root of the kinematic tree of the model.

Returns

The name of the model’s base frame.

virtual std::array<double, 3> basePosition() const override

Get the position of the base link.

Returns

The position of the base link in world coordinates.

virtual std::array<double, 4> baseOrientation() const override

Get the orientation of the base link.

Returns

The wxyz quaternion defining the orientation of the base link wrt the world frame.

virtual std::array<double, 3> baseBodyLinearVelocity() const override

Get the linear body velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear body velocity of the base link.

virtual std::array<double, 3> baseBodyAngularVelocity() const override

Get the angular body velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular body velocity of the base link.

virtual std::array<double, 3> baseWorldLinearVelocity() const override

Get the linear mixed velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The linear mixed velocity of the base link.

virtual std::array<double, 3> baseWorldAngularVelocity() const override

Get the angular mixed velocity of the base link.

Todo:

Add link to the velocity representation documentation page.

Returns

The angular mixed velocity of the base link.

virtual bool setBasePoseTarget(const std::array<double, 3> &position, const std::array<double, 4> &orientation) override

Set the pose target of the base link.

Parameters
  • position – The position target of the base link in world coordinates.

  • orientation – The wxyz quaternion defining the orientation target of the base link wrt the world frame.

Returns

True for success, false otherwise.

virtual bool setBasePositionTarget(const std::array<double, 3> &position) override

Set the position target of the base link.

Parameters

position – The position target of the base link in world coordinates.

Returns

True for success, false otherwise.

virtual bool setBaseOrientationTarget(const std::array<double, 4> &orientation) override

Set the orientation target of the base link.

Parameters

orientation – The wxyz quaternion defining the orientation target of the base link wrt the world frame.

Returns

True for success, false otherwise.

virtual bool setBaseWorldVelocityTarget(const std::array<double, 3> &linear, const std::array<double, 3> &angular) override

Set the mixed velocity target of the base link.

Parameters
  • linear – The mixed linear velocity target of the base link.

  • angular – The mixed angular velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldLinearVelocityTarget(const std::array<double, 3> &linear) override

Set the mixed linear velocity target of the base link.

Parameters

linear – The mixed linear velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldAngularVelocityTarget(const std::array<double, 3> &angular) override

Set the mixed angular velocity target of the base link.

Parameters

angular – The mixed angular velocity target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldLinearAccelerationTarget(const std::array<double, 3> &linear) override

Set the mixed linear acceleration target of the base link.

Parameters

linear – The mixed linear acceleration target of the base link.

Returns

True for success, false otherwise.

virtual bool setBaseWorldAngularAccelerationTarget(const std::array<double, 3> &angular) override

Set the mixed angular acceleration target of the base link.

Parameters

angular – The mixed angular acceleration target of the base link.

Returns

True for success, false otherwise.

virtual std::array<double, 3> basePositionTarget() const override

Get the position target of the base link.

Returns

The position target of the base link.

virtual std::array<double, 4> baseOrientationTarget() const override

Get the orientation target of the base link.

Returns

The quaternion defining the orientation target of the base link.

virtual std::array<double, 3> baseWorldLinearVelocityTarget() const override

Get the mixed linear velocity target of the base link.

Returns

The mixed linear velocity target of the base link.

virtual std::array<double, 3> baseWorldAngularVelocityTarget() const override

Get the mixed angular velocity target of the base link.

Returns

The mixed angular velocity target of the base link.

virtual std::array<double, 3> baseWorldLinearAccelerationTarget() const override

Get the mixed linear acceleration target of the base link.

Returns

The mixed linear acceleration target of the base link.

virtual std::array<double, 3> baseWorldAngularAccelerationTarget() const override

Get the mixed angular acceleration target of the base link.

Returns

The mixed angular acceleration target of the base link.

class World : public scenario::core::World, public scenario::gazebo::GazeboEntity, public std::enable_shared_from_this<scenario::gazebo::World>

Public Functions

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity worldEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertWorldPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the world.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool setPhysicsEngine(const PhysicsEngine engine)

Set the physics engine of the world.

By default, if the world file does not already contain a physics plugin, no physics is loaded by default. This method allows to insert in the simulator a plugin with one of the supported physics engines.

Parameters

engine – The desired physics engine.

Returns

True for success, false otherwise.

bool setGravity(const std::array<double, 3> &gravity)

Set the gravity of the world.

Note

This method must be called after setting the physics engine and before performing any physics step.

Parameters

gravity – The desired gravity vector.

Returns

True for success, false otherwise.

bool insertModel(const std::string &modelFile, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given path and insert it into the world.

This function is a shim over InsertModelFromFile for backwards compatibility.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • modelFile – A path to the URDF or SDF file to load and insert.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool insertModelFromFile(const std::string &path, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given path and insert it into the world.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • path – A path to the URDF or SDF file to load and insert.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool insertModelFromString(const std::string &sdfString, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given string and insert it into the world.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • sdfString – A string containing the model’s SDF/URDF XML.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool removeModel(const std::string &modelName)

Remove a model from the world.

Warning

In order to process the model removal, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters

modelName – The name of the model to remove.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the world is valid.

Returns

True if the world is valid, false otherwise.

virtual double time() const override

Get the simulated time.

Note

A physics plugin need to be part of the simulation in order to make the time flow.

Returns

The simulated time.

virtual std::string name() const override

Get the name of the world.

Returns

The name of the world.

virtual std::array<double, 3> gravity() const override

Get the gravity vector.

Returns

The gravity vector.

virtual std::vector<std::string> modelNames() const override

Get the name of the models that are part of the world.

Returns

The list of model names.

virtual scenario::core::ModelPtr getModel(const std::string &modelName) const override

Get a model part of the world.

Parameters

modelName – The name of the model to get.

Returns

The model if it is part of the world, nullptr otherwise.

virtual std::vector<core::ModelPtr> models(const std::vector<std::string> &modelNames = {}) const override

Get the models of the world.

Parameters

modelNames – Optional vector of considered models. By default, World::modelNames is used.

Returns

A vector of pointers to the model objects.

virtual uint64_t id() const override

Get the unique id of the object.

Note

It might differ from the entity number since a multi-world setting with the same models inserted in the same order would result to same numbering.

Returns

The unique object id. Invalid objects return 0.

virtual bool initialize(const ignition::gazebo::Entity worldEntity, ignition::gazebo::EntityComponentManager *ecm, ignition::gazebo::EventManager *eventManager) override

Initialize the object with entity data.

Parameters
  • linkEntity – The entity of the ECM.

  • ecm – The pointer to the ECM.

  • eventManager – The pointer to the EventManager.

Returns

True for success, false otherwise.

virtual bool createECMResources() override

Initialize the object.

Note

This method has to be called after GazeboEntity::initialize.

Returns

True for success, false otherwise.

bool insertWorldPlugin(const std::string &libName, const std::string &className, const std::string &context = {})

Insert a Ignition Gazebo plugin to the world.

Parameters
  • libName – The library name of the plugin.

  • className – The class name (or alias) of the plugin.

  • context – Optional XML plugin context.

Returns

True for success, false otherwise.

bool setPhysicsEngine(const PhysicsEngine engine)

Set the physics engine of the world.

By default, if the world file does not already contain a physics plugin, no physics is loaded by default. This method allows to insert in the simulator a plugin with one of the supported physics engines.

Parameters

engine – The desired physics engine.

Returns

True for success, false otherwise.

bool setGravity(const std::array<double, 3> &gravity)

Set the gravity of the world.

Note

This method must be called after setting the physics engine and before performing any physics step.

Parameters

gravity – The desired gravity vector.

Returns

True for success, false otherwise.

bool insertModel(const std::string &modelFile, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given path and insert it into the world.

This function is a shim over InsertModelFromFile for backwards compatibility.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • modelFile – A path to the URDF or SDF file to load and insert.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool insertModelFromFile(const std::string &path, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given path and insert it into the world.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • path – A path to the URDF or SDF file to load and insert.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool insertModelFromString(const std::string &sdfString, const core::Pose &pose = core::Pose::Identity(), const std::string &overrideModelName = {})

Load a model from the given string and insert it into the world.

Note

The default pose and model name are those specified in the robot description. If the pose is not specified, the identity is used.

Warning

In order to process the model insertion, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters
  • sdfString – A string containing the model’s SDF/URDF XML.

  • pose – The optional initial pose of the model.

  • overrideModelName – The optional name of the model. This is the name used to get the model with World::getModel.

Returns

True for success, false otherwise.

bool removeModel(const std::string &modelName)

Remove a model from the world.

Warning

In order to process the model removal, a simulator step must be executed. It could either be a paused or unpaused step.

Parameters

modelName – The name of the model to remove.

Returns

True for success, false otherwise.

virtual bool valid() const override

Check if the world is valid.

Returns

True if the world is valid, false otherwise.

virtual double time() const override

Get the simulated time.

Note

A physics plugin need to be part of the simulation in order to make the time flow.

Returns

The simulated time.

virtual std::string name() const override

Get the name of the world.

Returns

The name of the world.

virtual std::array<double, 3> gravity() const override

Get the gravity vector.

Returns

The gravity vector.

virtual std::vector<std::string> modelNames() const override

Get the name of the models that are part of the world.

Returns

The list of model names.

virtual scenario::core::ModelPtr getModel(const std::string &modelName) const override

Get a model part of the world.

Parameters

modelName – The name of the model to get.

Returns

The model if it is part of the world, nullptr otherwise.

virtual std::vector<core::ModelPtr> models(const std::vector<std::string> &modelNames = {}) const override

Get the models of the world.

Parameters

modelNames – Optional vector of considered models. By default, World::modelNames is used.

Returns

A vector of pointers to the model objects.

namespace exceptions
class ComponentNotFound : public std::runtime_error
class DOFMismatch : public std::runtime_error
class JointError : public std::runtime_error
class JointNotFound : public std::runtime_error
class LinkError : public std::runtime_error
class LinkNotFound : public std::runtime_error
class ModelError : public std::runtime_error
class ModelNotFound : public std::runtime_error
class NotImplementedError : public std::logic_error
namespace utils

Enums

enum Verbosity

Values:

enumerator SuppressAll
enumerator Error
enumerator Warning
enumerator Info
enumerator Debug
enumerator SuppressAll
enumerator Error
enumerator Warning
enumerator Info
enumerator Debug
enum Verbosity

Values:

enumerator SuppressAll
enumerator Error
enumerator Warning
enumerator Info
enumerator Debug
enumerator SuppressAll
enumerator Error
enumerator Warning
enumerator Info
enumerator Debug

Functions

std::shared_ptr<sdf::Root> getSdfRootFromFile(const std::string &sdfFileName)
std::shared_ptr<sdf::Root> getSdfRootFromString(const std::string &sdfString)
bool verboseFromEnvironment()
std::chrono::steady_clock::duration doubleToSteadyClockDuration(const double durationInSeconds)
double steadyClockDurationToDouble(const std::chrono::steady_clock::duration duration)
void rowMajorToColumnMajor(std::vector<double> &input, const long rows, const long cols)
bool parentModelJustCreated(const GazeboEntity &gazeboEntity)
template<typename ComponentTypeT, typename ComponentDataTypeT>
auto getComponent(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity, ComponentDataTypeT defaultValue = {})
template<typename ComponentTypeT>
auto getExistingComponent(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity)
template<typename ComponentTypeT>
auto getComponentData(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data())
template<typename ComponentTypeT>
auto getExistingComponentData(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data())
scenario::core::Pose fromIgnitionPose(const ignition::math::Pose3d &ignitionPose)
ignition::math::Pose3d toIgnitionPose(const scenario::core::Pose &pose)
scenario::core::Contact fromIgnitionContactMsgs(ignition::gazebo::EntityComponentManager *ecm, const ignition::msgs::Contact &contactMsg)
std::vector<scenario::core::Contact> fromIgnitionContactsMsgs(ignition::gazebo::EntityComponentManager *ecm, const ignition::msgs::Contacts &contactsMsg)
sdf::World renameSDFWorld(const sdf::World &world, const std::string &newWorldName)
bool renameSDFModel(sdf::Root &sdfRoot, const std::string &newModelName)
bool updateSDFPhysics(sdf::Root &sdfRoot, const double maxStepSize, const double rtf, const double realTimeUpdateRate, const size_t worldIndex = 0)
sdf::ElementPtr getPluginSDFElement(const std::string &libName, const std::string &className)
sdf::JointType toSdf(const scenario::core::JointType type)
scenario::core::JointType fromSdf(const sdf::JointType sdfType)
ignition::math::Vector3d fromModelToBaseLinearVelocity(const ignition::math::Vector3d &linModelVelocity, const ignition::math::Vector3d &angModelVelocity, const ignition::math::Pose3d &M_H_B, const ignition::math::Quaterniond &W_R_B)
ignition::math::Vector3d fromBaseToModelLinearVelocity(const ignition::math::Vector3d &linBaseVelocity, const ignition::math::Vector3d &angBaseVelocity, const ignition::math::Pose3d &M_H_B, const ignition::math::Quaterniond &W_R_B)
std::shared_ptr<World> getParentWorld(const GazeboEntity &gazeboEntity)
std::shared_ptr<Model> getParentModel(const GazeboEntity &gazeboEntity)
template<typename ComponentType>
ignition::gazebo::Entity getFirstParentEntityWithComponent(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity)
template<typename T>
auto defaultEqualityOperator(const T &a, const T &b) -> bool
template<typename ComponentTypeT, typename ComponentDataTypeT>
auto setComponentData(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity, const ComponentDataTypeT &data, const std::function<bool(const ComponentDataTypeT &a, const ComponentDataTypeT &b)> &eql = defaultEqualityOperator<ComponentDataTypeT>)
template<typename ComponentTypeT, typename ComponentDataTypeT>
auto setExistingComponentData(ignition::gazebo::EntityComponentManager *ecm, const ignition::gazebo::Entity entity, const ComponentDataTypeT &data, const std::function<bool(const ComponentDataTypeT &a, const ComponentDataTypeT &b)> &eql = defaultEqualityOperator<ComponentDataTypeT>)
static inline std::array<double, 3> fromIgnitionVector(const ignition::math::Vector3d &ignitionVector)
static inline std::array<double, 4> fromIgnitionQuaternion(const ignition::math::Quaterniond &ignitionQuaternion)
static inline ignition::math::Vector3d toIgnitionVector3(const std::array<double, 3> &vector)
static inline ignition::math::Vector4d toIgnitionVector4(const std::array<double, 4> &vector)
static inline ignition::math::Quaterniond toIgnitionQuaternion(const std::array<double, 4> &vector)
static inline ignition::math::PID toIgnitionPID(const scenario::core::PID &pid)
static inline scenario::core::PID fromIgnitionPID(const ignition::math::PID &pid)
void setVerbosity(const Verbosity level = DEFAULT_VERBOSITY)

Set the verbosity process-wise.

Accepted levels are the following:

  • Verbosity::SuppressAll: No messages.

  • Verbosity::Error: Error messages.

  • Verbosity::Warning: Error and warning messages.

  • Verbosity::Info: Error, warning, and info messages.

  • Verbosity::Debug: Error, warning, info, and debug messages.

If called without specifying the level, it will use Verbosity::Warning or Verbosity::Debug depending if the project was compiled respectively with Release or Debug flags.

Parameters

level – The verbosity level.

std::string findSdfFile(const std::string &fileName)

Find a SDF file in the filesystem.

The search path is defined with the IGN_GAZEBO_RESOURCE_PATH environment variable.

Parameters

fileName – The SDF file name.

Returns

The absolute path to the file if found, an empty string otherwise.

bool sdfStringValid(const std::string &sdfString)

Check if a SDF string is valid.

An SDF string could contain for instance an SDF model or an SDF world, and it is valid if it can be parsed successfully by the SDFormat library.

Parameters

sdfString – The SDF string to check.

Returns

True if the SDF string is valid, false otherwise.

std::string getSdfString(const std::string &fileName)

Get an SDF string from a SDF file.

Parameters

fileName – An SDF file. It could be either an absolute path to the file or the file name if the parent folder is part of the IGN_GAZEBO_RESOURCE_PATH environment variable.

Returns

The SDF string if the file was found and is valid, an empty string otherwise.

std::string getModelNameFromSdf(const std::string &fileName)

Get the name of a model from a SDF file or SDF string.

Note

sdformat supports only one model per SDF.

Parameters

fileName – An SDF file or string. It could be an absolute path to the file, the file name if the parent folder is part of the IGN_GAZEBO_RESOURCE_PATH environment variable, or a SDF string.

Returns

The name of the model if the SDF is valid, an empty string otherwise.

std::string getWorldNameFromSdf(const std::string &fileName, const size_t worldIndex = 0)

Get the name of a world from a SDF file or SDF string.

Parameters

fileName – An SDF file or string. It could be an absolute path to the file, the file name if the parent folder is part of the IGN_GAZEBO_RESOURCE_PATH environment variable, or a SDF string.

Returns

The name of the world if the SDF is valid, an empty string otherwise.

std::string getEmptyWorld()

Return a SDF string with an empty world.

An empty world only has a sun and the default DART physics profile enabled.

Note

The empty world does not have any ground plane.

Returns

A SDF string with the empty world.

std::string getModelFileFromFuel(const std::string &URI, const bool useCache = false)

Get a SDF model file from a Fuel URI.

Note

A valid URI has the following form: https://fuel.ignitionrobotics.org/openrobotics/models/model_name

Parameters
  • URI – A valid Fuel URI.

  • useCache – Load the model from the local cache.

Returns

The absolute path to the SDF model.

std::string getRandomString(const size_t length)

Generate a random alpha numeric string.

Parameters

length – The length of the string.

Returns

The random string.

std::string URDFFileToSDFString(const std::string &urdfFile)

Convert a URDF file to a SDF string.

Parameters

urdfFile – The absolute path to the URDF file.

Returns

The SDF string if the file exists and it was successfully converted, an empty string otherwise.

std::string URDFStringToSDFString(const std::string &urdfString)

Convert a URDF string to a SDF string.

Parameters

urdfFile – A URDF string.

Returns

The SDF string if the URDF string was successfully converted, an empty string otherwise.

std::vector<double> normalize(const std::vector<double> &input, const std::vector<double> &low, const std::vector<double> &high)

Normalize a vector in [-1, 1].

The normalization applies the following equation, where \( v \) is the input, \( l \) and \( h \) are respectively the lower and higher limits:

\( v_{normalized} = 2 \frac{v - l}{h - l} - 1 \)

The input, low and high arguments are broadcasted to a common size. Refer to the following for broadcasting definition:

https://numpy.org/doc/stable/user/basics.broadcasting.html

Note

If the lower limit matches the higher limit, the corresponding input value is not normalized.

Parameters
  • input – The input vector.

  • low – The lower limit.

  • high – The higher limit.

Throws

std::invalid_argument – If the arguments cannot be broadcasted.

Returns

The normalized input.

std::vector<double> denormalize(const std::vector<double> &input, const std::vector<double> &low, const std::vector<double> &high)

Denormalize a vector from [-1, 1].

The denormalization applies the following equation, where \( v \) is the input, \( l \) and \( h \) are respectively the lower and higher limits:

\( v_{denormalized} = \frac{1}{2} (v + 1)(h - l) - l \)

The input, low and high arguments are broadcasted to a common size. Refer to the following for broadcasting definition:

https://numpy.org/doc/stable/user/basics.broadcasting.html

Parameters
  • input – The input vector.

  • low – The lower limit.

  • high – The higher limit.

Throws

std::invalid_argument – If the arguments cannot be broadcasted.

Returns

The denormalized input.

bool insertPluginToGazeboEntity(const GazeboEntity &gazeboEntity, const std::string &libName, const std::string &className, const std::string &context = "")

Insert a plugin to any Gazebo entity.

Note

This function will not return true if the plugin is successful. This function just triggers an event that notifies the server to load a plugin, and it does not receive any return value that could be used to assess the outcome.

Parameters
  • gazeboEntity – The Gazebo entity (world, model, joint, …).

  • libName – The name of the plugin library.

  • className – The name of the class implementing the plugin.

  • context – The optional plugin SDF context.

Returns

True if the entity is valid, false otherwise.

static inline std::array<double, 3> fromIgnitionVector(const ignition::math::Vector3d &ignitionVector)
static inline std::array<double, 4> fromIgnitionQuaternion(const ignition::math::Quaterniond &ignitionQuaternion)
static inline ignition::math::Vector3d toIgnitionVector3(const std::array<double, 3> &vector)
static inline ignition::math::Vector4d toIgnitionVector4(const std::array<double, 4> &vector)
static inline ignition::math::Quaterniond toIgnitionQuaternion(const std::array<double, 4> &vector)
static inline ignition::math::PID toIgnitionPID(const scenario::core::PID &pid)
static inline scenario::core::PID fromIgnitionPID(const ignition::math::PID &pid)

Variables

const std::string ScenarioVerboseEnvVar = "SCENARIO_VERBOSE"
class FixedSizeQueue
class LinkWrenchCmd
class WrenchWithDuration