#include <mc_control/fsm/Controller.h>

Inheritance diagram for mc_control::fsm::Controller:

Collaboration diagram for mc_control::fsm::Controller:

Public Member Functions
	Controller (mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration &config, ControllerParameters params={})

	~Controller () override

bool	run () override

bool	run (mc_solver::FeedbackType fType)

void	reset (const ControllerResetData &data) override

virtual void	interrupt ()

bool	running ()

bool	resume (const std::string &state)

std::shared_ptr< mc_tasks::PostureTask >	getPostureTask (const std::string &robot)

mc_solver::ContactConstraint &	contactConstraint ()

StateFactory &	factory ()

Public Member Functions inherited from mc_control::MCController
virtual	~MCController ()

virtual void	createObserverPipelines (const mc_rtc::Configuration &config)

virtual bool	runObserverPipelines ()

virtual bool	resetObserverPipelines ()
	Reset the observers. More...

bool	hasObserverPipeline (const std::string &name) const

bool	hasObserverPipeline () const

const std::vector< mc_observers::ObserverPipeline > &	observerPipelines () const

std::vector< mc_observers::ObserverPipeline > &	observerPipelines ()

const mc_observers::ObserverPipeline &	observerPipeline (const std::string &name) const

mc_observers::ObserverPipeline &	observerPipeline (const std::string &name)

const mc_observers::ObserverPipeline &	observerPipeline () const

mc_observers::ObserverPipeline &	observerPipeline ()

bool	run (mc_solver::FeedbackType fType)

virtual void	stop ()

void	addCollisions (const std::string &r1, const std::string &r2, const std::vector< mc_rbdyn::Collision > &collisions)

bool	hasCollision (const std::string &r1, const std::string &r2, const mc_rbdyn::Collision &col) const noexcept

bool	hasCollision (const std::string &r1, const std::string &r2, const std::string &c1, const std::string &c2) const noexcept

void	removeCollisions (const std::string &r1, const std::string &r2, const std::vector< mc_rbdyn::Collision > &collisions)

void	removeCollisions (const std::string &r1, const std::string &r2)

void	addContact (const Contact &c)

void	removeContact (const Contact &c)

void	clearContacts ()

const ContactSet &	contacts () const

bool	hasContact (const Contact &c) const

bool	hasRobot (const std::string &robot) const noexcept

const mc_rbdyn::Robots &	robots () const noexcept

mc_rbdyn::Robots &	robots () noexcept

const mc_solver::QPSolver &	solver () const noexcept

mc_solver::QPSolver &	solver () noexcept

mc_rtc::Logger &	logger () noexcept

std::shared_ptr< mc_rtc::gui::StateBuilder >	gui () const noexcept

mc_rtc::DataStore &	datastore () noexcept

const mc_rtc::DataStore &	datastore () const noexcept

virtual void	supported_robots (std::vector< std::string > &out) const

mc_rbdyn::Robot &	loadRobot (mc_rbdyn::RobotModulePtr rm, const std::string &name)

void	removeRobot (const std::string &name)

mc_rtc::Configuration &	config ()

const mc_rtc::Configuration &	config () const

Gripper &	gripper (const std::string &robot, const std::string &gripper)

template<typename T = void>
void	anchorFrame (const sva::PTransformd &)

template<typename T = void>
void	anchorFrameReal (const sva::PTransformd &)

template<typename T = void>
const sva::PTransformd &	anchorFrame () const

template<typename T = void>
const sva::PTransformd &	anchorFrameReal () const

mc_rtc::Configuration	robot_config (const std::string &robot_name) const

mc_rtc::Configuration	robot_config (const mc_rbdyn::Robot &robot) const

const mc_rbdyn::Robot &	robot () const noexcept

mc_rbdyn::Robot &	robot () noexcept

const mc_rbdyn::Robot &	robot (const std::string &name) const

mc_rbdyn::Robot &	robot (const std::string &name)

const mc_rbdyn::Robot &	env () const noexcept

mc_rbdyn::Robot &	env () noexcept

const mc_rbdyn::Robots &	realRobots () const noexcept

mc_rbdyn::Robots &	realRobots () noexcept

const mc_rbdyn::Robot &	realRobot () const noexcept

mc_rbdyn::Robot &	realRobot () noexcept

const mc_rbdyn::Robot &	realRobot (const std::string &name) const

mc_rbdyn::Robot &	realRobot (const std::string &name)

const mc_rbdyn::Robots &	outputRobots () const noexcept

mc_rbdyn::Robots &	outputRobots () noexcept

const mc_rbdyn::Robot &	outputRobot () const noexcept

mc_rbdyn::Robot &	outputRobot () noexcept

const mc_rbdyn::Robot &	outputRobot (const std::string &name) const

mc_rbdyn::Robot &	outputRobot (const std::string &name)

const mc_rbdyn::Robots &	outputRealRobots () const noexcept

mc_rbdyn::Robots &	outputRealRobots () noexcept

const mc_rbdyn::Robot &	outputRealRobot () const noexcept

mc_rbdyn::Robot &	outputRealRobot () noexcept

const mc_rbdyn::Robot &	outputRealRobot (const std::string &name) const

mc_rbdyn::Robot &	outputRealRobot (const std::string &name)

Protected Attributes
std::map< std::string, std::shared_ptr< mc_tasks::PostureTask > >	posture_tasks_

std::map< std::string, double >	saved_posture_weights_

std::map< std::string, std::shared_ptr< mc_tasks::EndEffectorTask > >	ff_tasks_

StateFactory	factory_

bool	idle_keep_state_ = false

bool	running_ = false

bool	first_reset_ = true

Executor	executor_

Protected Attributes inherited from mc_control::MCController
std::shared_ptr< mc_solver::QPSolver >	qpsolver

mc_rbdyn::RobotsPtr	outputRobots_

mc_rbdyn::RobotsPtr	outputRealRobots_

std::vector< mc_rbdyn::RobotConverter >	converters_

std::vector< mc_observers::ObserverPipeline >	observerPipelines_

std::shared_ptr< mc_rtc::Logger >	logger_

std::shared_ptr< mc_rtc::gui::StateBuilder >	gui_

mc_rtc::Configuration	config_

mc_rtc::DataStore	datastore_

std::vector< mc_solver::ConstraintSetPtr >	constraints_

std::shared_ptr< mc_solver::ContactConstraint >	contact_constraint_ = nullptr

std::map< std::pair< std::string, std::string >, std::shared_ptr< mc_solver::CollisionsConstraint > >	collision_constraints_

ContactSet	contacts_

bool	contacts_changed_

std::vector< ContactTableDataT >	contacts_table_

duration_ms	updateContacts_dt_ {0}

Friends
struct	Executor

Additional Inherited Members
Public Types inherited from mc_control::MCController
using	Backend = mc_solver::QPSolver::Backend

Static Public Member Functions inherited from mc_control::MCController
static void	set_loading_location (std::string_view location)

static void	set_name (std::string_view name)

Public Attributes inherited from mc_control::MCController
const double	timeStep

mc_rtc::unique_ptr< mc_solver::ContactConstraint >	contactConstraint

mc_rtc::unique_ptr< mc_solver::DynamicsConstraint >	dynamicsConstraint

mc_rtc::unique_ptr< mc_solver::KinematicsConstraint >	kinematicsConstraint

mc_rtc::unique_ptr< mc_solver::CollisionsConstraint >	selfCollisionConstraint

mc_rtc::unique_ptr< mc_solver::CompoundJointConstraint >	compoundJointConstraint

std::shared_ptr< mc_tasks::PostureTask >	postureTask

const std::string	name_

const std::string	loading_location_

Protected Types inherited from mc_control::MCController
using	ContactTableDataT = std::tuple< std::string, std::string, std::string, std::string, std::string, double >

using	duration_ms = std::chrono::duration< double, std::milli >

Protected Member Functions inherited from mc_control::MCController
	MCController (std::shared_ptr< mc_rbdyn::RobotModule > robot, double dt, ControllerParameters params={})

	MCController (std::shared_ptr< mc_rbdyn::RobotModule > robot, double dt, const mc_rtc::Configuration &config, ControllerParameters params={})

	MCController (const std::vector< std::shared_ptr< mc_rbdyn::RobotModule >> &robot_modules, double dt, ControllerParameters params={})

	MCController (const std::vector< std::shared_ptr< mc_rbdyn::RobotModule >> &robot_modules, double dt, const mc_rtc::Configuration &config, ControllerParameters params={})

mc_rbdyn::Robot &	loadRobot (mc_rbdyn::RobotModulePtr rm, const std::string &name, mc_rbdyn::Robots &robots, const mc_rbdyn::LoadRobotParameters &params)

void	addRobotToLog (const mc_rbdyn::Robot &robot)

void	addRobotToGUI (const mc_rbdyn::Robot &robot)

void	updateContacts ()

Detailed Description

This controller implements a finite-state machine to handle complex scenarios in a generic fashion.

The FSM can be controlled in two ways:

managed: in that case, an external tool triggers transitions
self-managed: the FSM takes care of transition, in that case a transition map must be provided.

Constructor & Destructor Documentation

◆ Controller()

mc_control::fsm::Controller::Controller	(	mc_rbdyn::RobotModulePtr	rm,
		double	dt,
		const mc_rtc::Configuration &	config,
		ControllerParameters	params = `{}`
	)

◆ ~Controller()

mc_control::fsm::Controller::~Controller ( )

override

Member Function Documentation

◆ contactConstraint()

mc_solver::ContactConstraint& mc_control::fsm::Controller::contactConstraint ( )

inline

Access contact constraint

◆ factory()

StateFactory& mc_control::fsm::Controller::factory ( )

inline

Access the state factory

◆ getPostureTask()

std::shared_ptr<mc_tasks::PostureTask> mc_control::fsm::Controller::getPostureTask ( const std::string & robot )

Get the posture task associated to a robot

Returns a nullptr if the task does not exist (i.e. robot is not actuated or not loaded by the controller)

◆ interrupt()

virtual void mc_control::fsm::Controller::interrupt ( )

inlinevirtual

Stop the current state's execution

The controller will switch to its idle behaviour which maintains current contacts, free-flyer position/orientation and posture.

This function is virtual to allow derived implementation to handle interruptions differently.

◆ reset()

void mc_control::fsm::Controller::reset ( const ControllerResetData & reset_data )

overridevirtual

Reset the controller with data provided by ControllerResetData. This is called at two possible points during a simulation/live execution:

Actual start
Switch from a previous (MCController-like) controller In the first case, the data comes from the simulation/controller. In the second case, the data comes from the previous MCController instance.
Parameters

reset_data Contains information allowing to reset the controller properly

Note
The default implementation reset the main robot's state to that provided by reset_data (with a null speed/acceleration). It maintains the contacts as they were set by the controller previously.

Exceptions

if the main robot is not supported (see supported_robots())

Reimplemented from mc_control::MCController.

◆ resume()

bool mc_control::fsm::Controller::resume ( const std::string & state )

Resume the FSM execution on a new state

This call is ignored if running() returns true

Parameters

state State to start

Returns: True if the state was started

◆ run() [1/2]

bool mc_control::fsm::Controller::run ( )

overridevirtual

This function is called at each time step of the process driving the robot (i.e. simulation or robot's controller). This function is the most likely to be overriden for complex controller behaviours.

Returns: True if the solver succeeded, false otherwise

Note: This is meant to run in real-time hence some precaution should apply (e.g. no i/o blocking calls, no thread instantiation and such); The default implementation does the bare minimum (i.e. call run on QPSolver) It is recommended to use it in your override.

Reimplemented from mc_control::MCController.

◆ run() [2/2]

bool mc_control::fsm::Controller::run ( mc_solver::FeedbackType fType )

Can be called in derived class to run the FSM with a different feedback strategy

Parameters

fType Type of feedback used in the solver

◆ running()

bool mc_control::fsm::Controller::running ( )

inline

Check if current state is running

Friends And Related Function Documentation

◆ Executor

friend struct Executor

friend

Member Data Documentation

◆ executor_

Executor mc_control::fsm::Controller::executor_

protected

Main executor

◆ factory_

StateFactory mc_control::fsm::Controller::factory_

protected

State factory

◆ ff_tasks_

std::map<std::string, std::shared_ptr<mc_tasks::EndEffectorTask> > mc_control::fsm::Controller::ff_tasks_

protected

Creates a free-flyer end-effector task for each robot with a free flyer

◆ first_reset_

bool mc_control::fsm::Controller::first_reset_ = true

protected

First reset call

◆ idle_keep_state_

bool mc_control::fsm::Controller::idle_keep_state_ = false

protected

Behaviour during idle

◆ posture_tasks_

std::map<std::string, std::shared_ptr<mc_tasks::PostureTask> > mc_control::fsm::Controller::posture_tasks_

protected

Creates a posture task for each actuated robots (i.e. robot.dof() - robot.joint(0).dof() > 0 )

◆ running_

bool mc_control::fsm::Controller::running_ = false

protected

True if not idle

◆ saved_posture_weights_

std::map<std::string, double> mc_control::fsm::Controller::saved_posture_weights_

protected

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

include/mc_control/fsm/Controller.h

Public Member Functions

Protected Attributes

Friends

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Controller()

◆ ~Controller()

Member Function Documentation

◆ contactConstraint()

◆ factory()

◆ getPostureTask()

◆ interrupt()

◆ reset()

◆ resume()

◆ run() [1/2]

◆ run() [2/2]

◆ running()

Friends And Related Function Documentation

◆ Executor

Member Data Documentation

◆ executor_

◆ factory_

◆ ff_tasks_

◆ first_reset_

◆ idle_keep_state_

◆ posture_tasks_

◆ running_

◆ saved_posture_weights_