diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index 64c91616..4d858951 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -98,6 +98,7 @@ robot_name: lbr chain_root: lbr_link_0 chain_tip: lbr_link_ee + twist_in_tip_frame: true # if true, the twist command is expressed in the tip frame, otherwise in the root frame damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian max_linear_velocity: 0.1 # maximum linear velocity max_angular_velocity: 0.1 # maximum linear acceleration diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp index a10a2ba0..477ba98f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/twist_controller.hpp @@ -29,6 +29,7 @@ namespace lbr_ros2_control { struct TwistParameters { std::string chain_root; std::string chain_tip; + bool twist_in_tip_frame; double damping; double max_linear_velocity; double max_angular_velocity; diff --git a/lbr_ros2_control/src/controllers/twist_controller.cpp b/lbr_ros2_control/src/controllers/twist_controller.cpp index 0483180c..5cb7d7b2 100644 --- a/lbr_ros2_control/src/controllers/twist_controller.cpp +++ b/lbr_ros2_control/src/controllers/twist_controller.cpp @@ -25,6 +25,15 @@ void TwistImpl::compute(const geometry_msgs::msg::Twist::SharedPtr &twist_target return std::clamp(v, -parameters_.max_angular_velocity, parameters_.max_angular_velocity); }); + // if desired, transform to tip frame + if (parameters_.twist_in_tip_frame) { + auto chain_tip_frame = kinematics_ptr_->compute_fk(q); + twist_target_.topRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.topRows(3); + twist_target_.bottomRows(3) = + Eigen::Matrix3d::Map(chain_tip_frame.M.data).transpose() * twist_target_.bottomRows(3); + } + // compute jacobian auto jacobian = kinematics_ptr_->compute_jacobian(q); jacobian_inv_ = lbr_fri_ros2::pinv(jacobian.data, parameters_.damping); @@ -70,6 +79,7 @@ controller_interface::CallbackReturn TwistController::on_init() { this->get_node()->declare_parameter("robot_name", "lbr"); this->get_node()->declare_parameter("chain_root", "lbr_link_0"); this->get_node()->declare_parameter("chain_tip", "lbr_link_ee"); + this->get_node()->declare_parameter("twist_in_tip_frame", true); this->get_node()->declare_parameter("damping", 0.2); this->get_node()->declare_parameter("max_linear_velocity", 0.1); this->get_node()->declare_parameter("max_angular_velocity", 0.1); @@ -201,6 +211,7 @@ void TwistController::configure_twist_impl_() { this->get_robot_description(), TwistParameters{this->get_node()->get_parameter("chain_root").as_string(), this->get_node()->get_parameter("chain_tip").as_string(), + this->get_node()->get_parameter("twist_in_tip_frame").as_bool(), this->get_node()->get_parameter("damping").as_double(), this->get_node()->get_parameter("max_linear_velocity").as_double(), this->get_node()->get_parameter("max_angular_velocity").as_double()});