diff --git a/cartesian_force_controller/README.md b/cartesian_force_controller/README.md index f3ed2dff..d264ebf2 100644 --- a/cartesian_force_controller/README.md +++ b/cartesian_force_controller/README.md @@ -24,6 +24,7 @@ my_cartesian_force_controller: end_effector_link: "tool0" robot_base_link: "base_link" ft_sensor_ref_link: "sensor_link" + hand_frame_control: true joints: - joint1 - joint2 diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp index 107d6499..aa0a3be3 100644 --- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp +++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp @@ -69,6 +69,15 @@ init(HardwareInterface* hw, ros::NodeHandle& nh) return false; } + if(nh.hasParam("hand_frame_control")) + { + if (!nh.getParam("hand_frame_control",m_hand_frame_control)) + { + ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/hand_frame_control" << " from parameter server"); + return false; + } + } + // Make sure sensor link is part of the robot chain if(!Base::robotChainContains(m_ft_sensor_ref_link)) {