Robot interface augmented with ROS components. More...

#include <aikido/robot/ros/RosRobot.hpp>

Inheritance diagram for aikido::robot::ros::RosRobot:

Public Member Functions
	RosRobot (const dart::common::Uri &urdf, const dart::common::Uri &srdf, const std::string name, const bool addDefaultExecutors, const dart::common::ResourceRetrieverPtr &retriever=std::make_shared< aikido::io::CatkinResourceRetriever >(), const ::ros::NodeHandle &node=::ros::NodeHandle(), const std::string modeControllerName="")
	Construct a new RosRobot object. More...

	RosRobot (dart::dynamics::ReferentialSkeletonPtr refSkeleton, Robot *rootRobot, dart::collision::CollisionDetectorPtr collisionDetector, std::shared_ptr< dart::collision::BodyNodeCollisionFilter > collisionFilter, const std::string name, const ::ros::NodeHandle &node=::ros::NodeHandle(), const std::string modeControllerName="")
	Construct a new RosRobot as subrobot. More...

virtual	~RosRobot ()=default
	Destructor. More...

void	step (const std::chrono::system_clock::time_point &timepoint) override
	Steps the ros robot (and underlying executors and subrobots) through time. More...

RobotPtr	registerSubRobot (dart::dynamics::ReferentialSkeletonPtr refSkeleton, const std::string &name, const std::string &modeControllerName="")
	Registers a subset of the joints of the skeleton as a new RosRobot. More...

void	deactivateExecutor () override
	Stops the controller corresponding to this executor and deactivates executor. More...

int	registerExecutor (aikido::control::ExecutorPtr executor, std::string desiredName, std::string controllerName, hardware_interface::JointCommandModes controllerMode)
	Loads the controller. More...

int	registerExecutor (aikido::control::ExecutorPtr executor, std::string desiredName, std::string controllerName)
	Loads the controller. More...

bool	activateExecutor (const int id) override
	Deactivates the current active executor. More...

void	setRosControllerClients (const std::string modeControllerName)
	Sets the ros controller clients which enable controller listing, loading, switching and mode switching. More...

virtual bool	activateExecutor (const int id)
	Deactivates the current active executor. More...

virtual bool	activateExecutor (const std::string name)
	Convenience: Activates executor using name Deactivates the current active executor. More...

virtual bool	activateExecutor (const aikido::control::ExecutorType type)
	Convenience: Deactivates the current active executor. More...

Public Member Functions inherited from aikido::robot::Robot
	Robot (dart::dynamics::SkeletonPtr skeleton, const std::string name="robot", bool addDefaultExecutors=true)
	Construct a new Robot object. More...

	Robot (dart::dynamics::ReferentialSkeletonPtr refSkeleton, Robot *rootRobot, dart::collision::CollisionDetectorPtr collisionDetector, std::shared_ptr< dart::collision::BodyNodeCollisionFilter > collisionFilter, const std::string name="subrobot")
	Construct a new Robot as subrobot. More...

virtual	~Robot ()=default
	Destructor. More...

virtual void	ignoreSelfCollisionPairs (const std::vector< std::pair< std::string, std::string >> bodyNodeList)
	Manually add pairs of body nodes for which collision is ignored Note: adjacent body pairs are already ignored. More...

virtual void	enforceSelfCollisionPairs (const std::vector< std::pair< std::string, std::string >> bodyNodeList)
	Manually remove pairs of body nodes for which collision constraints are enforced. More...

constraint::dart::CollisionFreePtr	getSelfCollisionConstraint () const
	Get the self-collision constraint of the robot. More...

constraint::TestablePtr	combineCollisionConstraint (const constraint::dart::CollisionFreePtr &collisionFree) const
	Given a collision constraint, returns it combined with the self-collision constraint of the entire robot. More...

constraint::TestablePtr	getWorldCollisionConstraint (const std::vector< std::string > bodyNames=std::vector< std::string >()) const
	Get the collision constraint between this (sub)robot and selected bodies within its World, combined with its self-collision constraint. More...

virtual RobotPtr	registerSubRobot (dart::dynamics::ReferentialSkeletonPtr refSkeleton, const std::string &name)
	Registers a subset of the joints of the skeleton as a new robot. More...

RobotPtr	getSubRobotByName (const std::string &name) const
	Get existing subrobot pointer. More...

Eigen::VectorXd	getNamedConfiguration (const std::string &name) const
	Returns a named configuration, or size-0 vector if not found. More...

void	setNamedConfigurations (std::unordered_map< std::string, const Eigen::VectorXd > namedConfigurations)
	Sets the list of named configurations. More...

trajectory::TrajectoryPtr	planToConfiguration (const Eigen::VectorXd &goalConf, const constraint::TestablePtr &testableConstraint, const std::shared_ptr< planner::ConfigurationToConfigurationPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Plan the robot to a specific configuration. More...

trajectory::TrajectoryPtr	planToConfiguration (const Eigen::VectorXd &goalConf, const std::shared_ptr< planner::ConfigurationToConfigurationPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Default to selfCollisionConstraint. More...

trajectory::TrajectoryPtr	planToOffset (const std::string bodyNodeName, const Eigen::Vector3d &offset, const constraint::TestablePtr &testableConstraint, const std::shared_ptr< planner::dart::ConfigurationToEndEffectorOffsetPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Plan a specific body node of the robot to a position offset. More...

trajectory::TrajectoryPtr	planToOffset (const std::string bodyNodeName, const Eigen::Vector3d &offset, const std::shared_ptr< planner::dart::ConfigurationToEndEffectorOffsetPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Default to selfCollisionConstraint. More...

trajectory::TrajectoryPtr	planToPoseOffset (const std::string bodyNodeName, const Eigen::Vector3d &offset, const Eigen::Vector3d &rotation, const constraint::TestablePtr &testableConstraint, const std::shared_ptr< planner::dart::ConfigurationToEndEffectorPosePlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Plan a specific body node of the robot to a pose offset (i.e. More...

trajectory::TrajectoryPtr	planToPoseOffset (const std::string bodyNodeName, const Eigen::Vector3d &offset, const Eigen::Vector3d &rotation, const std::shared_ptr< planner::dart::ConfigurationToEndEffectorPosePlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr) const
	Default to selfCollisionConstraint. More...

trajectory::TrajectoryPtr	planToTSR (const std::string bodyNodeName, const constraint::dart::TSRPtr &tsr, const constraint::TestablePtr &testableConstraint, const util::PlanToTSRParameters &params=util::PlanToTSRParameters(), const std::shared_ptr< planner::ConfigurationToConfigurationPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr, const distance::ConstConfigurationRankerPtr &ranker=nullptr) const
	Plan a specific body node of the robot to a sample configuraiton within a Task Space Region. More...

trajectory::TrajectoryPtr	planToTSR (const std::string bodyNodeName, const constraint::dart::TSRPtr &tsr, const util::PlanToTSRParameters &params=util::PlanToTSRParameters(), const std::shared_ptr< planner::ConfigurationToConfigurationPlanner > &planner=nullptr, const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor=nullptr, const distance::ConstConfigurationRankerPtr &ranker=nullptr) const
	Default to selfCollisionConstraint. More...

virtual void	clearExecutors ()
	Deactivates the active executor. More...

virtual aikido::control::ExecutorPtr	getActiveExecutor ()
	Retrieves the active executor. More...

virtual int	registerExecutor (aikido::control::ExecutorPtr executor, std::string desiredName="")
	Add an executor to the inactive executors list. More...

virtual bool	activateExecutor (const std::string name)
	Convenience: Activates executor using name Deactivates the current active executor. More...

virtual bool	activateExecutor (const aikido::control::ExecutorType type)
	Convenience: Deactivates the current active executor. More...

template<aikido::control::ExecutorType T>
std::future< int >	executeJointCommand (const std::vector< double > &command, const std::chrono::duration< double > &timeout)
	Convenience: executes given joint command on the robot. More...

std::future< void >	executeTrajectory (const trajectory::TrajectoryPtr &trajectory)
	Convenience: executes given trajectory on the robot. More...

virtual void	cancelAllCommands (bool includeSubrobots=true, bool includeParents=false, const std::vector< std::string > excludeSubrobots=std::vector< std::string >())
	Cancels all running commands on this robot. More...

std::string	getName () const

dart::dynamics::MetaSkeletonPtr	getMetaSkeletonClone () const

dart::dynamics::MetaSkeletonPtr	getMetaSkeleton ()

const dart::dynamics::MetaSkeletonPtr	getMetaSkeleton () const

Eigen::VectorXd	getCurrentConfiguration () const

common::UniqueRNGPtr	cloneRNG () const

void	setRNG (common::UniqueRNGPtr rng)

aikido::planner::WorldPtr	getWorld () const

void	setWorld (aikido::planner::WorldPtr world)

void	setDefaultPostProcessor (const std::shared_ptr< aikido::planner::TrajectoryPostProcessor > trajPostProcessor)

std::shared_ptr< aikido::planner::TrajectoryPostProcessor >	getDefaultPostProcessor () const

void	setEnablePostProcessing (bool enable=true)

dart::dynamics::ConstSkeletonPtr	getRootSkeleton () const

dart::dynamics::SkeletonPtr	getRootSkeleton ()

util::VectorFieldPlannerParameters	getVFParams () const

void	setVFParams (util::VectorFieldPlannerParameters params)

Protected Member Functions
bool	startController (const std::string startControllerName)
	Starts the controller with name startControllerName. More...

bool	stopController (const std::string stopControllerName)
	Stops the controller with name stopControllerName. More...

bool	switchControllers (const std::vector< std::string > startControllersNames, const std::vector< std::string > stopControllersNames)
	Stops the controller with name stopControllerName and starts the controller with name startControllerName. More...

bool	loadController (const std::string loadControllerName)
	Loads the controller with name loadControllerName. More...

bool	switchControllerMode (const hardware_interface::JointCommandModes jointMode)
	Switches the controller mode to jointMode. More...

bool	isControllerActive (const std::string controllerName)
	Checks if the controller with name controllerName is active. More...

Protected Member Functions inherited from aikido::robot::Robot
virtual bool	validateSubRobot (dart::dynamics::ReferentialSkeletonPtr refSkeleton, const std::string &name)
	Checks validity of subrobot for registration: name should be unique. More...

Protected Attributes
std::unordered_map< int, hardware_interface::JointCommandModes >	mRosControllerModes

std::unordered_map< int, std::string >	mRosControllerNames

::ros::NodeHandle	mNode

std::shared_ptr<::ros::ServiceClient >	mRosListControllersServiceClient

std::shared_ptr<::ros::ServiceClient >	mRosLoadControllerServiceClient

std::shared_ptr<::ros::ServiceClient >	mRosSwitchControllerServiceClient

std::shared_ptr< aikido::control::ros::RosJointModeCommandClient >	mRosJointModeCommandClient

Protected Attributes inherited from aikido::robot::Robot
std::string	mName

dart::dynamics::MetaSkeletonPtr	mMetaSkeleton

aikido::statespace::dart::MetaSkeletonStateSpacePtr	mStateSpace

int	mActiveExecutor {-1}

std::vector< aikido::control::ExecutorPtr >	mExecutors

std::unordered_map< std::string, int >	mExecutorsNameMap

Robot *	mParentRobot {nullptr}

std::set< std::string >	mDofs

std::unordered_map< std::string, RobotPtr >	mSubRobots

dart::collision::CollisionDetectorPtr	mCollisionDetector

std::shared_ptr< dart::collision::BodyNodeCollisionFilter >	mSelfCollisionFilter

bool	mEnablePostProcessing {false}

std::shared_ptr< aikido::planner::TrajectoryPostProcessor >	mDefaultPostProcessor {nullptr}

std::unique_ptr< common::RNG >	mRng

aikido::planner::WorldPtr	mWorld {nullptr}

ConfigurationMap	mNamedConfigurations

util::VectorFieldPlannerParameters	mVFParams

Additional Inherited Members
Protected Types inherited from aikido::robot::Robot
using	ConfigurationMap = std::unordered_map< std::string, const Eigen::VectorXd >

Detailed Description

Robot interface augmented with ROS components.

Constructor & Destructor Documentation

◆ RosRobot() [1/2]

aikido::robot::ros::RosRobot::RosRobot	(	const dart::common::Uri &	urdf,
		const dart::common::Uri &	srdf,
		const std::string	name,
		const bool	addDefaultExecutors,
		const dart::common::ResourceRetrieverPtr &	retriever = `std::make_shared< aikido::io::CatkinResourceRetriever >()`,
		const ::ros::NodeHandle &	node = `::ros::NodeHandle()`,
		const std::string	modeControllerName = `""`
	)

Construct a new RosRobot object.

Parameters

[in]	urdf	The path to the robot's URDF.
[in]	srdf	The path to the robot's SRDF.
[in]	name	The name of the robot.
[in]	retriever	Retriever to access resources (urdf/srdf).
[in]	node	ROS node handle.

◆ RosRobot() [2/2]

aikido::robot::ros::RosRobot::RosRobot	(	dart::dynamics::ReferentialSkeletonPtr	refSkeleton,
		Robot *	rootRobot,
		dart::collision::CollisionDetectorPtr	collisionDetector,
		std::shared_ptr< dart::collision::BodyNodeCollisionFilter >	collisionFilter,
		const std::string	name,
		const ::ros::NodeHandle &	node = `::ros::NodeHandle()`,
		const std::string	modeControllerName = `""`
	)

Construct a new RosRobot as subrobot.

Documentation Inherited

◆ ~RosRobot()

virtual aikido::robot::ros::RosRobot::~RosRobot ( )

virtualdefault

Destructor.

Member Function Documentation

◆ activateExecutor() [1/4]

virtual bool aikido::robot::Robot::activateExecutor

Convenience: Deactivates the current active executor.

Activates the most recently registered executor of the given type.

Parameters

[in] type of executor to activate.

Returns: True if successful. If false, all executors are inactive.

◆ activateExecutor() [2/4]

virtual bool aikido::robot::Robot::activateExecutor

Deactivates the current active executor.

Sets an executor from the inactive executor list to be the active executor. Holds and releases DoFs as needed.

Parameters

[in] id of executor on executor list

Returns: True if successful. If false, all executors are inactive.

◆ activateExecutor() [3/4]

bool aikido::robot::ros::RosRobot::activateExecutor ( const int id )

overridevirtual

Deactivates the current active executor.

Starts the corresponding controller and switches to corresponding control mode. Sets an executor from the inactive executor list to be the active executor. Holds and releases DoFs as needed.

Parameters

[in] id of executor on executor list

Returns: True if successful. If false, all executors are inactive.

Reimplemented from aikido::robot::Robot.

◆ activateExecutor() [4/4]

virtual bool aikido::robot::Robot::activateExecutor

Convenience: Activates executor using name Deactivates the current active executor.

Sets an executor from the inactive executor list to be the active executor. Holds and releases DoFs as needed.

Parameters

[in] id of executor on executor list

Returns: True if successful. If false, all executors are inactive.

◆ deactivateExecutor()

void aikido::robot::ros::RosRobot::deactivateExecutor ( )

overridevirtual

Stops the controller corresponding to this executor and deactivates executor.

Throws a runtime_error if controller cannot be stopped.

Reimplemented from aikido::robot::Robot.

◆ isControllerActive()

bool aikido::robot::ros::RosRobot::isControllerActive ( const std::string controllerName )

protected

Checks if the controller with name controllerName is active.

Parameters

[in] controllerName name of controller to check.

Returns: True if controller is active. Otherwise false.

◆ loadController()

bool aikido::robot::ros::RosRobot::loadController ( const std::string loadControllerName )

protected

Loads the controller with name loadControllerName.

Parameters

[in] loadControllerName name of controller to load.

Returns: True if successful. Otherwise false.

◆ registerExecutor() [1/2]

int aikido::robot::ros::RosRobot::registerExecutor	(	aikido::control::ExecutorPtr	executor,
		std::string	desiredName,
		std::string	controllerName
	)

Loads the controller.

Warns if controller cannot be loaded. Adds an (executor, controller) to the inactive (executor, controller, control mode) list. Operations on this executor do not affect the control mode. Releases DoFs held by executor if held.

Parameters

[in]	executor	The Executor to add to the inactive list.
[in]	desiredName	The desired name for the executor.
[in]	controllerName	The corresponding controller to load and add to the inactive list.

Returns: A robot-unique non-negative ID (negative implies failure).

◆ registerExecutor() [2/2]

int aikido::robot::ros::RosRobot::registerExecutor	(	aikido::control::ExecutorPtr	executor,
		std::string	desiredName,
		std::string	controllerName,
		hardware_interface::JointCommandModes	controllerMode
	)

Loads the controller.

Warns if controller cannot be loaded. Adds an (executor, controller, control mode) to the inactive (executor, controller, control mode) list. Releases DoFs held by executor if held.

Parameters

[in]	executor	The Executor to add to the inactive list.
[in]	desiredName	The desired name for the executor.
[in]	controllerName	The corresponding controller to load and add to the inactive list.
[in]	controllerMode	The corresponding control mode to add to the inactive list.

Returns: A robot-unique non-negative ID (negative implies failure).

◆ registerSubRobot()

RobotPtr aikido::robot::ros::RosRobot::registerSubRobot	(	dart::dynamics::ReferentialSkeletonPtr	refSkeleton,
		const std::string &	name,
		const std::string &	modeControllerName = `""`
	)

Registers a subset of the joints of the skeleton as a new RosRobot.

Must be disjoint from other subrobots.

Parameters

[in]	metaSkeleton	The referential skeleton corresponding to the subrobot.
[in]	name	Name of the subrobot.

◆ setRosControllerClients()

void aikido::robot::ros::RosRobot::setRosControllerClients ( const std::string modeControllerName )

Sets the ros controller clients which enable controller listing, loading, switching and mode switching.

Parameters

[in] modeControllerName ros joint mode command client namespace.

◆ startController()

bool aikido::robot::ros::RosRobot::startController ( const std::string startControllerName )

protected

Starts the controller with name startControllerName.

Parameters

[in] startControllerName name of controller to start.

Returns: True if successful. Otherwise false.

◆ step()

void aikido::robot::ros::RosRobot::step ( const std::chrono::system_clock::time_point & timepoint )

overridevirtual

Steps the ros robot (and underlying executors and subrobots) through time.

Call regularly to update the state of the ros robot.

Parameters

[in] timepoint The point in time to step to.

Reimplemented from aikido::robot::Robot.

◆ stopController()

bool aikido::robot::ros::RosRobot::stopController ( const std::string stopControllerName )

protected

Stops the controller with name stopControllerName.

Parameters

[in] stopControllerName name of controller to stop.

Returns: True if successful. Otherwise false.

◆ switchControllerMode()

bool aikido::robot::ros::RosRobot::switchControllerMode ( const hardware_interface::JointCommandModes jointMode )

protected

Switches the controller mode to jointMode.

Parameters

[in] jointMode control mode to switch to.

Returns: True if successful. Otherwise false.

◆ switchControllers()

bool aikido::robot::ros::RosRobot::switchControllers	(	const std::vector< std::string >	startControllersNames,
		const std::vector< std::string >	stopControllersNames
	)

protected

Stops the controller with name stopControllerName and starts the controller with name startControllerName.

Parameters

[in]	startControllersNames	list of names of controllers to start.
[in]	stopControllersNames	list of names of controllers to stop.

Returns: True if successful. Otherwise false.

Member Data Documentation

◆ mNode

::ros::NodeHandle aikido::robot::ros::RosRobot::mNode

protected

◆ mRosControllerModes

std::unordered_map<int, hardware_interface::JointCommandModes> aikido::robot::ros::RosRobot::mRosControllerModes

protected

◆ mRosControllerNames

std::unordered_map<int, std::string> aikido::robot::ros::RosRobot::mRosControllerNames

protected

◆ mRosJointModeCommandClient

std::shared_ptr<aikido::control::ros::RosJointModeCommandClient> aikido::robot::ros::RosRobot::mRosJointModeCommandClient

protected

◆ mRosListControllersServiceClient

std::shared_ptr<::ros::ServiceClient> aikido::robot::ros::RosRobot::mRosListControllersServiceClient

protected

◆ mRosLoadControllerServiceClient

std::shared_ptr<::ros::ServiceClient> aikido::robot::ros::RosRobot::mRosLoadControllerServiceClient

protected

◆ mRosSwitchControllerServiceClient

std::shared_ptr<::ros::ServiceClient> aikido::robot::ros::RosRobot::mRosSwitchControllerServiceClient

protected

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ RosRobot() [1/2]

◆ RosRobot() [2/2]

◆ ~RosRobot()

Member Function Documentation

◆ activateExecutor() [1/4]

◆ activateExecutor() [2/4]

◆ activateExecutor() [3/4]

◆ activateExecutor() [4/4]

◆ deactivateExecutor()

◆ isControllerActive()

◆ loadController()

◆ registerExecutor() [1/2]

◆ registerExecutor() [2/2]

◆ registerSubRobot()

◆ setRosControllerClients()

◆ startController()

◆ step()

◆ stopController()

◆ switchControllerMode()

◆ switchControllers()

Member Data Documentation

◆ mNode

◆ mRosControllerModes

◆ mRosControllerNames

◆ mRosJointModeCommandClient

◆ mRosListControllersServiceClient

◆ mRosLoadControllerServiceClient

◆ mRosSwitchControllerServiceClient