add joint commands to cartesian ctrl status

ICube-Robotics · Mar 11, 2024 · 5fa9af1 · 5fa9af1
1 parent 636c7a0
commit 5fa9af1
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git 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
@@ -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