#include <cartesian_impedance_controller.h>

Inheritance diagram for serl_franka_controllers::CartesianImpedanceController:

Public Member Functions
bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &node_handle) override

void	starting (const ros::Time &) override

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

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)

Public Member Functions inherited from controller_interface::ControllerBase
virtual void	aborting (const ros::Time &)

virtual void	aborting (const ros::Time &)

bool	abortRequest (const ros::Time &time)

bool	abortRequest (const ros::Time &time)

	ControllerBase ()=default

	ControllerBase (const ControllerBase &)=delete

	ControllerBase (ControllerBase &&)=delete

bool	isAborted () const

bool	isAborted () const

bool	isInitialized () const

bool	isInitialized () const

bool	isRunning () const

bool	isRunning () const

bool	isStopped () const

bool	isStopped () const

bool	isWaiting () const

bool	isWaiting () const

ControllerBase &	operator= (const ControllerBase &)=delete

ControllerBase &	operator= (ControllerBase &&)=delete

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 void	waiting (const ros::Time &)

virtual void	waiting (const ros::Time &)

bool	waitRequest (const ros::Time &time)

bool	waitRequest (const ros::Time &time)

virtual	~ControllerBase ()=default

Private Member Functions
void	complianceParamCallback (serl_franka_controllers::compliance_paramConfig &config, uint32_t level)

void	equilibriumPoseCallback (const geometry_msgs::PoseStampedConstPtr &msg)

void	publishDebug (const ros::Time &time)

void	publishZeroJacobian (const ros::Time &time)

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

Private Attributes
Eigen::Matrix< double, 6, 6 >	cartesian_damping_

Eigen::Matrix< double, 6, 6 >	cartesian_damping_target_

Eigen::Matrix< double, 6, 6 >	cartesian_stiffness_

Eigen::Matrix< double, 6, 6 >	cartesian_stiffness_target_

const double	delta_tau_max_ {1.0}

ros::NodeHandle	dynamic_reconfigure_compliance_param_node_

std::unique_ptr< dynamic_reconfigure::Server< serl_franka_controllers::compliance_paramConfig > >	dynamic_server_compliance_param_

Eigen::Matrix< double, 6, 1 >	error_

Eigen::Matrix< double, 6, 1 >	error_i

double	filter_params_ {0.005}

std::array< double, 42 >	jacobian_array

double	joint1_nullspace_stiffness_ {20.0}

double	joint1_nullspace_stiffness_target_ {20.0}

std::vector< hardware_interface::JointHandle >	joint_handles_

Eigen::Matrix< double, 6, 6 >	Ki_

Eigen::Matrix< double, 6, 6 >	Ki_target_

std::unique_ptr< franka_hw::FrankaModelHandle >	model_handle_

double	nullspace_stiffness_ {20.0}

double	nullspace_stiffness_target_ {20.0}

Eigen::Quaterniond	orientation_d_

Eigen::Quaterniond	orientation_d_target_

Eigen::Vector3d	position_d_

Eigen::Vector3d	position_d_target_

realtime_tools::RealtimePublisher< serl_franka_controllers::ZeroJacobian >	publisher_franka_jacobian_

Eigen::Matrix< double, 7, 1 >	q_d_nullspace_

Eigen::Matrix< double, 3, 1 >	rotational_clip_max_

Eigen::Matrix< double, 3, 1 >	rotational_clip_min_

std::unique_ptr< franka_hw::FrankaStateHandle >	state_handle_

ros::Subscriber	sub_equilibrium_pose_

Eigen::Matrix< double, 3, 1 >	translational_clip_max_

Eigen::Matrix< double, 3, 1 >	translational_clip_min_

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

enum	ControllerState { ControllerState::CONSTRUCTED, ControllerState::INITIALIZED, ControllerState::RUNNING, ControllerState::STOPPED, ControllerState::WAITING, ControllerState::ABORTED }

Public Attributes inherited from controller_interface::ControllerBase
ControllerState	state_

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

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

Definition at line 27 of file cartesian_impedance_controller.h.

Member Function Documentation

◆ complianceParamCallback()

void serl_franka_controllers::CartesianImpedanceController::complianceParamCallback	(	serl_franka_controllers::compliance_paramConfig &	config,
		uint32_t	level
	)

private

Definition at line 245 of file cartesian_impedance_controller.cpp.

◆ equilibriumPoseCallback()

void serl_franka_controllers::CartesianImpedanceController::equilibriumPoseCallback ( const geometry_msgs::PoseStampedConstPtr & msg )

private

Definition at line 275 of file cartesian_impedance_controller.cpp.

◆ init()

bool serl_franka_controllers::CartesianImpedanceController::init	(	hardware_interface::RobotHW *	robot_hw,
		ros::NodeHandle &	node_handle
	)

overridevirtual

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

Definition at line 21 of file cartesian_impedance_controller.cpp.

◆ publishDebug()

void serl_franka_controllers::CartesianImpedanceController::publishDebug ( const ros::Time & time )

private

◆ publishZeroJacobian()

void serl_franka_controllers::CartesianImpedanceController::publishZeroJacobian ( const ros::Time & time )

private

Definition at line 223 of file cartesian_impedance_controller.cpp.

◆ saturateTorqueRate()

Eigen::Matrix< double, 7, 1 > serl_franka_controllers::CartesianImpedanceController::saturateTorqueRate	(	const Eigen::Matrix< double, 7, 1 > &	tau_d_calculated,
		const Eigen::Matrix< double, 7, 1 > &	tau_J_d
	)

private

Definition at line 233 of file cartesian_impedance_controller.cpp.

◆ starting()

void serl_franka_controllers::CartesianImpedanceController::starting ( const ros::Time & )

overridevirtual

Reimplemented from controller_interface::ControllerBase.

Definition at line 113 of file cartesian_impedance_controller.cpp.

◆ update()

void serl_franka_controllers::CartesianImpedanceController::update	(	const ros::Time &	time,
		const ros::Duration &	period
	)

overridevirtual

Implements controller_interface::ControllerBase.

Definition at line 134 of file cartesian_impedance_controller.cpp.

Member Data Documentation

◆ cartesian_damping_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::cartesian_damping_

private

Definition at line 55 of file cartesian_impedance_controller.h.

◆ cartesian_damping_target_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::cartesian_damping_target_

private

Definition at line 56 of file cartesian_impedance_controller.h.

◆ cartesian_stiffness_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::cartesian_stiffness_

private

Definition at line 53 of file cartesian_impedance_controller.h.

◆ cartesian_stiffness_target_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::cartesian_stiffness_target_

private

Definition at line 54 of file cartesian_impedance_controller.h.

◆ delta_tau_max_

const double serl_franka_controllers::CartesianImpedanceController::delta_tau_max_ {1.0}

private

Definition at line 52 of file cartesian_impedance_controller.h.

◆ dynamic_reconfigure_compliance_param_node_

ros::NodeHandle serl_franka_controllers::CartesianImpedanceController::dynamic_reconfigure_compliance_param_node_

private

Definition at line 76 of file cartesian_impedance_controller.h.

◆ dynamic_server_compliance_param_

std::unique_ptr<dynamic_reconfigure::Server<serl_franka_controllers::compliance_paramConfig> > serl_franka_controllers::CartesianImpedanceController::dynamic_server_compliance_param_

private

Definition at line 75 of file cartesian_impedance_controller.h.

◆ error_

Eigen::Matrix<double, 6, 1> serl_franka_controllers::CartesianImpedanceController::error_

private

Definition at line 67 of file cartesian_impedance_controller.h.

◆ error_i

Eigen::Matrix<double, 6, 1> serl_franka_controllers::CartesianImpedanceController::error_i

private

Definition at line 68 of file cartesian_impedance_controller.h.

◆ filter_params_

double serl_franka_controllers::CartesianImpedanceController::filter_params_ {0.005}

private

Definition at line 47 of file cartesian_impedance_controller.h.

◆ jacobian_array

std::array<double, 42> serl_franka_controllers::CartesianImpedanceController::jacobian_array

private

Definition at line 45 of file cartesian_impedance_controller.h.

◆ joint1_nullspace_stiffness_

double serl_franka_controllers::CartesianImpedanceController::joint1_nullspace_stiffness_ {20.0}

private

Definition at line 50 of file cartesian_impedance_controller.h.

◆ joint1_nullspace_stiffness_target_

double serl_franka_controllers::CartesianImpedanceController::joint1_nullspace_stiffness_target_ {20.0}

private

Definition at line 51 of file cartesian_impedance_controller.h.

◆ joint_handles_

std::vector<hardware_interface::JointHandle> serl_franka_controllers::CartesianImpedanceController::joint_handles_

private

Definition at line 44 of file cartesian_impedance_controller.h.

◆ Ki_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::Ki_

private

Definition at line 57 of file cartesian_impedance_controller.h.

◆ Ki_target_

Eigen::Matrix<double, 6, 6> serl_franka_controllers::CartesianImpedanceController::Ki_target_

private

Definition at line 58 of file cartesian_impedance_controller.h.

◆ model_handle_

std::unique_ptr<franka_hw::FrankaModelHandle> serl_franka_controllers::CartesianImpedanceController::model_handle_

private

Definition at line 43 of file cartesian_impedance_controller.h.

◆ nullspace_stiffness_

double serl_franka_controllers::CartesianImpedanceController::nullspace_stiffness_ {20.0}

private

Definition at line 48 of file cartesian_impedance_controller.h.

◆ nullspace_stiffness_target_

double serl_franka_controllers::CartesianImpedanceController::nullspace_stiffness_target_ {20.0}

private

Definition at line 49 of file cartesian_impedance_controller.h.

◆ orientation_d_

Eigen::Quaterniond serl_franka_controllers::CartesianImpedanceController::orientation_d_

private

Definition at line 69 of file cartesian_impedance_controller.h.

◆ orientation_d_target_

Eigen::Quaterniond serl_franka_controllers::CartesianImpedanceController::orientation_d_target_

private

Definition at line 71 of file cartesian_impedance_controller.h.

◆ position_d_

Eigen::Vector3d serl_franka_controllers::CartesianImpedanceController::position_d_

private

Definition at line 66 of file cartesian_impedance_controller.h.

◆ position_d_target_

Eigen::Vector3d serl_franka_controllers::CartesianImpedanceController::position_d_target_

private

Definition at line 70 of file cartesian_impedance_controller.h.

◆ publisher_franka_jacobian_

realtime_tools::RealtimePublisher<serl_franka_controllers::ZeroJacobian> serl_franka_controllers::CartesianImpedanceController::publisher_franka_jacobian_

private

Definition at line 80 of file cartesian_impedance_controller.h.

◆ q_d_nullspace_

Eigen::Matrix<double, 7, 1> serl_franka_controllers::CartesianImpedanceController::q_d_nullspace_

private

Definition at line 65 of file cartesian_impedance_controller.h.

◆ rotational_clip_max_

Eigen::Matrix<double, 3, 1> serl_franka_controllers::CartesianImpedanceController::rotational_clip_max_

private

Definition at line 64 of file cartesian_impedance_controller.h.

◆ rotational_clip_min_

Eigen::Matrix<double, 3, 1> serl_franka_controllers::CartesianImpedanceController::rotational_clip_min_

private

Definition at line 63 of file cartesian_impedance_controller.h.

◆ state_handle_

std::unique_ptr<franka_hw::FrankaStateHandle> serl_franka_controllers::CartesianImpedanceController::state_handle_

private

Definition at line 42 of file cartesian_impedance_controller.h.

◆ sub_equilibrium_pose_

ros::Subscriber serl_franka_controllers::CartesianImpedanceController::sub_equilibrium_pose_

private

Definition at line 83 of file cartesian_impedance_controller.h.

◆ translational_clip_max_

Eigen::Matrix<double, 3, 1> serl_franka_controllers::CartesianImpedanceController::translational_clip_max_

private

Definition at line 62 of file cartesian_impedance_controller.h.

◆ translational_clip_min_

Eigen::Matrix<double, 3, 1> serl_franka_controllers::CartesianImpedanceController::translational_clip_min_

private

Definition at line 61 of file cartesian_impedance_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

◆ complianceParamCallback()

◆ equilibriumPoseCallback()

◆ init()

◆ publishDebug()

◆ publishZeroJacobian()

◆ saturateTorqueRate()

◆ starting()

◆ update()

Member Data Documentation

◆ cartesian_damping_

◆ cartesian_damping_target_

◆ cartesian_stiffness_

◆ cartesian_stiffness_target_

◆ delta_tau_max_

◆ dynamic_reconfigure_compliance_param_node_

◆ dynamic_server_compliance_param_

◆ error_

◆ error_i

◆ filter_params_

◆ jacobian_array

◆ joint1_nullspace_stiffness_

◆ joint1_nullspace_stiffness_target_

◆ joint_handles_

◆ Ki_

◆ Ki_target_

◆ model_handle_

◆ nullspace_stiffness_

◆ nullspace_stiffness_target_

◆ orientation_d_

◆ orientation_d_target_

◆ position_d_

◆ position_d_target_

◆ publisher_franka_jacobian_

◆ q_d_nullspace_

◆ rotational_clip_max_

◆ rotational_clip_min_

◆ state_handle_

◆ sub_equilibrium_pose_

◆ translational_clip_max_

◆ translational_clip_min_