RobWorkProject  23.9.11-
Public Types | Public Member Functions | List of all members
DynamicSimulator Class Reference

A general physics engine interface for simulating dynamics of objects and robot devices. More...

#include <DynamicSimulator.hpp>

Inherits Simulator.

Public Types

typedef rw::core::Ptr< DynamicSimulatorPtr
 smart pointer type of this class
 
- Public Types inherited from Simulator
typedef rw::core::Ptr< SimulatorPtr
 smart pointer type of simulator
 

Public Member Functions

 DynamicSimulator (rwsim::dynamics::DynamicWorkCell::Ptr dworkcell, rw::core::Ptr< PhysicsEngine > pengine)
 Constructor. More...
 
 DynamicSimulator (rwsim::dynamics::DynamicWorkCell::Ptr dworkcell)
 Constructor for a DynamicSimulator using a default PhysicsEngine. More...
 
virtual ~DynamicSimulator ()
 destructor
 
void exitPhysics ()
 cleans up the allocated storage
 
double getTime ()
 gets the the current simulated time More...
 
void setEnabled (dynamics::Body::Ptr body, bool enabled)
 Enables or disables a body. More...
 
drawable::SimulatorDebugRender::Ptr createDebugRender ()
 create a debug render for the specific implementation More...
 
rw::core::PropertyMapgetPropertyMap ()
 Get the properties used by the simulator. More...
 
void addController (rw::core::Ptr< rwlibs::simulation::SimulatedController > controller)
 add a simulated controller to this simulator More...
 
void removeController (rw::core::Ptr< rwlibs::simulation::SimulatedController > controller)
 removes a simulated controller from this simulator More...
 
void addBody (rwsim::dynamics::Body::Ptr body, rw::kinematics::State &state)
 Add a body to the simulator. More...
 
void addBody (rwsim::dynamics::Body::Ptr body)
 Add a body to the simulator. More...
 
void addDevice (rwsim::dynamics::DynamicDevice::Ptr dev, rw::kinematics::State &state)
 Add a device to the simulator. More...
 
void addDevice (rwsim::dynamics::DynamicDevice::Ptr dev)
 Add a device to the simulator. More...
 
void addSensor (rwlibs::simulation::SimulatedSensor::Ptr sensor, rw::kinematics::State &state)
 Add a simulated sensor to the simulator. More...
 
void addSensor (rwlibs::simulation::SimulatedSensor::Ptr sensor)
 Add a simulated sensor to the simulator. More...
 
void removeSensor (rwlibs::simulation::SimulatedSensor::Ptr sensor)
 Remove a simulated sensor from the simulator. More...
 
std::vector< rwlibs::simulation::SimulatedSensor::PtrgetSensors ()
 get the list of simulated sensors More...
 
void step (double dt)
 take a step forward in time with timestep dt. More...
 
rw::kinematics::StategetState ()
 get current state of simulator More...
 
void reset (const rw::kinematics::State &state)
 reset velocity and acceleration of all bodies to 0. And sets the position of all bodies to that described in state More...
 
void init (rw::kinematics::State &state)
 initialize simulator with state variables More...
 
void setEnabled (rw::core::Ptr< rw::kinematics::Frame > f, bool)
 
void setDynamicsEnabled (rwsim::dynamics::Body::Ptr body, bool enabled)
 Enable or disable a body in the simulation. More...
 
void setTarget (rwsim::dynamics::Body::Ptr body, const rw::math::Transform3D<> &target, rw::kinematics::State &state, double maxLinVel=0.5, double maxLinAcc=1.0, double maxAngVel=0.4, double maxAngAcc=1.0)
 Set a target position of a body. This will add forces/velocities to a body such that it moves toward the target pose. More...
 
void setTarget (rwsim::dynamics::Body::Ptr body, const rw::math::Transform3D<> &target, double maxLinVel=0.5, double maxLinAcc=1.0, double maxAngVel=0.4, double maxAngAcc=1.0)
 Set a target position of a body. This will add forces/velocities to a body such that it moves toward the target pose. More...
 
void setTarget (dynamics::Body::Ptr body, rw::trajectory::Trajectory< rw::math::Transform3D<>>::Ptr traj)
 Set a target trajectory for a body. More...
 
void setTarget (dynamics::Body::Ptr body, const rw::math::VelocityScrew6D<> &velocity)
 Set a velocity target. More...
 
void disableBodyControl (dynamics::Body::Ptr body)
 disables the target control of body body. More...
 
void disableBodyControl ()
 Disable all control for all bodies.
 
rwsim::control::BodyController::Ptr getBodyController ()
 Get the body controller. More...
 
void attach (rwsim::dynamics::Body::Ptr b1, rwsim::dynamics::Body::Ptr b2)
 Attach bodies. More...
 
void detach (rwsim::dynamics::Body::Ptr b1, rwsim::dynamics::Body::Ptr b2)
 Detach bodies. More...
 
rwsim::dynamics::DynamicWorkCell::Ptr getDynamicWorkCell ()
 Get the dynamic workcell used by the simulator. More...
 
- Public Member Functions inherited from Simulator
virtual rw::sensor::Sensor::Ptr getSensor (const std::string &name)
 get sensor with specific name
 
virtual rwlibs::control::Controller::Ptr getController (const std::string &name)
 get controller with specific name
 
template<class SIMSENSORTYPE >
rw::sensor::Sensor::Ptr getSensorHandle (SIMSENSORTYPE *ssensor)
 get sensorhandle to a statefull instance of the simulated sensor, controlling the sensor in this simulator. More...
 
template<class SIMSENSORTYPE >
bool hasHandle (SIMSENSORTYPE *ssensor)
 Test if handle for simulatedsensor exists. More...
 
template<class SIMSENSORTYPE >
void addHandle (SIMSENSORTYPE *ssensor, rw::sensor::Sensor::Ptr sensor)
 add handle to a specific simulated sensor More...
 

Detailed Description

A general physics engine interface for simulating dynamics of objects and robot devices.

The general step looks like this:;

Foreach controller=_controllers controller->update(dt,state);

physicsEngine->step(dt, state);

Foreach sensor=_sensors sensor->update(dt,state)

Constructor & Destructor Documentation

◆ DynamicSimulator() [1/2]

Constructor.

Parameters
dworkcell[in] the dynamic workcell.
pengine[in] the physics engine to use.

◆ DynamicSimulator() [2/2]

Constructor for a DynamicSimulator using a default PhysicsEngine.

Parameters
dworkcell[in] the dynamic workcell.

Member Function Documentation

◆ addBody() [1/2]

void addBody ( rwsim::dynamics::Body::Ptr  body)
inline

Add a body to the simulator.

Parameters
body[in] the body to add.

◆ addBody() [2/2]

void addBody ( rwsim::dynamics::Body::Ptr  body,
rw::kinematics::State state 
)

Add a body to the simulator.

Parameters
body[in] the body to add.
state[in] the state giving the initial pose of the body.

◆ addController()

void addController ( rw::core::Ptr< rwlibs::simulation::SimulatedController controller)

add a simulated controller to this simulator

Parameters
controller[in] the controller to add.

◆ addDevice() [1/2]

void addDevice ( rwsim::dynamics::DynamicDevice::Ptr  dev)
inline

Add a device to the simulator.

Parameters
dev[in] the device to add.

◆ addDevice() [2/2]

void addDevice ( rwsim::dynamics::DynamicDevice::Ptr  dev,
rw::kinematics::State state 
)

Add a device to the simulator.

Parameters
dev[in] the device to add.
state[in/out] the state giving the initial configuration of the device, which might be changed.

◆ addSensor() [1/2]

void addSensor ( rwlibs::simulation::SimulatedSensor::Ptr  sensor)
inline

Add a simulated sensor to the simulator.

Parameters
sensor[in] the sensor to add.

◆ addSensor() [2/2]

void addSensor ( rwlibs::simulation::SimulatedSensor::Ptr  sensor,
rw::kinematics::State state 
)

Add a simulated sensor to the simulator.

Parameters
sensor[in] the sensor to add.
state[in/out] if the sensor is not registered in the state, it will be registered.

◆ attach()

Attach bodies.

Parameters
b1[in] first body.
b2[in] second body.

◆ createDebugRender()

drawable::SimulatorDebugRender::Ptr createDebugRender ( )

create a debug render for the specific implementation

Returns
NULL if no render is available else a valid render

◆ detach()

Detach bodies.

Parameters
b1[in] first body.
b2[in] second body.

◆ disableBodyControl()

void disableBodyControl ( dynamics::Body::Ptr  body)

disables the target control of body body.

Parameters
body[in] the body.

◆ getBodyController()

rwsim::control::BodyController::Ptr getBodyController ( )
inline

Get the body controller.

Returns
the controller.

◆ getDynamicWorkCell()

rwsim::dynamics::DynamicWorkCell::Ptr getDynamicWorkCell ( )
inline

Get the dynamic workcell used by the simulator.

Returns
the dynamic workcell.

◆ getPropertyMap()

rw::core::PropertyMap& getPropertyMap ( )
virtual

Get the properties used by the simulator.

Returns
a reference to the properties.

Implements Simulator.

◆ getSensors()

std::vector<rwlibs::simulation::SimulatedSensor::Ptr> getSensors ( )

get the list of simulated sensors

Returns
vector of sensors.

◆ getState()

rw::kinematics::State& getState ( )
virtual

get current state of simulator

Returns
current state of simulator

Implements Simulator.

◆ getTime()

double getTime ( )
virtual

gets the the current simulated time

Returns
current simulated time.

Implements Simulator.

◆ init()

void init ( rw::kinematics::State state)
virtual

initialize simulator with state variables

Note
initialize simulator physics with state

Implements Simulator.

◆ removeController()

void removeController ( rw::core::Ptr< rwlibs::simulation::SimulatedController controller)

removes a simulated controller from this simulator

Parameters
controller[in] the controller to remove.

◆ removeSensor()

void removeSensor ( rwlibs::simulation::SimulatedSensor::Ptr  sensor)

Remove a simulated sensor from the simulator.

Parameters
sensor[in] the sensor to remove.

◆ reset()

void reset ( const rw::kinematics::State state)
virtual

reset velocity and acceleration of all bodies to 0. And sets the position of all bodies to that described in state

Note
resets position, velocity and acceleration of all bodies to that defined in state or by the default behavior of body/device interfaces.

Implements Simulator.

◆ setDynamicsEnabled()

void setDynamicsEnabled ( rwsim::dynamics::Body::Ptr  body,
bool  enabled 
)

Enable or disable a body in the simulation.

Parameters
body[in] the body.
enabled[in] boolean indicating if body should be enabled or not.

◆ setEnabled() [1/2]

void setEnabled ( dynamics::Body::Ptr  body,
bool  enabled 
)

Enables or disables a body.

Parameters
body[in] the body.
enabled[in] boolean indicating whether body should be enabled or disabled.

◆ setEnabled() [2/2]

void setEnabled ( rw::core::Ptr< rw::kinematics::Frame f,
bool   
)
virtual

Enables or disables simulation of a frame

Note
this only has an effect if the frame f is successfully mapped to any fo the bodies in the scene eg. a body frame of one of the bodies.

Implements Simulator.

◆ setTarget() [1/4]

void setTarget ( dynamics::Body::Ptr  body,
const rw::math::VelocityScrew6D<> &  velocity 
)

Set a velocity target.

Parameters
body[in] the body that should move.
velocity[in] the velocity target.

◆ setTarget() [2/4]

void setTarget ( dynamics::Body::Ptr  body,
rw::trajectory::Trajectory< rw::math::Transform3D<>>::Ptr  traj 
)

Set a target trajectory for a body.

Parameters
body[in] the body.
traj[in] the trajectory.

◆ setTarget() [3/4]

void setTarget ( rwsim::dynamics::Body::Ptr  body,
const rw::math::Transform3D<> &  target,
double  maxLinVel = 0.5,
double  maxLinAcc = 1.0,
double  maxAngVel = 0.4,
double  maxAngAcc = 1.0 
)
inline

Set a target position of a body. This will add forces/velocities to a body such that it moves toward the target pose.

Parameters
body[in] the body to control
target[in] the target pose
maxLinVel[in] (optional) maximum linear velocity of the body in \(\frac{m}{s}\).
maxLinAcc[in] (optional) maximum linear acceleration of the body in \(\frac{m}{s^2}\).
maxAngVel[in] (optional) maximum angular velocity of the body in \(\frac{rad}{s}\).
maxAngAcc[in] (optional) maximum angular acceleration of the body in \(\frac{rad}{s^2}\).

◆ setTarget() [4/4]

void setTarget ( rwsim::dynamics::Body::Ptr  body,
const rw::math::Transform3D<> &  target,
rw::kinematics::State state,
double  maxLinVel = 0.5,
double  maxLinAcc = 1.0,
double  maxAngVel = 0.4,
double  maxAngAcc = 1.0 
)

Set a target position of a body. This will add forces/velocities to a body such that it moves toward the target pose.

Parameters
body[in] the body to control
target[in] the target pose
state[in] the current state.
maxLinVel[in] (optional) maximum linear velocity of the body in \(\frac{m}{s}\).
maxLinAcc[in] (optional) maximum linear acceleration of the body in \(\frac{m}{s^2}\).
maxAngVel[in] (optional) maximum angular velocity of the body in \(\frac{rad}{s}\).
maxAngAcc[in] (optional) maximum angular acceleration of the body in \(\frac{rad}{s^2}\).

◆ step()

void step ( double  dt)
virtual

take a step forward in time with timestep dt.

Parameters
dt[in] the time step

Implements Simulator.


The documentation for this class was generated from the following file: