From 64a3878a1f924e21f9822803ca173a1d82c4fa2c Mon Sep 17 00:00:00 2001 From: Andreas Kuhner Date: Wed, 6 Dec 2023 09:23:57 +0100 Subject: [PATCH] refactor: Moved more of the assign functionality into functions --- CHANGELOG.md | 9 ++- .../src/franka_robot_state.cpp | 56 ++++++------------- .../src/translation_utils.cpp | 34 +++++++++++ .../src/translation_utils.hpp | 15 +++++ 4 files changed, 73 insertions(+), 41 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 060daeef..bba65e53 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,11 +1,16 @@ # Changelog -## 0.1.9 - 2023-12-04 +## 0.1.10 - 2023-12-04 Requires libfranka >= 0.13.0, required ROS 2 Humble * Adapted the franka robot state broadcaster to use ROS 2 message types -* Adapted the Cartesian velocity command interface to use ROS 2 message types +* Adapted the Cartesian velocity command interface to use Eigen types + +## 0.1.9 - 2023-12-04 + +Requires libfranka >= 0.13.0, required ROS 2 Humble + * franka\_hardware: add state interfaces for initial position, cartesian pose and elbow. * franka\_hardware: support cartesian pose interface. * franka\_semantic\_component: support cartesian pose interface. diff --git a/franka_semantic_components/src/franka_robot_state.cpp b/franka_semantic_components/src/franka_robot_state.cpp index 9edf140a..824539a0 100644 --- a/franka_semantic_components/src/franka_robot_state.cpp +++ b/franka_semantic_components/src/franka_robot_state.cpp @@ -117,42 +117,33 @@ auto FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& return false; } + // Update the time stamps of the data + translation::updateTimeStamps(message.header.stamp, message); + // Collision and contact indicators message.collision_indicators = translation::toCollisionIndicators( robot_state_ptr->cartesian_collision, robot_state_ptr->cartesian_contact, robot_state_ptr->joint_collision, robot_state_ptr->joint_contact); // The joint states - message.measured_joint_state.header.stamp = message.header.stamp; - message.measured_joint_state.position = - std::vector(robot_state_ptr->q.cbegin(), robot_state_ptr->q.cend()); - message.measured_joint_state.velocity = - std::vector(robot_state_ptr->dq.cbegin(), robot_state_ptr->dq.cend()); - message.measured_joint_state.effort = - std::vector(robot_state_ptr->tau_J.cbegin(), robot_state_ptr->tau_J.cend()); - - message.desired_joint_state.header.stamp = message.header.stamp; - message.desired_joint_state.position = - std::vector(robot_state_ptr->q_d.cbegin(), robot_state_ptr->q_d.cend()); - message.desired_joint_state.velocity = - std::vector(robot_state_ptr->dq_d.cbegin(), robot_state_ptr->dq_d.cend()); - message.desired_joint_state.effort = - std::vector(robot_state_ptr->tau_J_d.cbegin(), robot_state_ptr->tau_J_d.cend()); - - message.measured_joint_motor_state.header.stamp = message.header.stamp; + message.measured_joint_state.position = translation::toJointStateVector(robot_state_ptr->q); + message.measured_joint_state.velocity = translation::toJointStateVector(robot_state_ptr->dq); + message.measured_joint_state.effort = translation::toJointStateVector(robot_state_ptr->tau_J); + + message.desired_joint_state.position = translation::toJointStateVector(robot_state_ptr->q_d); + message.desired_joint_state.velocity = translation::toJointStateVector(robot_state_ptr->dq_d); + message.desired_joint_state.effort = translation::toJointStateVector(robot_state_ptr->tau_J_d); + message.measured_joint_motor_state.position = - std::vector(robot_state_ptr->theta.cbegin(), robot_state_ptr->theta.cend()); + translation::toJointStateVector(robot_state_ptr->theta); message.measured_joint_motor_state.velocity = - std::vector(robot_state_ptr->dtheta.cbegin(), robot_state_ptr->dtheta.cend()); + translation::toJointStateVector(robot_state_ptr->dtheta); - message.tau_ext_hat_filtered.header.stamp = message.header.stamp; - message.tau_ext_hat_filtered.effort = std::vector( - robot_state_ptr->tau_ext_hat_filtered.cbegin(), robot_state_ptr->tau_ext_hat_filtered.cend()); + message.tau_ext_hat_filtered.effort = + translation::toJointStateVector(robot_state_ptr->tau_ext_hat_filtered); - for (size_t i = 0; i < robot_state_ptr->q.size(); i++) { - message.ddq_d[i] = robot_state_ptr->ddq_d[i]; - message.dtau_j[i] = robot_state_ptr->dtau_J[i]; - } + message.ddq_d = robot_state_ptr->ddq_d; + message.dtau_j = robot_state_ptr->dtau_J; // Output for the elbow message.elbow = translation::toElbow(robot_state_ptr->elbow, robot_state_ptr->elbow_d, @@ -160,39 +151,26 @@ auto FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& robot_state_ptr->ddelbow_c); // Active wrenches on the stiffness frame - message.k_f_ext_hat_k.header.stamp = message.header.stamp; message.k_f_ext_hat_k.wrench = translation::toWrench(robot_state_ptr->K_F_ext_hat_K); - message.o_f_ext_hat_k.header.stamp = message.header.stamp; message.o_f_ext_hat_k.wrench = translation::toWrench(robot_state_ptr->O_F_ext_hat_K); // The transformations between different frames - message.o_t_ee.header.stamp = message.header.stamp; message.o_t_ee.pose = translation::toPose(robot_state_ptr->O_T_EE); - message.o_t_ee_d.header.stamp = message.header.stamp; message.o_t_ee_d.pose = translation::toPose(robot_state_ptr->O_T_EE_d); - message.o_t_ee_c.header.stamp = message.header.stamp; message.o_t_ee_c.pose = translation::toPose(robot_state_ptr->O_T_EE_c); - message.f_t_ee.header.stamp = message.header.stamp; message.f_t_ee.pose = translation::toPose(robot_state_ptr->F_T_EE); - message.ee_t_k.header.stamp = message.header.stamp; message.ee_t_k.pose = translation::toPose(robot_state_ptr->EE_T_K); - message.o_dp_ee_d.header.stamp = message.header.stamp; message.o_dp_ee_d.twist = translation::toTwist(robot_state_ptr->O_dP_EE_d); - message.o_dp_ee_c.header.stamp = message.header.stamp; message.o_dp_ee_c.twist = translation::toTwist(robot_state_ptr->O_dP_EE_c); - message.o_ddp_ee_c.header.stamp = message.header.stamp; message.o_ddp_ee_c.accel = translation::toAccel(robot_state_ptr->O_ddP_EE_c); // The inertias of the robot - message.inertia_ee.header.stamp = message.header.stamp; message.inertia_ee.inertia = translation::toInertia( robot_state_ptr->m_ee, robot_state_ptr->F_x_Cee, robot_state_ptr->I_ee); - message.inertia_load.header.stamp = message.header.stamp; message.inertia_load.inertia = translation::toInertia( robot_state_ptr->m_load, robot_state_ptr->F_x_Cload, robot_state_ptr->I_load); - message.inertia_total.header.stamp = message.header.stamp; message.inertia_total.inertia = translation::toInertia( robot_state_ptr->m_total, robot_state_ptr->F_x_Ctotal, robot_state_ptr->I_total); diff --git a/franka_semantic_components/src/translation_utils.cpp b/franka_semantic_components/src/translation_utils.cpp index 88d829e6..f96e1738 100644 --- a/franka_semantic_components/src/translation_utils.cpp +++ b/franka_semantic_components/src/translation_utils.cpp @@ -302,5 +302,39 @@ auto toElbow(const std::array& elbow, return elbow_message; } +auto toJointStateVector(const std::array& data_vector) -> std::vector { + return {data_vector.cbegin(), data_vector.cend()}; +} + +auto updateTimeStamps(const builtin_interfaces::msg::Time& time_stamps, + franka_msgs::msg::FrankaRobotState& robot_state) -> void { + // The joint states + robot_state.measured_joint_state.header.stamp = time_stamps; + robot_state.desired_joint_state.header.stamp = time_stamps; + robot_state.measured_joint_motor_state.header.stamp = time_stamps; + robot_state.tau_ext_hat_filtered.header.stamp = time_stamps; + + // Active wrenches on the stiffness frame + robot_state.k_f_ext_hat_k.header.stamp = time_stamps; + robot_state.o_f_ext_hat_k.header.stamp = time_stamps; + + // The transformations between different frames + robot_state.o_t_ee.header.stamp = time_stamps; + robot_state.o_t_ee_d.header.stamp = time_stamps; + robot_state.o_t_ee_c.header.stamp = time_stamps; + + robot_state.f_t_ee.header.stamp = time_stamps; + robot_state.ee_t_k.header.stamp = time_stamps; + + robot_state.o_dp_ee_d.header.stamp = time_stamps; + robot_state.o_dp_ee_c.header.stamp = time_stamps; + robot_state.o_ddp_ee_c.header.stamp = time_stamps; + + // The inertias of the robot + robot_state.inertia_ee.header.stamp = time_stamps; + robot_state.inertia_load.header.stamp = time_stamps; + robot_state.inertia_total.header.stamp = time_stamps; +} + } // namespace translation } // namespace franka_semantic_components diff --git a/franka_semantic_components/src/translation_utils.hpp b/franka_semantic_components/src/translation_utils.hpp index e18197f2..88ae3f03 100644 --- a/franka_semantic_components/src/translation_utils.hpp +++ b/franka_semantic_components/src/translation_utils.hpp @@ -19,6 +19,9 @@ #include "franka_msgs/msg/collision_indicators.hpp" #include "franka_msgs/msg/elbow.hpp" #include "franka_msgs/msg/errors.hpp" +#include "franka_msgs/msg/franka_robot_state.hpp" + +#include "builtin_interfaces/msg/time.hpp" #include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/inertia.hpp" @@ -101,5 +104,17 @@ auto toElbow(const std::array& elbow, const std::array& delbow_c, const std::array& ddelbow_c) -> franka_msgs::msg::Elbow; +/** + * @param data_vector Translates this data vector from an array to a vector + * @return std::vector The translated vector + */ +auto toJointStateVector(const std::array& data_vector) -> std::vector; + +/** + * @param robot_state Updates the the + */ +auto updateTimeStamps(const builtin_interfaces::msg::Time& time_stamps, + franka_msgs::msg::FrankaRobotState& robot_state) -> void; + } // namespace translation } // namespace franka_semantic_components