diff --git a/CHANGELOG.md b/CHANGELOG.md index d1d879a8706e76f77119a42025a5cfe61cc43167..486286c1f37e4d6f7dbb86fd77053a5bd5399fb1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,8 +4,8 @@ * Added missing run-time dependencies to `franka_description` and `franka_control` * Added epsilon to grasp action - * Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total` - to the robot state + * Added `tau_J_d`, `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal`, `I_total`, + `theta` and `dtheta` to `franka::RobotState` * Updated and improved examples * Fixed includes for Eigen3 in `franka_example_controllers` * Fixed gripper state publishing in `franka_gripper_node` diff --git a/franka_control/src/franka_state_controller.cpp b/franka_control/src/franka_state_controller.cpp index 2fdcdbad2d65c137d1ca0915d46cc73b139a53e8..fb6d22e08cb38eb622cbb187279e20faaeed5c79 100644 --- a/franka_control/src/franka_state_controller.cpp +++ b/franka_control/src/franka_state_controller.cpp @@ -255,6 +255,12 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) { "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtau_J), "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.tau_J_d), + "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.theta), + "Robot state joint members do not have same size"); + static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.dtheta), + "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_collision), "Robot state joint members do not have same size"); static_assert(sizeof(robot_state_.q) == sizeof(robot_state_.joint_contact), @@ -268,6 +274,9 @@ void FrankaStateController::publishFrankaStates(const ros::Time& time) { publisher_franka_states_.msg_.dq_d[i] = robot_state_.dq_d[i]; publisher_franka_states_.msg_.tau_J[i] = robot_state_.tau_J[i]; publisher_franka_states_.msg_.dtau_J[i] = robot_state_.dtau_J[i]; + publisher_franka_states_.msg_.tau_J_d[i] = robot_state_.tau_J_d[i]; + publisher_franka_states_.msg_.theta[i] = robot_state_.theta[i]; + publisher_franka_states_.msg_.dtheta[i] = robot_state_.dtheta[i]; publisher_franka_states_.msg_.joint_collision[i] = robot_state_.joint_collision[i]; publisher_franka_states_.msg_.joint_contact[i] = robot_state_.joint_contact[i]; publisher_franka_states_.msg_.tau_ext_hat_filtered[i] = robot_state_.tau_ext_hat_filtered[i]; diff --git a/franka_msgs/msg/FrankaState.msg b/franka_msgs/msg/FrankaState.msg index fea9184f82ca538953f37027a330cbf132d9eda5..7f8f376a1d21fa6166e05ee4952995bbf1b3b2d5 100644 --- a/franka_msgs/msg/FrankaState.msg +++ b/franka_msgs/msg/FrankaState.msg @@ -5,8 +5,11 @@ float64[7] q float64[7] q_d float64[7] dq float64[7] dq_d +float64[7] theta +float64[7] dtheta float64[7] tau_J float64[7] dtau_J +float64[7] tau_J_d float64[6] K_F_ext_hat_K float64[2] elbow float64[2] elbow_d