Controller class for ros_control that renders two decoupled Cartesian impedances for the tracking of two target poses for the two endeffectors. More...

#include <dual_arm_cartesian_impedance_example_controller.h>

Inheritance diagram for franka_example_controllers::DualArmCartesianImpedanceExampleController:

[legend]

Public Member Functions
bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &node_handle) override
	Initializes the controller class to be ready to run. More...

void	starting (const ros::Time &) override
	Prepares the controller for the real-time execution. More...

void	update (const ros::Time &, const ros::Duration &period) override
	Computes the control-law and commands the resulting joint torques to the robot. More...

Public Member Functions inherited from controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface >
virtual bool	init (hardware_interface::RobotHW *, ros::NodeHandle &, ros::NodeHandle &)

	MultiInterfaceController (bool allow_optional_interfaces=false)

virtual	~MultiInterfaceController ()

Public Member Functions inherited from controller_interface::ControllerBase
	ControllerBase ()

bool	isRunning ()

bool	isRunning ()

bool	startRequest (const ros::Time &time)

bool	startRequest (const ros::Time &time)

virtual void	stopping (const ros::Time &)

virtual void	stopping (const ros::Time &)

bool	stopRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

virtual	~ControllerBase ()

Private Member Functions
void	complianceParamCallback (franka_combined_example_controllers::dual_arm_compliance_paramConfig &config, uint32_t)
	Callback for updates on the parameterization of the controller in terms of stiffnesses. More...

bool	initArm (hardware_interface::RobotHW *robot_hw, const std::string &arm_id, const std::vector< std::string > &joint_names)
	Initializes a single Panda robot arm. More...

void	publishCenteringPose ()
	Publishes a Pose Stamped for visualization of the current centering pose. More...

Eigen::Matrix< double, 7, 1 >	saturateTorqueRate (const FrankaDataContainer &arm_data, const Eigen::Matrix< double, 7, 1 > &tau_d_calculated, const Eigen::Matrix< double, 7, 1 > &tau_J_d)
	Saturates torque commands to ensure feasibility. More...

void	startingArm (FrankaDataContainer &arm_data)
	Prepares all internal states to be ready to run the real-time control for one arm. More...

void	targetPoseCallback (const geometry_msgs::PoseStamped::ConstPtr &msg)
	Callback method that handles updates of the target poses. More...

void	updateArm (FrankaDataContainer &arm_data)
	Computes the decoupled controller update for a single arm. More...

Private Attributes
std::map< std::string, FrankaDataContainer >	arms_data_
	Holds all relevant data for both arms. More...

realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped >	center_frame_pub_
	Rate to trigger publishing the current pose of the centering frame. More...

ros::NodeHandle	dynamic_reconfigure_compliance_param_node_

std::unique_ptr< dynamic_reconfigure::Server< franka_combined_example_controllers::dual_arm_compliance_paramConfig > >	dynamic_server_compliance_param_
	Nodehandle for the dynamic reconfigure namespace. More...

Eigen::Affine3d	EEl_T_C_
	< Transformation from the centering frame to the left end effector. More...

Eigen::Affine3d	EEr_T_EEl_
	< Target transformation between the two endeffectors. More...

std::string	left_arm_id_
	Name of the left arm, retrieved from the parameter server. More...

Eigen::Affine3d	Ol_T_Or_

franka_hw::TriggerRate	publish_rate_

std::string	right_arm_id_
	Name of the right arm, retrieved from the parameter server. More...

ros::Subscriber	sub_target_pose_left_

Additional Inherited Members
Public Types inherited from controller_interface::ControllerBase
typedef std::vector< hardware_interface::InterfaceResources >	ClaimedResources

Public Attributes inherited from controller_interface::ControllerBase
	CONSTRUCTED

	INITIALIZED

	RUNNING

enum controller_interface::ControllerBase:: { ... }	state_

Protected Member Functions inherited from controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface >
virtual bool	initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources)

Static Protected Member Functions inherited from controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface >
static void	clearClaims (hardware_interface::RobotHW *robot_hw)

static void	extractInterfaceResources (hardware_interface::RobotHW robot_hw_in, hardware_interface::RobotHW robot_hw_out)

static bool	hasRequiredInterfaces (hardware_interface::RobotHW *robot_hw)

static void	populateClaimedResources (hardware_interface::RobotHW *robot_hw, ClaimedResources &claimed_resources)

Protected Attributes inherited from controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface >
bool	allow_optional_interfaces_

hardware_interface::RobotHW	robot_hw_ctrl_

Detailed Description

Controller class for ros_control that renders two decoupled Cartesian impedances for the tracking of two target poses for the two endeffectors.

The controller can be reparameterized at runtime via dynamic reconfigure servers.

Definition at line 63 of file dual_arm_cartesian_impedance_example_controller.h.

Member Function Documentation

void franka_example_controllers::DualArmCartesianImpedanceExampleController::complianceParamCallback	(	franka_combined_example_controllers::dual_arm_compliance_paramConfig &	config,
		uint32_t
	)

private

Callback for updates on the parameterization of the controller in terms of stiffnesses.

Parameters

[in] config Data container for configuration updates.Target pose subscriber

Definition at line 316 of file dual_arm_cartesian_impedance_example_controller.cpp.

bool franka_example_controllers::DualArmCartesianImpedanceExampleController::init	(	hardware_interface::RobotHW *	robot_hw,
		ros::NodeHandle &	node_handle
	)

overridevirtual

Initializes the controller class to be ready to run.

Parameters

[in]	robot_hw	Pointer to a RobotHW class to get interfaces and resource handles.
[in]	node_handle	Nodehandle that allows getting parameterizations from the server and starting subscribers.

Returns: True if the controller was initialized successfully, false otherwise.

Reimplemented from controller_interface::MultiInterfaceController< franka_hw::FrankaModelInterface, hardware_interface::EffortJointInterface, franka_hw::FrankaStateInterface >.

Definition at line 93 of file dual_arm_cartesian_impedance_example_controller.cpp.

bool franka_example_controllers::DualArmCartesianImpedanceExampleController::initArm	(	hardware_interface::RobotHW *	robot_hw,
		const std::string &	arm_id,
		const std::vector< std::string > &	joint_names
	)

private

Initializes a single Panda robot arm.

Parameters

[in]	robot_hw	A pointer the RobotHW class for getting interfaces and resource handles.
[in]	arm_id	The name of the panda arm.
[in]	joint_names	The names of all joints of the panda.

Returns: True if successful, false otherwise.

Definition at line 23 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::publishCenteringPose ( )

private

Publishes a Pose Stamped for visualization of the current centering pose.

Definition at line 396 of file dual_arm_cartesian_impedance_example_controller.cpp.

Eigen::Matrix< double, 7, 1 > franka_example_controllers::DualArmCartesianImpedanceExampleController::saturateTorqueRate	(	const FrankaDataContainer &	arm_data,
		const Eigen::Matrix< double, 7, 1 > &	tau_d_calculated,
		const Eigen::Matrix< double, 7, 1 > &	tau_J_d
	)

private

Saturates torque commands to ensure feasibility.

Parameters

[in]	arm_data	The data container of the arm.
[in]	tau_d_calculated	The raw command according to the control law.
[in]	tau_J_d	The current desired torque, read from the robot state.

Returns: The saturated torque command for the 7 joints of one arm.

Definition at line 303 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::starting ( const ros::Time & )

overridevirtual

Prepares the controller for the real-time execution.

This method is executed once every time the controller is started and runs in real-time.

Reimplemented from controller_interface::ControllerBase.

Definition at line 200 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::startingArm ( FrankaDataContainer & arm_data )

private

Prepares all internal states to be ready to run the real-time control for one arm.

Parameters

[in] arm_data The data container of the arm to prepare for the control loop.Dynamic reconfigure server

Definition at line 177 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::targetPoseCallback ( const geometry_msgs::PoseStamped::ConstPtr & msg )

private

Callback method that handles updates of the target poses.

Parameters

[in] msg New target pose.

Definition at line 348 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::update	(	const ros::Time &	,
		const ros::Duration &	period
	)

overridevirtual

Computes the control-law and commands the resulting joint torques to the robot.

Parameters

[in] period The control period (here 0.001s).

Implements controller_interface::ControllerBase.

Definition at line 219 of file dual_arm_cartesian_impedance_example_controller.cpp.

void franka_example_controllers::DualArmCartesianImpedanceExampleController::updateArm ( FrankaDataContainer & arm_data )

private

Computes the decoupled controller update for a single arm.

Parameters

[in] arm_data The data container of the arm to control.

Definition at line 229 of file dual_arm_cartesian_impedance_example_controller.cpp.

Member Data Documentation

std::map<std::string, FrankaDataContainer> franka_example_controllers::DualArmCartesianImpedanceExampleController::arms_data_

private

Holds all relevant data for both arms.

Definition at line 94 of file dual_arm_cartesian_impedance_example_controller.h.

realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> franka_example_controllers::DualArmCartesianImpedanceExampleController::center_frame_pub_

private

Rate to trigger publishing the current pose of the centering frame.

Definition at line 106 of file dual_arm_cartesian_impedance_example_controller.h.

ros::NodeHandle franka_example_controllers::DualArmCartesianImpedanceExampleController::dynamic_reconfigure_compliance_param_node_

private

Definition at line 155 of file dual_arm_cartesian_impedance_example_controller.h.

std::unique_ptr<dynamic_reconfigure::Server< franka_combined_example_controllers::dual_arm_compliance_paramConfig> > franka_example_controllers::DualArmCartesianImpedanceExampleController::dynamic_server_compliance_param_

private

Nodehandle for the dynamic reconfigure namespace.

Definition at line 152 of file dual_arm_cartesian_impedance_example_controller.h.

Eigen::Affine3d franka_example_controllers::DualArmCartesianImpedanceExampleController::EEl_T_C_

private

< Transformation from the centering frame to the left end effector.

Publisher for the centering tracking frame of the coordinated motion.

Definition at line 103 of file dual_arm_cartesian_impedance_example_controller.h.

Eigen::Affine3d franka_example_controllers::DualArmCartesianImpedanceExampleController::EEr_T_EEl_

private

< Target transformation between the two endeffectors.

Definition at line 101 of file dual_arm_cartesian_impedance_example_controller.h.

std::string franka_example_controllers::DualArmCartesianImpedanceExampleController::left_arm_id_

private

Name of the left arm, retrieved from the parameter server.

Definition at line 95 of file dual_arm_cartesian_impedance_example_controller.h.

Eigen::Affine3d franka_example_controllers::DualArmCartesianImpedanceExampleController::Ol_T_Or_

private

Definition at line 99 of file dual_arm_cartesian_impedance_example_controller.h.

franka_hw::TriggerRate franka_example_controllers::DualArmCartesianImpedanceExampleController::publish_rate_

private

Definition at line 108 of file dual_arm_cartesian_impedance_example_controller.h.

std::string franka_example_controllers::DualArmCartesianImpedanceExampleController::right_arm_id_

private

Name of the right arm, retrieved from the parameter server.

Transformation between base frames of the robots.

Definition at line 96 of file dual_arm_cartesian_impedance_example_controller.h.

ros::Subscriber franka_example_controllers::DualArmCartesianImpedanceExampleController::sub_target_pose_left_

private

Definition at line 167 of file dual_arm_cartesian_impedance_example_controller.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Function Documentation

Member Data Documentation