diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index fe9fc48a..6c339d31 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -32,6 +32,8 @@ RUN apt-get update && \ ros-humble-controller-manager \ ros-humble-joint-state-publisher-gui \ ros-humble-control-msgs \ + ros-humble-geometry-msgs \ + ros-humble-sensor-msgs \ ros-humble-gazebo-ros \ ros-humble-control-toolbox \ ros-humble-ackermann-msgs \ diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 7394af76..40343d04 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -12,8 +12,13 @@ "ms-python.python", "bungcip.better-toml", "ms-vscode.cpptools", - "ms-vscode.cpptools-extension-pack" + "ms-vscode.cpptools-extension-pack", + "ms-vscode.cmake-tools", + "xaver.clang-format", + "josetr.cmake-language-support-vscode", + "twxs.cmake", + "cheshirekow.cmake-format" ] } } -} \ No newline at end of file +} diff --git a/CHANGELOG.md b/CHANGELOG.md index ddd36f2b..bba65e53 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,12 @@ # Changelog +## 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 Eigen types + ## 0.1.9 - 2023-12-04 Requires libfranka >= 0.13.0, required ROS 2 Humble @@ -95,4 +102,4 @@ Requires libfranka >= 0.10.0, required ROS 2 Humble ### Fixed -* franka\_hardware Fix the mismatched joint state interface type logger error message. \ No newline at end of file +* franka\_hardware Fix the mismatched joint state interface type logger error message. diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp index 862b7b9c..4fad3e5c 100644 --- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp +++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp @@ -54,9 +54,11 @@ controller_interface::return_type CartesianVelocityExampleController::update( double v_x = std::cos(k_angle_) * v; double v_z = -std::sin(k_angle_) * v; - std::array cartesian_velocity_command = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}}; + Eigen::Vector3d cartesian_linear_velocity(v_x, 0.0, v_z); + Eigen::Vector3d cartesian_angular_velocity(0.0, 0.0, 0.0); - if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command)) { + if (franka_cartesian_velocity_->setCommand(cartesian_linear_velocity, + cartesian_angular_velocity)) { return controller_interface::return_type::OK; } else { RCLCPP_FATAL(get_node()->get_logger(), @@ -110,4 +112,4 @@ controller_interface::CallbackReturn CartesianVelocityExampleController::on_deac #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianVelocityExampleController, - controller_interface::ControllerInterface) \ No newline at end of file + controller_interface::ControllerInterface) diff --git a/franka_example_controllers/src/elbow_example_controller.cpp b/franka_example_controllers/src/elbow_example_controller.cpp index ea21d515..12a9ca92 100644 --- a/franka_example_controllers/src/elbow_example_controller.cpp +++ b/franka_example_controllers/src/elbow_example_controller.cpp @@ -53,11 +53,14 @@ controller_interface::return_type ElbowExampleController::update( double angle = M_PI / 15.0 * (1.0 - std::cos(M_PI / 5.0 * elapsed_time_)); - std::array cartesian_velocity_command = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + Eigen::Vector3d cartesian_linear_velocity(0.0, 0.0, 0.0); + Eigen::Vector3d cartesian_angular_velocity(0.0, 0.0, 0.0); + std::array elbow_command = { {initial_elbow_configuration_[0] + angle, initial_elbow_configuration_[1]}}; - if (franka_cartesian_velocity_->setCommand(cartesian_velocity_command, elbow_command)) { + if (franka_cartesian_velocity_->setCommand(cartesian_linear_velocity, cartesian_angular_velocity, + elbow_command)) { return controller_interface::return_type::OK; } else { RCLCPP_FATAL(get_node()->get_logger(), @@ -114,4 +117,4 @@ controller_interface::CallbackReturn ElbowExampleController::on_deactivate( #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE PLUGINLIB_EXPORT_CLASS(franka_example_controllers::ElbowExampleController, - controller_interface::ControllerInterface) \ No newline at end of file + controller_interface::ControllerInterface) diff --git a/franka_msgs/CMakeLists.txt b/franka_msgs/CMakeLists.txt index f972ba5d..00d702ae 100644 --- a/franka_msgs/CMakeLists.txt +++ b/franka_msgs/CMakeLists.txt @@ -4,11 +4,15 @@ project(franka_msgs) find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "action/Grasp.action" "action/Homing.action" "action/Move.action" + "msg/CollisionIndicators.msg" + "msg/Elbow.msg" "msg/Errors.msg" "msg/GraspEpsilon.msg" "msg/FrankaRobotState.msg" @@ -19,7 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetJointStiffness.srv" "srv/SetStiffnessFrame.srv" "srv/SetLoad.srv" - DEPENDENCIES std_msgs builtin_interfaces + DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces ) if(BUILD_TESTING) @@ -27,4 +31,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() \ No newline at end of file +ament_package() diff --git a/franka_msgs/msg/CollisionIndicators.msg b/franka_msgs/msg/CollisionIndicators.msg new file mode 100644 index 00000000..03e92733 --- /dev/null +++ b/franka_msgs/msg/CollisionIndicators.msg @@ -0,0 +1,8 @@ +### Indicates which dimensions have an active contact/collision flag raised +geometry_msgs/Vector3 is_cartesian_linear_collision +geometry_msgs/Vector3 is_cartesian_angular_collision +geometry_msgs/Vector3 is_cartesian_linear_contact +geometry_msgs/Vector3 is_cartesian_angular_contact + +float64[7] is_joint_collision +float64[7] is_joint_contact diff --git a/franka_msgs/msg/Elbow.msg b/franka_msgs/msg/Elbow.msg new file mode 100644 index 00000000..d69eee91 --- /dev/null +++ b/franka_msgs/msg/Elbow.msg @@ -0,0 +1,6 @@ +### The state of the elbow +float64[2] position +float64[2] desired_position +float64[2] commanded_position +float64[2] commanded_velocity +float64[2] commanded_acceleration diff --git a/franka_msgs/msg/FrankaRobotState.msg b/franka_msgs/msg/FrankaRobotState.msg index d9ed3529..14dc9606 100644 --- a/franka_msgs/msg/FrankaRobotState.msg +++ b/franka_msgs/msg/FrankaRobotState.msg @@ -1,45 +1,66 @@ +### Default parts of the message std_msgs/Header header -float64[6] cartesian_collision -float64[6] cartesian_contact -float64[7] q -float64[7] q_d -float64[7] dq -float64[7] dq_d + +### Indicates which dimensions have an active contact/collision flag raised +CollisionIndicators collision_indicators + +### The state of the arm in joint space +# The joint state consisting out of position (q), velocity (dq) and effort (tau_J) +sensor_msgs/JointState measured_joint_state + +# The desired joint state consisting out of position (q_d), velocity (dq_d) and effort (tau_J_d) +sensor_msgs/JointState desired_joint_state + +# The measured motor state of the joints consisting out of position (theta) and velocity (dtheta) +sensor_msgs/JointState measured_joint_motor_state + +# The desired joint acceleration float64[7] ddq_d -float64[7] theta -float64[7] dtheta -float64[7] tau_j +# The derivative of the measured torque signal float64[7] dtau_j -float64[7] tau_j_d -float64[6] k_f_ext_hat_k -float64[2] elbow -float64[2] elbow_d -float64[2] elbow_c -float64[2] delbow_c -float64[2] ddelbow_c -float64[7] joint_collision -float64[7] joint_contact -float64[6] o_f_ext_hat_k -float64[6] o_dp_ee_d -float64[6] o_dp_ee_c -float64[6] o_ddp_ee_c -float64[7] tau_ext_hat_filtered -float64 m_ee -float64[3] f_x_cee -float64[9] i_ee -float64 m_load -float64[3] f_x_cload -float64[9] i_load -float64 m_total -float64[3] f_x_ctotal -float64[9] i_total -float64[16] o_t_ee -float64[16] o_t_ee_d -float64[16] o_t_ee_c -float64[16] f_t_ee -float64[16] ee_t_k +# Filtered external torque. The JointState consists out of effort (tau_ext_hat_filtered) +sensor_msgs/JointState tau_ext_hat_filtered + +### The state of the elbow +Elbow elbow + +### The active wrenches acting on the stiffness frame expressed relative to +# stiffness frame +geometry_msgs/WrenchStamped k_f_ext_hat_k +# base frame +geometry_msgs/WrenchStamped o_f_ext_hat_k + +### The different inertias of the arm +# The end-effector inertia +geometry_msgs/InertiaStamped inertia_ee +# The load inertia +geometry_msgs/InertiaStamped inertia_load +# The total (end-effector + load) inertia +geometry_msgs/InertiaStamped inertia_total + +### The poses describing the transformations between different frames of the arm +# Measured end-effector pose in base frame +geometry_msgs/PoseStamped o_t_ee +# Last desired end-effector pose of motion generation in base frame +geometry_msgs/PoseStamped o_t_ee_d +# Last commanded end-effector pose of motion generation in base frame +geometry_msgs/PoseStamped o_t_ee_c +# Flange to end-effector frame +geometry_msgs/PoseStamped f_t_ee +# End-effector to stiffness frame +geometry_msgs/PoseStamped ee_t_k + +# Desired end effector twist in base frame +geometry_msgs/TwistStamped o_dp_ee_d +# Last commanded end effector twist in base frame +geometry_msgs/TwistStamped o_dp_ee_c +# Last commanded end effector acceleration in base frame +geometry_msgs/AccelStamped o_ddp_ee_c + +### Additional information float64 time float64 control_command_success_rate + uint8 ROBOT_MODE_OTHER=0 uint8 ROBOT_MODE_IDLE=1 uint8 ROBOT_MODE_MOVE=2 @@ -48,5 +69,6 @@ uint8 ROBOT_MODE_REFLEX=4 uint8 ROBOT_MODE_USER_STOPPED=5 uint8 ROBOT_MODE_AUTOMATIC_ERROR_RECOVERY=6 uint8 robot_mode + Errors current_errors -Errors last_motion_errors \ No newline at end of file +Errors last_motion_errors diff --git a/franka_msgs/package.xml b/franka_msgs/package.xml index 34ad7966..0106479b 100644 --- a/franka_msgs/package.xml +++ b/franka_msgs/package.xml @@ -17,10 +17,12 @@ builtin_interfaces std_msgs action_msgs + sensor_msgs + geometry_msgs rosidl_default_runtime rosidl_interface_packages ament_cmake - \ No newline at end of file + diff --git a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp index c8ded784..cc1ed825 100644 --- a/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp +++ b/franka_robot_state_broadcaster/src/franka_robot_state_broadcaster.cpp @@ -73,7 +73,7 @@ controller_interface::CallbackReturn FrankaRobotStateBroadcaster::on_configure( realtime_franka_state_publisher = std::make_shared>( franka_state_publisher); - ; + franka_robot_state->initialize_robot_state_msg(realtime_franka_state_publisher->msg_); } catch (const std::exception& e) { fprintf(stderr, "Exception thrown during publisher creation at configure stage with message : %s \n", diff --git a/franka_semantic_components/CMakeLists.txt b/franka_semantic_components/CMakeLists.txt index 65335f4f..3e51d719 100644 --- a/franka_semantic_components/CMakeLists.txt +++ b/franka_semantic_components/CMakeLists.txt @@ -12,7 +12,7 @@ endif() option(CHECK_TIDY "Adds clang-tidy tests" OFF) -set(THIS_PACKAGE_INCLUDE_DEPENDS Franka franka_hardware franka_msgs hardware_interface rclcpp_lifecycle) +set(THIS_PACKAGE_INCLUDE_DEPENDS Franka franka_hardware franka_msgs geometry_msgs sensor_msgs hardware_interface rclcpp_lifecycle) # find dependencies find_package(ament_cmake REQUIRED) @@ -28,11 +28,17 @@ add_library(franka_semantic_components SHARED src/franka_robot_model.cpp src/franka_cartesian_velocity_interface.cpp src/franka_cartesian_pose_interface.cpp - src/franka_semantic_component_interface.cpp) + src/franka_semantic_component_interface.cpp + src/translation_utils.cpp +) target_include_directories(franka_semantic_components PRIVATE include ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(franka_semantic_components Franka::Franka) +target_link_libraries(franka_semantic_components + Franka::Franka + Eigen3::Eigen) ament_target_dependencies(franka_semantic_components Franka + sensor_msgs + geometry_msgs ${THIS_PACKAGE_INCLUDE_DEPENDS}) install(TARGETS franka_semantic_components DESTINATION lib) @@ -73,6 +79,8 @@ if(BUILD_TESTING) controller_interface hardware_interface franka_msgs + sensor_msgs + geometry_msgs Franka ) ament_add_gmock(test_franka_robot_model test/franka_robot_model_test.cpp) @@ -85,6 +93,8 @@ if(BUILD_TESTING) controller_interface hardware_interface franka_msgs + sensor_msgs + geometry_msgs Franka ) @@ -101,6 +111,8 @@ if(BUILD_TESTING) ament_target_dependencies( franka_semantic_component_interface_test hardware_interface + sensor_msgs + geometry_msgs ) ament_add_gmock( @@ -133,6 +145,8 @@ if(BUILD_TESTING) ament_target_dependencies( franka_cartesian_pose_interface_test hardware_interface + sensor_msgs + geometry_msgs ) endif() diff --git a/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp index b318857f..20c56cab 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_cartesian_velocity_interface.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,15 +14,18 @@ #pragma once -#include -#include -#include - #include "franka/control_types.h" #include "franka/robot_state.h" #include "franka_semantic_components/franka_semantic_component_interface.hpp" +#include "geometry_msgs/msg/twist.hpp" + #include +#include +#include +#include + +#include namespace franka_semantic_components { /** @@ -50,23 +53,24 @@ class FrankaCartesianVelocityInterface : public FrankaSemanticComponentInterface virtual ~FrankaCartesianVelocityInterface() = default; /** - * Sets the given command. - * - * @param[in] command Command to set. + * @param twist_command The velocity command in Cartesian coordinates + * @return true if successul * * @return if successful true, else when elbow is activated false. */ - bool setCommand(const std::array& command); + bool setCommand(const Eigen::Vector3d& linear_velocity_command, + const Eigen::Vector3d& angular_velocity_command); /** - * Sets the given command. - * - * @param[in] cartesian_velocity_command Command to set. - * @param[in] elbow Elbow to set. + * @param twist_command The velocity command in Cartesian coordinates + * @param elbow_command The elbow command: {joint_3, sign(joint_4)} + * @return true if successul * * @return if successful true, else when elbow is not activated false. */ - bool setCommand(const std::array& command, const std::array& elbow); + bool setCommand(const Eigen::Vector3d& linear_velocity_command, + const Eigen::Vector3d& angular_velocity_command, + const std::array& elbow_command); /** * Get the commanded elbow interface elbow values. diff --git a/franka_semantic_components/include/franka_semantic_components/franka_robot_model.hpp b/franka_semantic_components/include/franka_semantic_components/franka_robot_model.hpp index 73c07309..9227f97e 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_robot_model.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_robot_model.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp index 53c77c52..2010cb28 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_robot_state.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,7 +19,6 @@ #include #include "franka/robot_state.h" -#include "franka_msgs/msg/errors.hpp" #include "franka_msgs/msg/franka_robot_state.hpp" #include "semantic_components/semantic_component_interface.hpp" @@ -31,11 +30,16 @@ class FrankaRobotState virtual ~FrankaRobotState() = default; + /** + * @param[in/out] message Initializes this message to contain the respective frame_id information + */ + auto initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) -> void; + /** * Constructs and return a FrankaRobotState message from the current values. * \return FrankaRobotState message from values; */ - bool get_values_as_message(franka_msgs::msg::FrankaRobotState& message); + auto get_values_as_message(franka_msgs::msg::FrankaRobotState& message) -> bool; protected: franka::RobotState* robot_state_ptr; diff --git a/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp b/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp index 2703ce41..a9f8defc 100644 --- a/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp +++ b/franka_semantic_components/include/franka_semantic_components/franka_semantic_component_interface.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/package.xml b/franka_semantic_components/package.xml index ccd94676..aabbe252 100644 --- a/franka_semantic_components/package.xml +++ b/franka_semantic_components/package.xml @@ -3,14 +3,16 @@ franka_semantic_components 0.1.0 - franka_semantic_components provides semantic components for using Franka Emika research robots with ros2_control - Franka Emika GmbH + franka_semantic_components provides semantic components for using Franka Robotics research robots with ros2_control + Franka Robotics GmbH Apache 2.0 ament_cmake rclcpp franka_msgs + sensor_msgs + geometry_msgs hardware_interface franka_hardware @@ -22,6 +24,7 @@ ament_cmake_pep257 ament_cmake_xmllint ament_cmake_gmock + ament_cmake_clang_tidy ament_cmake diff --git a/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp b/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp index 43cf167e..04e82177 100644 --- a/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp +++ b/franka_semantic_components/src/franka_cartesian_velocity_interface.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,13 +20,17 @@ #include "rclcpp/logging.hpp" namespace { -std::vector combineArraysToVector(const std::array& cartesian_velocity_command, +std::vector combineArraysToVector(const Eigen::Vector3d& linear_velocity_command, + const Eigen::Vector3d& angular_velocity_command, const std::array& elbow_command) { - std::vector full_command; - full_command.reserve(cartesian_velocity_command.size() + elbow_command.size()); - full_command.insert(full_command.end(), cartesian_velocity_command.begin(), - cartesian_velocity_command.end()); - full_command.insert(full_command.end(), elbow_command.begin(), elbow_command.end()); + std::vector full_command{linear_velocity_command.x(), + linear_velocity_command.y(), + linear_velocity_command.z(), + angular_velocity_command.x(), + angular_velocity_command.y(), + angular_velocity_command.z(), + elbow_command[0], + elbow_command[1]}; return full_command; } @@ -60,32 +64,31 @@ FrankaCartesianVelocityInterface::FrankaCartesianVelocityInterface(bool command_ } } -bool FrankaCartesianVelocityInterface::setCommand( - const std::array& cartesian_velocity_command, - const std::array& elbow_command) { +bool FrankaCartesianVelocityInterface::setCommand(const Eigen::Vector3d& linear_velocity_command, + const Eigen::Vector3d& angular_velocity_command, + const std::array& elbow_command) { if (!command_elbow_active_) { RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_velocity_interface"), "Elbow command interface must be claimed to command elbow."); return false; } - auto full_command = combineArraysToVector(cartesian_velocity_command, elbow_command); + auto full_command = + combineArraysToVector(linear_velocity_command, angular_velocity_command, elbow_command); return set_values(full_command); } -bool FrankaCartesianVelocityInterface::setCommand( - const std::array& cartesian_velocity_command) { +bool FrankaCartesianVelocityInterface::setCommand(const Eigen::Vector3d& linear_velocity_command, + const Eigen::Vector3d& angular_velocity_command) { if (command_elbow_active_) { RCLCPP_ERROR(rclcpp::get_logger("franka_cartesian_velocity_interface"), "Elbow command interface must not claimed, if elbow is not commanded."); return false; } - std::vector full_command; - - full_command.reserve(cartesian_velocity_command.size()); - full_command.insert(full_command.end(), cartesian_velocity_command.begin(), - cartesian_velocity_command.end()); + std::vector full_command{linear_velocity_command.x(), linear_velocity_command.y(), + linear_velocity_command.z(), angular_velocity_command.x(), + angular_velocity_command.y(), angular_velocity_command.z()}; return set_values(full_command); } diff --git a/franka_semantic_components/src/franka_robot_model.cpp b/franka_semantic_components/src/franka_robot_model.cpp index fe5a11eb..3632e00e 100644 --- a/franka_semantic_components/src/franka_robot_model.cpp +++ b/franka_semantic_components/src/franka_robot_model.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -68,4 +68,4 @@ void FrankaRobotModel::initialize() { } initialized = true; } -} // namespace franka_semantic_components \ No newline at end of file +} // namespace franka_semantic_components diff --git a/franka_semantic_components/src/franka_robot_state.cpp b/franka_semantic_components/src/franka_robot_state.cpp index 88cc746e..824539a0 100644 --- a/franka_semantic_components/src/franka_robot_state.cpp +++ b/franka_semantic_components/src/franka_robot_state.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,8 +17,22 @@ #include #include "rclcpp/logging.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "std_msgs/msg/header.hpp" +#include "std_msgs/msg/string.hpp" +#include "translation_utils.hpp" + namespace { +// TODO(kuhn_an): Are these correct? +const std::string kLinkName = "_link"; +const size_t kTotalAmountOfLinks = 11; +const size_t kBaseLinkIndex = 0; +const size_t kFlangeLinkIndex = 7; +const size_t kEndEffectorLinkIndex = 8; +const size_t kLoadLinkIndex = 9; +const size_t kStiffnessLinkIndex = 10; + // Example implementation of bit_cast: https://en.cppreference.com/w/cpp/numeric/bit_cast template std::enable_if_t && @@ -34,112 +48,6 @@ bit_cast(const From& src) noexcept { return dst; } -franka_msgs::msg::Errors errorsToMessage(const franka::Errors& error) { - franka_msgs::msg::Errors message; - message.joint_position_limits_violation = - static_cast( - error.joint_position_limits_violation); - message.cartesian_position_limits_violation = - static_cast( - error.cartesian_position_limits_violation); - message.self_collision_avoidance_violation = - static_cast( - error.self_collision_avoidance_violation); - message.joint_velocity_violation = - static_cast(error.joint_velocity_violation); - message.cartesian_velocity_violation = - static_cast( - error.cartesian_velocity_violation); - message.force_control_safety_violation = - static_cast( - error.force_control_safety_violation); - message.joint_reflex = static_cast(error.joint_reflex); - message.cartesian_reflex = - static_cast(error.cartesian_reflex); - message.max_goal_pose_deviation_violation = - static_cast( - error.max_goal_pose_deviation_violation); - message.max_path_pose_deviation_violation = - static_cast( - error.max_path_pose_deviation_violation); - message.cartesian_velocity_profile_safety_violation = - static_cast( - error.cartesian_velocity_profile_safety_violation); - message.joint_position_motion_generator_start_pose_invalid = - static_cast( - error.joint_position_motion_generator_start_pose_invalid); - message.joint_motion_generator_position_limits_violation = - static_cast( - error.joint_motion_generator_position_limits_violation); - message.joint_motion_generator_velocity_limits_violation = - static_cast( - error.joint_motion_generator_velocity_limits_violation); - message.joint_motion_generator_velocity_discontinuity = - static_cast( - error.joint_motion_generator_velocity_discontinuity); - message.joint_motion_generator_acceleration_discontinuity = - static_cast( - error.joint_motion_generator_acceleration_discontinuity); - message.cartesian_position_motion_generator_start_pose_invalid = - static_cast( - error.cartesian_position_motion_generator_start_pose_invalid); - message.cartesian_motion_generator_elbow_limit_violation = - static_cast( - error.cartesian_motion_generator_elbow_limit_violation); - message.cartesian_motion_generator_velocity_limits_violation = - static_cast( - error.cartesian_motion_generator_velocity_limits_violation); - message.cartesian_motion_generator_velocity_discontinuity = - static_cast( - error.cartesian_motion_generator_velocity_discontinuity); - message.cartesian_motion_generator_acceleration_discontinuity = - static_cast( - error.cartesian_motion_generator_acceleration_discontinuity); - message.cartesian_motion_generator_elbow_sign_inconsistent = - static_cast( - error.cartesian_motion_generator_elbow_sign_inconsistent); - message.cartesian_motion_generator_start_elbow_invalid = - static_cast( - error.cartesian_motion_generator_start_elbow_invalid); - message.cartesian_motion_generator_joint_position_limits_violation = - static_cast( - error.cartesian_motion_generator_joint_position_limits_violation); - message.cartesian_motion_generator_joint_velocity_limits_violation = - static_cast( - error.cartesian_motion_generator_joint_velocity_limits_violation); - message.cartesian_motion_generator_joint_velocity_discontinuity = - static_cast( - error.cartesian_motion_generator_joint_velocity_discontinuity); - message.cartesian_motion_generator_joint_acceleration_discontinuity = - static_cast( - error.cartesian_motion_generator_joint_acceleration_discontinuity); - message.cartesian_position_motion_generator_invalid_frame = - static_cast( - error.cartesian_position_motion_generator_invalid_frame); - message.force_controller_desired_force_tolerance_violation = - static_cast( - error.force_controller_desired_force_tolerance_violation); - message.controller_torque_discontinuity = - static_cast( - error.controller_torque_discontinuity); - message.start_elbow_sign_inconsistent = - static_cast( - error.start_elbow_sign_inconsistent); - message.communication_constraints_violation = - static_cast( - error.communication_constraints_violation); - message.power_limit_violation = - static_cast(error.power_limit_violation); - message.joint_p2p_insufficient_torque_for_planning = - static_cast( - error.joint_p2p_insufficient_torque_for_planning); - message.tau_j_range_violation = - static_cast(error.tau_j_range_violation); - message.instability_detected = - static_cast(error.instability_detected); - return message; -} - } // anonymous namespace namespace franka_semantic_components { @@ -149,7 +57,49 @@ FrankaRobotState::FrankaRobotState(const std::string& name) : SemanticComponentI // TODO: Set default values to NaN } -bool FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& message) { +auto FrankaRobotState::initialize_robot_state_msg(franka_msgs::msg::FrankaRobotState& message) + -> void { + // The joint states + std::vector joint_names; + // Starting from the base link (aka joint0) up to the end-effector (aka joint8), load (aka joint9) + // and stiffness frame (aka joint10) + for (size_t i = 0; i < kTotalAmountOfLinks; ++i) { + joint_names.push_back(robot_name_ + kLinkName + std::to_string(i)); + } + + // The joint state - joint 0 is the base while joint 8 is the last joint + message.measured_joint_state.name = + std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + message.desired_joint_state.name = + std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + message.measured_joint_motor_state.name = + std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + message.tau_ext_hat_filtered.name = + std::vector(joint_names.cbegin() + 1, joint_names.cbegin() + 8); + + // Active wrenches + message.o_f_ext_hat_k.header.frame_id = joint_names[kBaseLinkIndex]; + message.k_f_ext_hat_k.header.frame_id = joint_names[kStiffnessLinkIndex]; + + // Different pose frames + message.o_t_ee.header.frame_id = joint_names[kBaseLinkIndex]; + message.o_t_ee_d.header.frame_id = joint_names[kBaseLinkIndex]; + message.o_t_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; + + message.f_t_ee.header.frame_id = joint_names[kFlangeLinkIndex]; + message.ee_t_k.header.frame_id = joint_names[kEndEffectorLinkIndex]; + + message.o_dp_ee_d.header.frame_id = joint_names[kBaseLinkIndex]; + message.o_dp_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; + message.o_ddp_ee_c.header.frame_id = joint_names[kBaseLinkIndex]; + + // The inertias + message.inertia_ee.header.frame_id = joint_names[kEndEffectorLinkIndex]; + message.inertia_load.header.frame_id = joint_names[kLoadLinkIndex]; + message.inertia_total.header.frame_id = joint_names[kEndEffectorLinkIndex]; +} + +auto FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& message) -> bool { const std::string full_interface_name = robot_name_ + "/" + state_interface_name_; auto franka_state_interface = @@ -167,123 +117,68 @@ bool FrankaRobotState::get_values_as_message(franka_msgs::msg::FrankaRobotState& return false; } - static_assert( - sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->cartesian_contact), - "Robot state Cartesian members do not have same size"); - static_assert( - sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->K_F_ext_hat_K), - "Robot state Cartesian members do not have same size"); - static_assert( - sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->O_F_ext_hat_K), - "Robot state Cartesian members do not have same size"); - static_assert(sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->O_dP_EE_d), - "Robot state Cartesian members do not have same size"); - static_assert(sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->O_dP_EE_c), - "Robot state Cartesian members do not have same size"); - static_assert(sizeof(robot_state_ptr->cartesian_collision) == sizeof(robot_state_ptr->O_ddP_EE_c), - "Robot state Cartesian members do not have same size"); - for (size_t i = 0; i < robot_state_ptr->cartesian_collision.size(); i++) { - message.cartesian_collision[i] = robot_state_ptr->cartesian_collision[i]; - message.cartesian_contact[i] = robot_state_ptr->cartesian_contact[i]; - message.k_f_ext_hat_k[i] = robot_state_ptr->K_F_ext_hat_K[i]; - message.o_f_ext_hat_k[i] = robot_state_ptr->O_F_ext_hat_K[i]; - message.o_dp_ee_d[i] = robot_state_ptr->O_dP_EE_d[i]; - message.o_dp_ee_c[i] = robot_state_ptr->O_dP_EE_c[i]; - message.o_ddp_ee_c[i] = robot_state_ptr->O_ddP_EE_c[i]; - } + // Update the time stamps of the data + translation::updateTimeStamps(message.header.stamp, message); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->q_d), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->dq), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->dq_d), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->ddq_d), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->tau_J), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->dtau_J), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->tau_J_d), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->theta), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->dtheta), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->joint_collision), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->joint_contact), - "Robot state joint members do not have same size"); - static_assert(sizeof(robot_state_ptr->q) == sizeof(robot_state_ptr->tau_ext_hat_filtered), - "Robot state joint members do not have same size"); - for (size_t i = 0; i < robot_state_ptr->q.size(); i++) { - message.q[i] = robot_state_ptr->q[i]; - message.q_d[i] = robot_state_ptr->q_d[i]; - message.dq[i] = robot_state_ptr->dq[i]; - message.dq_d[i] = robot_state_ptr->dq_d[i]; - message.ddq_d[i] = robot_state_ptr->ddq_d[i]; - message.tau_j[i] = robot_state_ptr->tau_J[i]; - message.dtau_j[i] = robot_state_ptr->dtau_J[i]; - message.tau_j_d[i] = robot_state_ptr->tau_J_d[i]; - message.theta[i] = robot_state_ptr->theta[i]; - message.dtheta[i] = robot_state_ptr->dtheta[i]; - message.joint_collision[i] = robot_state_ptr->joint_collision[i]; - message.joint_contact[i] = robot_state_ptr->joint_contact[i]; - message.tau_ext_hat_filtered[i] = robot_state_ptr->tau_ext_hat_filtered[i]; - } + // 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); - static_assert(sizeof(robot_state_ptr->elbow) == sizeof(robot_state_ptr->elbow_d), - "Robot state elbow configuration members do not have same size"); - static_assert(sizeof(robot_state_ptr->elbow) == sizeof(robot_state_ptr->elbow_c), - "Robot state elbow configuration members do not have same size"); - static_assert(sizeof(robot_state_ptr->elbow) == sizeof(robot_state_ptr->delbow_c), - "Robot state elbow configuration members do not have same size"); - static_assert(sizeof(robot_state_ptr->elbow) == sizeof(robot_state_ptr->ddelbow_c), - "Robot state elbow configuration members do not have same size"); - - for (size_t i = 0; i < robot_state_ptr->elbow.size(); i++) { - message.elbow[i] = robot_state_ptr->elbow[i]; - message.elbow_d[i] = robot_state_ptr->elbow_d[i]; - message.elbow_c[i] = robot_state_ptr->elbow_c[i]; - message.delbow_c[i] = robot_state_ptr->delbow_c[i]; - message.ddelbow_c[i] = robot_state_ptr->ddelbow_c[i]; - } + // The joint states + 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); - static_assert(sizeof(robot_state_ptr->O_T_EE) == sizeof(robot_state_ptr->F_T_EE), - "Robot state transforms do not have same size"); - static_assert(sizeof(robot_state_ptr->O_T_EE) == sizeof(robot_state_ptr->EE_T_K), - "Robot state transforms do not have same size"); - static_assert(sizeof(robot_state_ptr->O_T_EE) == sizeof(robot_state_ptr->O_T_EE_d), - "Robot state transforms do not have same size"); - static_assert(sizeof(robot_state_ptr->O_T_EE) == sizeof(robot_state_ptr->O_T_EE_c), - "Robot state transforms do not have same size"); - for (size_t i = 0; i < robot_state_ptr->O_T_EE.size(); i++) { - message.o_t_ee[i] = robot_state_ptr->O_T_EE[i]; - message.f_t_ee[i] = robot_state_ptr->F_T_EE[i]; - message.ee_t_k[i] = robot_state_ptr->EE_T_K[i]; - message.o_t_ee_d[i] = robot_state_ptr->O_T_EE_d[i]; - message.o_t_ee_c[i] = robot_state_ptr->O_T_EE_c[i]; - } - message.m_ee = robot_state_ptr->m_ee; - message.m_load = robot_state_ptr->m_load; - message.m_total = robot_state_ptr->m_total; - - for (size_t i = 0; i < robot_state_ptr->I_load.size(); i++) { - message.i_ee[i] = robot_state_ptr->I_ee[i]; - message.i_load[i] = robot_state_ptr->I_load[i]; - message.i_total[i] = robot_state_ptr->I_total[i]; - } + 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); - for (size_t i = 0; i < robot_state_ptr->F_x_Cload.size(); i++) { - message.f_x_cee[i] = robot_state_ptr->F_x_Cee[i]; - message.f_x_cload[i] = robot_state_ptr->F_x_Cload[i]; - message.f_x_ctotal[i] = robot_state_ptr->F_x_Ctotal[i]; - } + message.measured_joint_motor_state.position = + translation::toJointStateVector(robot_state_ptr->theta); + message.measured_joint_motor_state.velocity = + translation::toJointStateVector(robot_state_ptr->dtheta); + + message.tau_ext_hat_filtered.effort = + translation::toJointStateVector(robot_state_ptr->tau_ext_hat_filtered); + + 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, + robot_state_ptr->elbow_c, robot_state_ptr->delbow_c, + robot_state_ptr->ddelbow_c); + + // Active wrenches on the stiffness frame + message.k_f_ext_hat_k.wrench = translation::toWrench(robot_state_ptr->K_F_ext_hat_K); + 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.pose = translation::toPose(robot_state_ptr->O_T_EE); + message.o_t_ee_d.pose = translation::toPose(robot_state_ptr->O_T_EE_d); + message.o_t_ee_c.pose = translation::toPose(robot_state_ptr->O_T_EE_c); + + message.f_t_ee.pose = translation::toPose(robot_state_ptr->F_T_EE); + message.ee_t_k.pose = translation::toPose(robot_state_ptr->EE_T_K); + + message.o_dp_ee_d.twist = translation::toTwist(robot_state_ptr->O_dP_EE_d); + message.o_dp_ee_c.twist = translation::toTwist(robot_state_ptr->O_dP_EE_c); + message.o_ddp_ee_c.accel = translation::toAccel(robot_state_ptr->O_ddP_EE_c); + + // The inertias of the robot + 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.inertia = translation::toInertia( + robot_state_ptr->m_load, robot_state_ptr->F_x_Cload, robot_state_ptr->I_load); + message.inertia_total.inertia = translation::toInertia( + robot_state_ptr->m_total, robot_state_ptr->F_x_Ctotal, robot_state_ptr->I_total); + // Errors and more message.time = robot_state_ptr->time.toSec(); message.control_command_success_rate = robot_state_ptr->control_command_success_rate; - message.current_errors = errorsToMessage(robot_state_ptr->current_errors); - message.last_motion_errors = errorsToMessage(robot_state_ptr->last_motion_errors); + message.current_errors = translation::errorsToMessage(robot_state_ptr->current_errors); + message.last_motion_errors = translation::errorsToMessage(robot_state_ptr->last_motion_errors); switch (robot_state_ptr->robot_mode) { case franka::RobotMode::kOther: diff --git a/franka_semantic_components/src/franka_semantic_component_interface.cpp b/franka_semantic_components/src/franka_semantic_component_interface.cpp index 17481f08..ec8b05eb 100644 --- a/franka_semantic_components/src/franka_semantic_component_interface.cpp +++ b/franka_semantic_components/src/franka_semantic_component_interface.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/src/translation_utils.cpp b/franka_semantic_components/src/translation_utils.cpp new file mode 100644 index 00000000..f96e1738 --- /dev/null +++ b/franka_semantic_components/src/translation_utils.cpp @@ -0,0 +1,340 @@ +// Copyright (c) 2023 Franka Robotics GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "translation_utils.hpp" + +#include +#include + +namespace franka_semantic_components { +namespace translation { + +franka_msgs::msg::Errors errorsToMessage(const franka::Errors& error) { + franka_msgs::msg::Errors message; + message.joint_position_limits_violation = + static_cast( + error.joint_position_limits_violation); + message.cartesian_position_limits_violation = + static_cast( + error.cartesian_position_limits_violation); + message.self_collision_avoidance_violation = + static_cast( + error.self_collision_avoidance_violation); + message.joint_velocity_violation = + static_cast(error.joint_velocity_violation); + message.cartesian_velocity_violation = + static_cast( + error.cartesian_velocity_violation); + message.force_control_safety_violation = + static_cast( + error.force_control_safety_violation); + message.joint_reflex = static_cast(error.joint_reflex); + message.cartesian_reflex = + static_cast(error.cartesian_reflex); + message.max_goal_pose_deviation_violation = + static_cast( + error.max_goal_pose_deviation_violation); + message.max_path_pose_deviation_violation = + static_cast( + error.max_path_pose_deviation_violation); + message.cartesian_velocity_profile_safety_violation = + static_cast( + error.cartesian_velocity_profile_safety_violation); + message.joint_position_motion_generator_start_pose_invalid = + static_cast( + error.joint_position_motion_generator_start_pose_invalid); + message.joint_motion_generator_position_limits_violation = + static_cast( + error.joint_motion_generator_position_limits_violation); + message.joint_motion_generator_velocity_limits_violation = + static_cast( + error.joint_motion_generator_velocity_limits_violation); + message.joint_motion_generator_velocity_discontinuity = + static_cast( + error.joint_motion_generator_velocity_discontinuity); + message.joint_motion_generator_acceleration_discontinuity = + static_cast( + error.joint_motion_generator_acceleration_discontinuity); + message.cartesian_position_motion_generator_start_pose_invalid = + static_cast( + error.cartesian_position_motion_generator_start_pose_invalid); + message.cartesian_motion_generator_elbow_limit_violation = + static_cast( + error.cartesian_motion_generator_elbow_limit_violation); + message.cartesian_motion_generator_velocity_limits_violation = + static_cast( + error.cartesian_motion_generator_velocity_limits_violation); + message.cartesian_motion_generator_velocity_discontinuity = + static_cast( + error.cartesian_motion_generator_velocity_discontinuity); + message.cartesian_motion_generator_acceleration_discontinuity = + static_cast( + error.cartesian_motion_generator_acceleration_discontinuity); + message.cartesian_motion_generator_elbow_sign_inconsistent = + static_cast( + error.cartesian_motion_generator_elbow_sign_inconsistent); + message.cartesian_motion_generator_start_elbow_invalid = + static_cast( + error.cartesian_motion_generator_start_elbow_invalid); + message.cartesian_motion_generator_joint_position_limits_violation = + static_cast( + error.cartesian_motion_generator_joint_position_limits_violation); + message.cartesian_motion_generator_joint_velocity_limits_violation = + static_cast( + error.cartesian_motion_generator_joint_velocity_limits_violation); + message.cartesian_motion_generator_joint_velocity_discontinuity = + static_cast( + error.cartesian_motion_generator_joint_velocity_discontinuity); + message.cartesian_motion_generator_joint_acceleration_discontinuity = + static_cast( + error.cartesian_motion_generator_joint_acceleration_discontinuity); + message.cartesian_position_motion_generator_invalid_frame = + static_cast( + error.cartesian_position_motion_generator_invalid_frame); + message.force_controller_desired_force_tolerance_violation = + static_cast( + error.force_controller_desired_force_tolerance_violation); + message.controller_torque_discontinuity = + static_cast( + error.controller_torque_discontinuity); + message.start_elbow_sign_inconsistent = + static_cast( + error.start_elbow_sign_inconsistent); + message.communication_constraints_violation = + static_cast( + error.communication_constraints_violation); + message.power_limit_violation = + static_cast(error.power_limit_violation); + message.joint_p2p_insufficient_torque_for_planning = + static_cast( + error.joint_p2p_insufficient_torque_for_planning); + message.tau_j_range_violation = + static_cast(error.tau_j_range_violation); + message.instability_detected = + static_cast(error.instability_detected); + return message; +} + +/** + * @param input_wrench The wrench which should be translated + * @return geometry_msgs::msg::Wrench The translated wrench + */ +auto toWrench(const std::array& input_wrench) -> geometry_msgs::msg::Wrench { + geometry_msgs::msg::Wrench output_wrench; + output_wrench.force = geometry_msgs::build() + .x(input_wrench[0]) + .y(input_wrench[1]) + .z(input_wrench[2]); + output_wrench.torque = geometry_msgs::build() + .x(input_wrench[3]) + .y(input_wrench[4]) + .z(input_wrench[5]); + + return output_wrench; +} + +/** + * @param input_twist The twist which should be translated + * @return geometry_msgs::msg::Twist The translated twist + */ +auto toTwist(const std::array& input_twist) -> geometry_msgs::msg::Twist { + geometry_msgs::msg::Twist output_twist; + output_twist.linear = geometry_msgs::build() + .x(input_twist[0]) + .y(input_twist[1]) + .z(input_twist[2]); + output_twist.angular = geometry_msgs::build() + .x(input_twist[3]) + .y(input_twist[4]) + .z(input_twist[5]); + + return output_twist; +} + +/** + * @param input_accel The acceleration which should be translated + * @return geometry_msgs::msg::Accel The translated acceleration + */ +auto toAccel(const std::array& input_accel) -> geometry_msgs::msg::Accel { + geometry_msgs::msg::Accel output_accel; + output_accel.linear = geometry_msgs::build() + .x(input_accel[0]) + .y(input_accel[1]) + .z(input_accel[2]); + output_accel.angular = geometry_msgs::build() + .x(input_accel[3]) + .y(input_accel[4]) + .z(input_accel[5]); + + return output_accel; +} + +/** + * @param input_pose The pose which should be translated + * @return geometry_msgs::msg::Pose The translated pose + */ +auto toPose(const std::array& input_pose) -> geometry_msgs::msg::Pose { + const Eigen::Map transformation_matrix(input_pose.data()); + const Eigen::Quaterniond quaternion(transformation_matrix.topLeftCorner<3, 3>()); + const Eigen::Translation3d translation(transformation_matrix.block<3, 1>(0, 3)); + + geometry_msgs::msg::Pose output_pose; + output_pose.position = geometry_msgs::build() + .x(translation.x()) + .y(translation.y()) + .z(translation.z()); + output_pose.orientation = geometry_msgs::build() + .x(quaternion.x()) + .y(quaternion.y()) + .z(quaternion.z()) + .w(quaternion.w()); + + return output_pose; +} + +/** + * @param mass The mass for the inertia + * @param center_of_mass The center of mass for the inertia + * @param inertia_matrix The inertia matrix + * @return geometry_msgs::msg::Inertia The translated inertia + */ +auto toInertia(double mass, + const std::array& center_of_mass, + const std::array& inertia_matrix) -> geometry_msgs::msg::Inertia { + geometry_msgs::msg::Inertia output_inertia = + geometry_msgs::build() + .m(mass) + .com(geometry_msgs::build() + .x(center_of_mass[0]) + .y(center_of_mass[1]) + .z(center_of_mass[2])) + // Definition of inertia matrix can be found here: + // https://docs.ros2.org/latest/api/geometry_msgs/msg/Inertia.html + .ixx(inertia_matrix[0]) + .ixy(inertia_matrix[1]) + .ixz(inertia_matrix[2]) + .iyy(inertia_matrix[4]) + .iyz(inertia_matrix[5]) + .izz(inertia_matrix[8]); + + return output_inertia; +} + +/** + * The indicators within this message represent if a collision/contact is active for Cartesian/joint + * space + * @param cartesian_collision The Cartesian collision flags + * @param cartesian_contact The Cartesian contact flags + * @param joint_collision The joint collision flags + * @param joint_contact The joint contact flags + * @return franka_msgs::msg::CollisionIndicators The translated CollisionIndicator message + */ +auto toCollisionIndicators(const std::array& cartesian_collision, + const std::array& cartesian_contact, + const std::array& joint_collision, + const std::array& joint_contact) + -> franka_msgs::msg::CollisionIndicators { + franka_msgs::msg::CollisionIndicators collision_indicators; + collision_indicators.is_cartesian_linear_collision = + geometry_msgs::build() + .x(cartesian_collision[0]) + .y(cartesian_collision[1]) + .z(cartesian_collision[2]); + collision_indicators.is_cartesian_angular_collision = + geometry_msgs::build() + .x(cartesian_collision[3]) + .y(cartesian_collision[4]) + .z(cartesian_collision[5]); + + collision_indicators.is_cartesian_linear_contact = + geometry_msgs::build() + .x(cartesian_contact[0]) + .y(cartesian_contact[1]) + .z(cartesian_contact[2]); + collision_indicators.is_cartesian_angular_contact = + geometry_msgs::build() + .x(cartesian_contact[3]) + .y(cartesian_contact[4]) + .z(cartesian_contact[5]); + + std::copy(joint_collision.cbegin(), joint_collision.cend(), + collision_indicators.is_joint_collision.begin()); + std::copy(joint_contact.cbegin(), joint_contact.cend(), + collision_indicators.is_joint_contact.begin()); + + return collision_indicators; +} + +/** + * @param elbow The elbow position + * @param elbow_d The desired elbow position + * @param elbow_c The commanded elbow position + * @param delbow_c The commanded elbow velocity + * @param ddelbow_c The commanded elbow acceleration + * @return franka_msgs::msg::Elbow The translated elbow message + */ +auto toElbow(const std::array& elbow, + const std::array& elbow_d, + const std::array& elbow_c, + const std::array& delbow_c, + const std::array& ddelbow_c) -> franka_msgs::msg::Elbow { + franka_msgs::msg::Elbow elbow_message; + + for (size_t i = 0; i < elbow.size(); i++) { + elbow_message.position.at(i) = elbow.at(i); + elbow_message.desired_position.at(i) = elbow_d.at(i); + elbow_message.commanded_position.at(i) = elbow_c.at(i); + elbow_message.commanded_velocity.at(i) = delbow_c.at(i); + elbow_message.commanded_acceleration.at(i) = ddelbow_c.at(i); + } + + 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 new file mode 100644 index 00000000..88ae3f03 --- /dev/null +++ b/franka_semantic_components/src/translation_utils.hpp @@ -0,0 +1,120 @@ +// Copyright (c) 2023 Franka Robotics GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "franka/robot_state.h" + +#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" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "geometry_msgs/msg/wrench.hpp" + +namespace franka_semantic_components { +namespace translation { + +/** + * @param error The internal error buffer + * @return franka_msgs::msg::Errors The translated errors + */ +auto errorsToMessage(const franka::Errors& error) -> franka_msgs::msg::Errors; + +/** + * @param input_wrench The wrench which should be translated + * @return geometry_msgs::msg::Wrench The translated wrench + */ +auto toWrench(const std::array& input_wrench) -> geometry_msgs::msg::Wrench; + +/** + * @param input_twist The twist which should be translated + * @return geometry_msgs::msg::Twist The translated twist + */ +auto toTwist(const std::array& input_twist) -> geometry_msgs::msg::Twist; + +/** + * @param input_accel The acceleration which should be translated + * @return geometry_msgs::msg::Accel The translated acceleration + */ +auto toAccel(const std::array& input_accel) -> geometry_msgs::msg::Accel; + +/** + * @param input_pose The pose which should be translated + * @return geometry_msgs::msg::Pose The translated pose + */ +auto toPose(const std::array& input_pose) -> geometry_msgs::msg::Pose; + +/** + * @param mass The mass for the inertia + * @param center_of_mass The center of mass for the inertia + * @param inertia_matrix The inertia matrix + * @return geometry_msgs::msg::Inertia The translated inertia + */ +auto toInertia(double mass, + const std::array& center_of_mass, + const std::array& inertia_matrix) -> geometry_msgs::msg::Inertia; + +/** + * The indicators within this message represent if a collision/contact is active for Cartesian/joint + * space + * @param cartesian_collision The Cartesian collision flags + * @param cartesian_contact The Cartesian contact flags + * @param joint_collision The joint collision flags + * @param joint_contact The joint contact flags + * @return franka_msgs::msg::CollisionIndicators The translated CollisionIndicator message + */ +auto toCollisionIndicators(const std::array& cartesian_collision, + const std::array& cartesian_contact, + const std::array& joint_collision, + const std::array& joint_contact) + -> franka_msgs::msg::CollisionIndicators; + +/** + * @param elbow The elbow position + * @param elbow_d The desired elbow position + * @param elbow_c The commanded elbow position + * @param delbow_c The commanded elbow velocity + * @param ddelbow_c The commanded elbow acceleration + * @return franka_msgs::msg::Elbow The translated elbow message + */ +auto toElbow(const std::array& elbow, + const std::array& elbow_d, + const std::array& elbow_c, + 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 diff --git a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp index 174eb789..3a996153 100644 --- a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp +++ b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,6 +13,7 @@ // limitations under the License. #include "franka_cartesian_velocity_interface_test.hpp" +#include "../src/translation_utils.hpp" #include #include @@ -82,10 +83,16 @@ TEST_F(FrankaCartesianVelocityTest, given_correct_interfaces_set_velocity_with_elbow_command_expect_successful) { setUpInterfaces(true); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); std::array new_elbow_command{0.0, 2.0}; - auto success = - franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities, new_elbow_command); + auto success = franka_cartesian_command_friend->setCommand( + cartesian_linear_velocity, cartesian_angular_velocity, new_elbow_command); ASSERT_TRUE(success); @@ -99,10 +106,16 @@ TEST_F(FrankaCartesianVelocityTest, given_correct_interfaces_when_get_elbow_command_values_called_expect_successful) { setUpInterfaces(true); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); std::array new_elbow_command{0.0, 2.0}; - auto success_set_command = - franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities, new_elbow_command); + auto success_set_command = franka_cartesian_command_friend->setCommand( + cartesian_linear_velocity, cartesian_angular_velocity, new_elbow_command); ASSERT_TRUE(success_set_command); @@ -121,9 +134,15 @@ TEST_F(FrankaCartesianVelocityTest, given_elbow_is_not_activated_when_elbow_command_get_value_is_called_expect_throw) { setUpInterfaces(false); std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); - auto success_set_command = - franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities); + auto success_set_command = franka_cartesian_command_friend->setCommand( + cartesian_linear_velocity, cartesian_angular_velocity); ASSERT_TRUE(success_set_command); @@ -139,9 +158,16 @@ TEST_F( FrankaCartesianVelocityTest, given_elbow_and_cartesian_velocity_claimed_when_set_velocity_command_without_elbow_expect_failure) { setUpInterfaces(true); - std::array new_hw_cartesian_velocities_{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); - auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities_); + auto success = franka_cartesian_command_friend->setCommand(cartesian_linear_velocity, + cartesian_angular_velocity); ASSERT_FALSE(success); @@ -152,11 +178,17 @@ TEST_F( FrankaCartesianVelocityTest, given_only_cartesian_velocity_claimed_without_elbow_when_set_velocity_command_cartesian_vel_expect_successful) { setUpInterfaces(false); - std::array new_hw_cartesian_velocities_{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); std::array default_zero_elbow_command{0.0, 0.0}; - auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities_, - default_zero_elbow_command); + auto success = franka_cartesian_command_friend->setCommand( + cartesian_linear_velocity, cartesian_angular_velocity, default_zero_elbow_command); ASSERT_FALSE(success); @@ -168,12 +200,19 @@ TEST_F(FrankaCartesianVelocityTest, franka_cartesian_command_friend = std::make_unique(true); hardware_interface::CommandInterface dummy_command_interface{"dummy", "dummy_cartesian_velocity", &hw_elbow_command_.at(0)}; - std::array new_hw_cartesian_velocities_{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + std::array new_hw_cartesian_velocities{0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + Eigen::Vector3d cartesian_linear_velocity(new_hw_cartesian_velocities[0], + new_hw_cartesian_velocities[1], + new_hw_cartesian_velocities[2]); + Eigen::Vector3d cartesian_angular_velocity(new_hw_cartesian_velocities[3], + new_hw_cartesian_velocities[4], + new_hw_cartesian_velocities[5]); temp_command_interfaces.emplace_back(dummy_command_interface); franka_cartesian_command_friend->assign_loaned_command_interfaces(temp_command_interfaces); - auto success = franka_cartesian_command_friend->setCommand(new_hw_cartesian_velocities_); + auto success = franka_cartesian_command_friend->setCommand(cartesian_linear_velocity, + cartesian_angular_velocity); ASSERT_FALSE(success); @@ -193,4 +232,4 @@ TEST_F(FrankaCartesianVelocityTest, setUpInterfaces(false); ASSERT_THROW(franka_cartesian_command_friend->getInitialElbowConfiguration(), std::runtime_error); -} \ No newline at end of file +} diff --git a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp index 1a4147d4..036f6f58 100644 --- a/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp +++ b/franka_semantic_components/test/franka_cartesian_velocity_interface_test.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/test/franka_robot_model_test.cpp b/franka_semantic_components/test/franka_robot_model_test.cpp index 990d1c5f..e9009af2 100644 --- a/franka_semantic_components/test/franka_robot_model_test.cpp +++ b/franka_semantic_components/test/franka_robot_model_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/test/franka_robot_model_test.hpp b/franka_semantic_components/test/franka_robot_model_test.hpp index 17815ffa..350b3d25 100644 --- a/franka_semantic_components/test/franka_robot_model_test.hpp +++ b/franka_semantic_components/test/franka_robot_model_test.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/test/franka_robot_state_test.cpp b/franka_semantic_components/test/franka_robot_state_test.cpp index 9761e3a1..1b92722f 100644 --- a/franka_semantic_components/test/franka_robot_state_test.cpp +++ b/franka_semantic_components/test/franka_robot_state_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -32,6 +32,7 @@ void FrankaRobotStateTest::SetUp() { robot_state.q = joint_angles; robot_state.q_d = joint_velocities; + robot_state.O_T_EE = end_effector_pose; robot_state.robot_mode = robot_mode; hardware_interface::StateInterface franka_hw_state{ @@ -68,12 +69,27 @@ TEST_F(FrankaRobotStateTest, robot_state_ptr_uncasted_correctly) { } TEST_F(FrankaRobotStateTest, - given_franka_semantic_state_initialized_when_message_returned_expect_correct_values) { - ASSERT_EQ(joint_angles, franka_robot_state_msg.q); - ASSERT_EQ(joint_velocities, franka_robot_state_msg.q_d); + givenFrankaSemanticStateInitialized_whenMessageReturnedExpectsCorrectValues) { + ASSERT_THAT(joint_angles, + ::testing::ElementsAreArray(franka_robot_state_msg.measured_joint_state.position)); + ASSERT_THAT(joint_velocities, + ::testing::ElementsAreArray(franka_robot_state_msg.desired_joint_state.position)); + + ASSERT_EQ(end_effector_pose[12], franka_robot_state_msg.o_t_ee.pose.position.x); + ASSERT_EQ(end_effector_pose[13], franka_robot_state_msg.o_t_ee.pose.position.y); + ASSERT_EQ(end_effector_pose[14], franka_robot_state_msg.o_t_ee.pose.position.z); + ASSERT_EQ(franka_msgs::msg::FrankaRobotState::ROBOT_MODE_USER_STOPPED, franka_robot_state_msg.robot_mode); franka_state_friend->release_interfaces(); // validate the count of state_interfaces_ ASSERT_EQ(franka_state_friend->state_interfaces_.size(), 0u); -} \ No newline at end of file +} + +TEST_F(FrankaRobotStateTest, givenInitializedRobotStateMsg_thenCorrectFrameIDs) { + franka_state_friend->initialize_robot_state_msg(franka_robot_state_msg); + + ASSERT_EQ(franka_robot_state_msg.o_t_ee.header.frame_id, "panda_link0"); + ASSERT_EQ(franka_robot_state_msg.ee_t_k.header.frame_id, "panda_link8"); + ASSERT_EQ(franka_robot_state_msg.measured_joint_state.name[1], "panda_link2"); +} diff --git a/franka_semantic_components/test/franka_robot_state_test.hpp b/franka_semantic_components/test/franka_robot_state_test.hpp index e542c7e2..2b79b256 100644 --- a/franka_semantic_components/test/franka_robot_state_test.hpp +++ b/franka_semantic_components/test/franka_robot_state_test.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,7 +26,7 @@ class FrankaRobotStateTestFriend : public franka_semantic_components::FrankaRobotState { FRIEND_TEST(FrankaRobotStateTest, validate_state_names_and_size); FRIEND_TEST(FrankaRobotStateTest, - given_franka_semantic_state_initialized_when_message_returned_expect_correct_values); + givenFrankaSemanticStateInitialized_whenMessageReturnedExpectsCorrectValues); FRIEND_TEST(FrankaRobotStateTest, robot_state_ptr_uncasted_correctly); public: @@ -52,6 +52,8 @@ class FrankaRobotStateTest : public ::testing::Test { std::array joint_angles = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; std::array joint_velocities = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::array end_effector_pose = {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 2.2, 3.8, 93.23, 1.0}; franka::RobotMode robot_mode = franka::RobotMode::kUserStopped; franka_msgs::msg::FrankaRobotState franka_robot_state_msg; diff --git a/franka_semantic_components/test/franka_semantic_interface_test.cpp b/franka_semantic_components/test/franka_semantic_interface_test.cpp index 0565a170..0d80ad48 100644 --- a/franka_semantic_components/test/franka_semantic_interface_test.cpp +++ b/franka_semantic_components/test/franka_semantic_interface_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/franka_semantic_components/test/franka_semantic_interface_test.hpp b/franka_semantic_components/test/franka_semantic_interface_test.hpp index 64dfa65d..38474e87 100644 --- a/franka_semantic_components/test/franka_semantic_interface_test.hpp +++ b/franka_semantic_components/test/franka_semantic_interface_test.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Franka Emika GmbH +// Copyright (c) 2023 Franka Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License.