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