How to get current cartesian velocity and cartesian acceleration in franka_ros2_example_controllers

[ergo-zyh]

Hello, I am realizing a cartesain controller on my won, which need to get current cartesian velocity and cartesian acceleration. In franka_ros(ROS1), I can get this message by using state_handle_->getRobotState(). But I can't find any interface to get this message in franka_ros2. Can you help me? Thanks a lot !!!!

[AndreasKuhner]

Hi @ergo-zyh ,
you can check out the franka_robot_state_broadcaster which gives you access to the robot state: https://github.com/frankarobotics/franka_ros2/tree/humble/franka_robot_state_broadcaster. You can set it up similar to the example https://github.com/frankarobotics/franka_ros2/blob/humble/franka_example_controllers/include/franka_example_controllers/model_example_controller.hpp. Only, instead of the robot model, you have the robot state (aka using this header: https://github.com/frankarobotics/franka_ros2/blob/humble/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp)

Cheers,
Andreas

[ergo-zyh]

Thanks, I will try it!
I follow the sequence to get current robot state in running controller:

1. if (!franka_robot_state_) {
   franka_robot_state_ = std::make_unique<franka_semantic_components::FrankaRobotState>(
   franka_semantic_components::FrankaRobotState(arm_id_ + "/" + "robot_state",
   robot_description_));
   }
   franka_robot_state_->initialize_robot_state_msg(franka_robot_state_msg_);
2. franka_robot_state_->assign_loaned_state_interfaces(state_interfaces_);
3. franka_robot_state_->get_values_as_message(franka_robot_state_msg_);
   and I get current pose in auto current_pose = franka_robot_state_msg_.o_t_ee; But I get the error: what(): libfranka: Move command aborted: motion aborted by reflex! ["cartesian_position_motion_generator_start_pose_invalid"]
   Is there any difference in the current pose between franka_robot_state_ and franka_cartesian_pose_ ?
   Can you help me? Thanks.