From 5fa9af12342c055be005a8b96442c39fec8dea92 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Mon, 11 Mar 2024 15:25:01 +0100 Subject: [PATCH] add joint commands to cartesian ctrl status --- .../src/cartesian_admittance_rule.cpp | 11 +++++++++++ .../msg/AdmittanceControllerState.msg | 3 +++ 2 files changed, 14 insertions(+) diff --git a/cartesian_admittance_controller/src/cartesian_admittance_rule.cpp b/cartesian_admittance_controller/src/cartesian_admittance_rule.cpp index eea0e06..dafa974 100644 --- a/cartesian_admittance_controller/src/cartesian_admittance_rule.cpp +++ b/cartesian_admittance_controller/src/cartesian_admittance_rule.cpp @@ -309,6 +309,17 @@ CartesianAdmittanceRule::controller_state_to_msg( // Fill commands admittance_state_msg.robot_command_twist = Eigen::toMsg(admittance_state_.robot_command_twist); + admittance_state_msg.joint_command_position.resize(num_joints_); + admittance_state_msg.joint_command_velocity.resize(num_joints_); + admittance_state_msg.joint_command_acceleration.resize(num_joints_); + + for (int i = 0; i < num_joints_; i++) { + admittance_state_msg.joint_command_position[i] = admittance_state_.joint_command_position[i]; + admittance_state_msg.joint_command_velocity[i] = admittance_state_.joint_command_velocity[i]; + admittance_state_msg.joint_command_acceleration[i] = + admittance_state_.joint_command_acceleration[i]; + } + // Fill diagnostic data admittance_state_msg.diagnostic_data.keys.clear(); admittance_state_msg.diagnostic_data.values.clear(); diff --git a/cartesian_control_msgs/msg/AdmittanceControllerState.msg b/cartesian_control_msgs/msg/AdmittanceControllerState.msg index 4d29e25..26371b8 100644 --- a/cartesian_control_msgs/msg/AdmittanceControllerState.msg +++ b/cartesian_control_msgs/msg/AdmittanceControllerState.msg @@ -21,6 +21,9 @@ std_msgs/Float64MultiArray rendered_stiffness # optional # Commands geometry_msgs/Twist robot_command_twist +float64[] joint_command_position +float64[] joint_command_velocity +float64[] joint_command_acceleration # Misc. cartesian_control_msgs/KeyValues diagnostic_data