From 5ccc08b13bf47ef0509317eaa1eb20eaea6909ae Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 30 Aug 2024 16:15:07 +0200 Subject: [PATCH] add teleop controller (backport #35) (#37) * add teleop controller (#35) * import teleoperation msgs * import and refractor teleoperation controller (from https://github.com/tpoignonec/passive_vic_teleop_ros2 / commit SHA = de3d28f0d4564299251f8d38f36908fe1d762b5c) * fix teleop controller name * fix rule plugin export * remove testing for now * misc fixes * #IF guard for humble * fix deps * comment out humble-specific code (cherry picked from commit 5d1210779e3b451dfb45c2438456ddf04322e0ef) * reintroduce humble specific precautions in teleop ctrl --------- Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com> Co-authored-by: Thibault Poignonec --- cartesian_control_msgs/CMakeLists.txt | 6 + .../msg/TeleopCompliance.msg | 8 + .../msg/TeleopControllerState.msg | 44 ++ cartesian_vic_controller/CMakeLists.txt | 2 +- .../CMakeLists.txt | 107 ++++ .../cartesian_vic_teleop_controller.xml | 9 + .../cartesian_vic_teleop_rules.xml | 9 + .../Ros2VersionConfig.h.in | 3 + .../cartesian_vic_teleop_controller.hpp | 129 ++++ .../cartesian_vic_teleop_logic.hpp | 203 +++++++ .../mapping_manager.hpp | 242 ++++++++ .../rules/vanilla_teleop_rule.hpp | 53 ++ .../teleop_data.hpp | 93 +++ .../teleop_rule.hpp | 87 +++ .../visibility_control.h | 49 ++ cartesian_vic_teleop_controller/package.xml | 41 ++ .../src/cartesian_vic_teleop_controller.cpp | 385 ++++++++++++ .../src/cartesian_vic_teleop_logic.cpp | 575 ++++++++++++++++++ .../src/mapping_manager.cpp | 263 ++++++++ .../src/rules/vanilla_teleop_rule.cpp | 108 ++++ .../src/teleop_data.cpp | 114 ++++ .../src/teleop_rule.cpp | 73 +++ 22 files changed, 2602 insertions(+), 1 deletion(-) create mode 100644 cartesian_control_msgs/msg/TeleopCompliance.msg create mode 100644 cartesian_control_msgs/msg/TeleopControllerState.msg create mode 100644 cartesian_vic_teleop_controller/CMakeLists.txt create mode 100644 cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml create mode 100644 cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/mapping_manager.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/rules/vanilla_teleop_rule.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_data.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_rule.hpp create mode 100644 cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/visibility_control.h create mode 100644 cartesian_vic_teleop_controller/package.xml create mode 100644 cartesian_vic_teleop_controller/src/cartesian_vic_teleop_controller.cpp create mode 100644 cartesian_vic_teleop_controller/src/cartesian_vic_teleop_logic.cpp create mode 100644 cartesian_vic_teleop_controller/src/mapping_manager.cpp create mode 100644 cartesian_vic_teleop_controller/src/rules/vanilla_teleop_rule.cpp create mode 100644 cartesian_vic_teleop_controller/src/teleop_data.cpp create mode 100644 cartesian_vic_teleop_controller/src/teleop_rule.cpp diff --git a/cartesian_control_msgs/CMakeLists.txt b/cartesian_control_msgs/CMakeLists.txt index ade7192..9748ec5 100644 --- a/cartesian_control_msgs/CMakeLists.txt +++ b/cartesian_control_msgs/CMakeLists.txt @@ -19,12 +19,18 @@ find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) set(msg_files +# General cartesian msgs msg/CartesianState.msg msg/CartesianTrajectoryPoint.msg msg/CartesianTrajectory.msg msg/CartesianCompliance.msg + # VIC msgs msg/CompliantFrameTrajectory.msg msg/VicControllerState.msg + # Teleoperation msgs + msg/TeleopControllerState.msg + msg/TeleopCompliance.msg + # Debug msgs msg/KeyValues.msg ) diff --git a/cartesian_control_msgs/msg/TeleopCompliance.msg b/cartesian_control_msgs/msg/TeleopCompliance.msg new file mode 100644 index 0000000..a76cd7f --- /dev/null +++ b/cartesian_control_msgs/msg/TeleopCompliance.msg @@ -0,0 +1,8 @@ +# Note: all matrices are expressed in Follower frame of reference!!! +std_msgs/Float64MultiArray leader_desired_inertia +std_msgs/Float64MultiArray leader_desired_stiffness +std_msgs/Float64MultiArray leader_desired_damping + +std_msgs/Float64MultiArray follower_desired_inertia +std_msgs/Float64MultiArray follower_desired_stiffness +std_msgs/Float64MultiArray follower_desired_damping diff --git a/cartesian_control_msgs/msg/TeleopControllerState.msg b/cartesian_control_msgs/msg/TeleopControllerState.msg new file mode 100644 index 0000000..c529f0f --- /dev/null +++ b/cartesian_control_msgs/msg/TeleopControllerState.msg @@ -0,0 +1,44 @@ +std_msgs/Header header + +# Teleop controller input data +std_msgs/Bool workspace_is_engaged + +geometry_msgs/Pose x_1 +geometry_msgs/Twist x_1_dot +geometry_msgs/Accel x_1_ddot +geometry_msgs/Wrench f_1 + +std_msgs/Float64MultiArray desired_inertia_1 +std_msgs/Float64MultiArray desired_damping_1 +std_msgs/Float64MultiArray desired_stiffness_1 + +geometry_msgs/Pose x_2 +geometry_msgs/Twist x_2_dot +geometry_msgs/Accel x_2_ddot +geometry_msgs/Wrench f_2 + +std_msgs/Float64MultiArray desired_inertia_2 +std_msgs/Float64MultiArray desired_damping_2 +std_msgs/Float64MultiArray desired_stiffness_2 + +# Teleop controller ref +geometry_msgs/Pose x_1_d +geometry_msgs/Twist x_1_dot_d +geometry_msgs/Accel x_1_ddot_d +geometry_msgs/Wrench f_1_d + +std_msgs/Float64MultiArray rendered_inertia_1 +std_msgs/Float64MultiArray rendered_damping_1 +std_msgs/Float64MultiArray rendered_stiffness_1 + +geometry_msgs/Pose x_2_d +geometry_msgs/Twist x_2_dot_d +geometry_msgs/Accel x_2_ddot_d +geometry_msgs/Wrench f_2_d + +std_msgs/Float64MultiArray rendered_inertia_2 +std_msgs/Float64MultiArray rendered_damping_2 +std_msgs/Float64MultiArray rendered_stiffness_2 + +# Misc. +cartesian_control_msgs/KeyValues diagnostic_data diff --git a/cartesian_vic_controller/CMakeLists.txt b/cartesian_vic_controller/CMakeLists.txt index 7a1f028..ecc473c 100644 --- a/cartesian_vic_controller/CMakeLists.txt +++ b/cartesian_vic_controller/CMakeLists.txt @@ -82,7 +82,7 @@ target_link_libraries(cartesian_vic_rules PUBLIC ament_target_dependencies(cartesian_vic_rules PUBLIC ${DEPENDENCIES}) pluginlib_export_plugin_description_file(cartesian_vic_controller cartesian_vic_rules.xml) -if(BUILD_TESTING) +if(0) # (BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(ros2_control_test_assets REQUIRED) diff --git a/cartesian_vic_teleop_controller/CMakeLists.txt b/cartesian_vic_teleop_controller/CMakeLists.txt new file mode 100644 index 0000000..644125c --- /dev/null +++ b/cartesian_vic_teleop_controller/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.16) +project(cartesian_vic_teleop_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +# Use CMake to pass the current ROS_DISTRO via variables into a preprocessor template. +# We then include this file and switch between the different APIs. +if($ENV{ROS_DISTRO} STREQUAL "rolling") + set(CARTESIAN_CONTROLLERS_ROLLING TRUE) +elseif($ENV{ROS_DISTRO} STREQUAL "jazzy") + set(CARTESIAN_CONTROLLERS_JAZZY TRUE) +elseif($ENV{ROS_DISTRO} STREQUAL "humble") + set(CARTESIAN_CONTROLLERS_HUMBLE TRUE) +else() + message(WARNING "ROS2 version must be {rolling|jazzy|humble}") +endif() +configure_file(include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in include/cartesian_vic_teleop_controller/Ros2VersionConfig.h) + +set(DEPENDENCIES + Eigen3 + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs + cartesian_control_msgs + cartesian_vic_controller +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${DEPENDENCIES}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(${PROJECT_NAME} SHARED + # ROS2 controller impl. + src/cartesian_vic_teleop_controller.cpp + # Logic + src/cartesian_vic_teleop_logic.cpp + src/mapping_manager.cpp + src/teleop_data.cpp + src/teleop_rule.cpp + # Async node impl. + # TODO +) +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + # $ # LP solvers + $ # ROS2VersionConfig.h + $ +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPENDENCIES}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(${PROJECT_NAME} PRIVATE "cartesian_vic_teleop_controller_BUILDING_DLL") + +pluginlib_export_plugin_description_file(controller_interface cartesian_vic_teleop_controller.xml) + +# VIC-based teleoperation rule plugins library +add_library(cartesian_vic_teleop_rules SHARED + src/rules/vanilla_teleop_rule.cpp +) +target_compile_features(cartesian_vic_teleop_rules PUBLIC cxx_std_17) +target_include_directories(cartesian_vic_teleop_rules PUBLIC + $ + $ +) +target_link_libraries(cartesian_vic_teleop_rules PUBLIC + cartesian_vic_teleop_controller +) +ament_target_dependencies( + cartesian_vic_teleop_rules + PUBLIC + ${DEPENDENCIES} +) +pluginlib_export_plugin_description_file(cartesian_vic_teleop_controller cartesian_vic_teleop_rules.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS + ${PROJECT_NAME} + cartesian_vic_teleop_rules + EXPORT export_cartesian_vic_teleop_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_cartesian_vic_teleop_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${DEPENDENCIES}) +ament_package() diff --git a/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml b/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml new file mode 100644 index 0000000..8f4f269 --- /dev/null +++ b/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.xml @@ -0,0 +1,9 @@ + + + + Cartesian teleoperation controller (leader-side only!) based on the cartesian_vic_controller package. + + + diff --git a/cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml b/cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml new file mode 100644 index 0000000..9cd1dbe --- /dev/null +++ b/cartesian_vic_teleop_controller/cartesian_vic_teleop_rules.xml @@ -0,0 +1,9 @@ + + + + Basic position-position with force feedback. + + + diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in new file mode 100644 index 0000000..ec2df71 --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/Ros2VersionConfig.h.in @@ -0,0 +1,3 @@ +#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING ${CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING} +#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY ${CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY} +#cmakedefine CARTESIAN_VIC_TELEOP_CONTROLLER_HUMBLE ${CARTESIAN_VIC_TELEOP_CONTROLLER_HUMBLE} diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp new file mode 100644 index 0000000..0ff6a23 --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp @@ -0,0 +1,129 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "cartesian_vic_teleop_controller/visibility_control.h" +#include "cartesian_vic_teleop_controller/mapping_manager.hpp" +#include "cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + +// Base class for VIC controller +#include "cartesian_vic_controller/cartesian_vic_controller.hpp" + +// ROS2 standard msgs +#include "std_msgs/msg/bool.hpp" + +// Custom msgs +#include "cartesian_control_msgs/msg/vic_controller_state.hpp" +#include "cartesian_control_msgs/msg/cartesian_trajectory.hpp" +#include "cartesian_control_msgs/msg/compliant_frame_trajectory.hpp" +#include "cartesian_control_msgs/msg/teleop_controller_state.hpp" +#include "cartesian_control_msgs/msg/teleop_compliance.hpp" + +namespace cartesian_vic_teleop_controller +{ +class CartesianVicTeleopController : public cartesian_vic_controller::CartesianVicController +{ +public: + CartesianVicTeleopController(); + virtual ~CartesianVicTeleopController() = default; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + + CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State & previous_state) override; + + using Base = cartesian_vic_controller::CartesianVicController; + +protected: + PassiveVicTeleopLogic teleop_logic_; + + // Publisher teleop controller state (for logging purposes only) + rclcpp::Publisher::SharedPtr + non_rt_teleop_state_publisher_; + std::unique_ptr> teleop_state_publisher_; + cartesian_control_msgs::msg::TeleopControllerState teleop_state_msg_; + + // Publisher follower robot VIC reference trajectory + rclcpp::Publisher::SharedPtr + non_rt_follower_vic_ref_publisher_; + std::unique_ptr> follower_vic_ref_publisher_; + + // Subscriber to follower VIC state + // TODO(tpoignonec): remove follower_vic_state_msg_ + cartesian_control_msgs::msg::VicControllerState follower_vic_state_msg_; + std::shared_ptr + follower_vic_state_msg_ptr_; + rclcpp::Subscription::SharedPtr + follower_vic_state_subscriber_; + realtime_tools::RealtimeBuffer> input_follower_vic_state_msg_; + + // Subscriber to teleoperation compliance ref. + rclcpp::Subscription< + cartesian_control_msgs::msg::TeleopCompliance>::SharedPtr teleop_compliance_subscriber_; + realtime_tools::RealtimeBuffer> input_teleop_compliance_msg_; + std::shared_ptr teleop_compliance_msg_ptr_; + + // Subscriber to follower VIC state + rclcpp::Subscription::SharedPtr is_clutched_subscriber_; + realtime_tools::RealtimeBuffer> input_is_clutched_msg_; + std::shared_ptr is_clutched_msg_ptr_; + + // Storage + cartesian_control_msgs::msg::CompliantFrameTrajectory leader_vic_ref_; + cartesian_control_msgs::msg::CompliantFrameTrajectory follower_vic_ref_; +}; + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_CONTROLLER_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp new file mode 100644 index 0000000..4642d2e --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp @@ -0,0 +1,203 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_LOGIC_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_LOGIC_HPP_ + +#include +#include + +#include "pluginlib/class_loader.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/time.hpp" + +#include "cartesian_vic_teleop_controller/Ros2VersionConfig.h" // ROS2 version macros + +#include "cartesian_vic_teleop_controller/mapping_manager.hpp" +#include "cartesian_vic_teleop_controller/teleop_data.hpp" +#include "cartesian_vic_teleop_controller/teleop_rule.hpp" + +#include "cartesian_control_msgs/msg/teleop_controller_state.hpp" +#include "cartesian_control_msgs/msg/teleop_compliance.hpp" + +// VIC controllers state +#include "cartesian_vic_controller/cartesian_vic_state.hpp" + +// Custom msgs +#include "cartesian_control_msgs/msg/vic_controller_state.hpp" +#include "cartesian_control_msgs/msg/cartesian_trajectory.hpp" +#include "cartesian_control_msgs/msg/compliant_frame_trajectory.hpp" + +namespace cartesian_vic_teleop_controller +{ + +using VicStateMsg = cartesian_control_msgs::msg::VicControllerState; +using CompliantFrameTrajectoryMsg = cartesian_control_msgs::msg::CompliantFrameTrajectory; + +class PassiveVicTeleopLogic +{ +public: + PassiveVicTeleopLogic(); + ~PassiveVicTeleopLogic() = default; + + /// Initialize the logic from VicStateMsg messages + bool init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg, + const std::string & param_namespace = "teleoperation"); + + /// Convenience method to initialize the logic from leader input data + bool init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const cartesian_vic_controller::VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg, + const std::string & param_namespace = "teleoperation"); + + /// Update the logic: update mapping, update rule, ad compute the compliant trajectories + bool update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged, + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg); + + /// Convenience method to update the logic from leader input data + bool update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged, + const cartesian_vic_controller::VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg); + + /// Set the desired compliance of the leader / follower VIC controllers + bool setTeleoperationCompliance(const cartesian_control_msgs::msg::TeleopCompliance & msg); + + /// Return true if the logic has been initialized + bool is_initialized() const {return is_initialized_;} + + /// Get the computed leader compliant trajectory + bool get_leader_vic_ref(CompliantFrameTrajectoryMsg & leader_vic_ref_msg); + + /// Get the computed follower compliant trajectory + bool get_follower_vic_ref(CompliantFrameTrajectoryMsg & follower_vic_ref_msg); + + /// Retrieve the TeleopControllerState msg from rule. Call update() first!!! + bool get_current_state_msg(cartesian_control_msgs::msg::TeleopControllerState & msg); + +protected: + // Internal methods + bool extract_input_data( + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg); + + bool extract_input_data( + const cartesian_vic_controller::VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg); + + bool extract_follower_input_data( + const VicStateMsg & follower_vic_state_msg); + + bool internal_init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const std::string & param_namespace); + + bool internal_update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged); + +protected: + /// ROS2 local logger + rclcpp::Logger logger_; + + /// ROS2 local clock for throttle-type logging + rclcpp::Clock internal_clock_; + + /// True if init() has been called successfully. + bool is_initialized_ = false; + + /// True if update() has been called successfully at least once. + bool has_update_been_called_ = false; + + /// Ros2 parameter server + std::shared_ptr parameters_interface_; + + /// Teleop rule plugin loader + std::shared_ptr> + teleop_rule_loader_; + + /// Teleop rule plugin instance + std::unique_ptr teleop_rule_; + + /// True if the init() function of the rule has been called successfully. + bool is_teleop_rule_initialized_ = false; + + /// Mapping manager: mapping from/to master/follower workspaces + MappingManager mapping_manager_; + + /// Current pose mapping, i.e., follower_pose = transformation * leader_pose + Eigen::Isometry3d transformation_leader_to_follower; + + /* Most recent measurement data + * + * !!!IMPORTANT!!! All in the follower base frame + */ + TeleopDataInput teleop_data_input_; + + /** Computed VIC references for the leader and follower robots + * + * !!!IMPORTANT!!! All in the follower base frame + */ + TeleopDataOutput teleop_data_output_; + + /// Computed compliant frame trajectory for the leader robot + CompliantFrameTrajectoryMsg leader_vic_ref_; + + /// Computed compliant frame trajectory for the follower robot + CompliantFrameTrajectoryMsg follower_vic_ref_; + +private: + // Transient data storage, internal use only! + Eigen::Isometry3d leader_pose_in_leader_base_frame_; + Eigen::Matrix leader_velocity_in_leader_base_frame_; + Eigen::Matrix leader_acceleration_in_leader_base_frame_; + Eigen::Matrix leader_wrench_in_leader_base_frame_; + Eigen::Matrix leader_natural_inertia_in_leader_base_frame_; + + Eigen::Isometry3d follower_pose_in_follower_base_frame_; + Eigen::Matrix follower_velocity_in_follower_base_frame_; + Eigen::Matrix follower_acceleration_in_follower_base_frame_; + Eigen::Matrix follower_wrench_in_follower_base_frame_; + Eigen::Matrix follower_natural_inertia_in_follower_base_frame_; + + Eigen::Isometry3d leader_reference_pose_in_leader_base_frame_; + Eigen::Matrix leader_reference_velocity_in_leader_base_frame_; + Eigen::Matrix leader_reference_acceleration_in_leader_base_frame_; + Eigen::Matrix leader_reference_wrench_in_leader_base_frame_; + Eigen::Matrix leader_reference_inertia_in_leader_base_frame_; + Eigen::Matrix leader_reference_stiffness_in_leader_base_frame_; + Eigen::Matrix leader_reference_damping_in_leader_base_frame_; +}; + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__CARTESIAN_VIC_TELEOP_LOGIC_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/mapping_manager.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/mapping_manager.hpp new file mode 100644 index 0000000..b19bfde --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/mapping_manager.hpp @@ -0,0 +1,242 @@ +// Copyright 2022, ICube Laboratory, University of Strasbourg +// +// 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. + + +//----------------------------------------------------------------------------- +/*!\file mapping_manager.hpp + * + * \author thibault Poignonec + * \date 2022/06/25 + * + * + * Note: imported from https://github.com/tpoignonec/teleop_controllers + */ +//----------------------------------------------------------------------------- + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__MAPPING_MANAGER_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__MAPPING_MANAGER_HPP_ + +#include +#include + +#include +#include + +#include + +namespace cartesian_vic_teleop_controller +{ + +class MappingManager +{ +public: + enum CLUTCHING_STATE + { + DISABLED, + ENGAGED, + DISENGAGED + }; + + MappingManager(); + ~MappingManager(); + + /** + * @brief Init mapping manager: instantiate/reset object and initialize master->follower mapping + * + * @todo Add a "clutch rotation"cparameter (disabled for now and should be disabled by default if implemented) + */ + bool init( + const Eigen::Matrix3d & master_base_to_follower_base_axis_mapping + ); + + /** + * @brief Update mapping manager internal state. To be call at each time period! + */ + bool update( + const Eigen::Isometry3d & master_pose_in_master_base, + bool clutch_is_on = false + ); + + /** + * @brief Return true if the mapping has been initialized + */ + bool is_initialized() {return mapping_initialized_;} + + /** + * @brief TODO + * + * @param follower_pose_in_follower_base TODO + * @param master_pose_in_master_base TODO + * @param master_base_to_follower_base_axis_mapping TODO + */ + + bool reset_mapping( + const Eigen::Isometry3d & follower_pose_in_follower_base, + const Eigen::Isometry3d & master_pose_in_master_base, + const Eigen::Matrix3d & master_base_to_follower_base_axis_mapping + ); + /// @overload + bool reset_mapping( + const Eigen::Isometry3d & follower_pose_in_follower_base, + const Eigen::Isometry3d & master_pose_in_master_base + ); + + + // ------------- Pose mapping utils --------------------- + /** + * @brief Compute mapping from master pose to follower pose + * + * @param master_pose_in_master_base Cartesian pose of the master robot tool w.r.t. its frame of reference ('master base'). + * The pose is represented as a homogeneous rigid transformation \f${}^{\text{master base}}T_{\text{master tool}}\f$. + * @param mapped_master_pose Mapping of the master into follower robot operational space such that the return value is + * \f$ {}^{\text{follower base}}T_{\text{master base}} . {}^{\text{master base}}T_{\text{master_tool}} . {}^{\text{master_tool}}T_{\text{follower tool}}\f$ + */ + bool map_master_tool_to_follower_tool( + const Eigen::Isometry3d & master_pose_in_master_base, + Eigen::Isometry3d & mapped_master_pose) const; + + /** + * @brief Compute inverse mapping (from follower to master) + * + * @see map_master_tool_to_follower_tool + */ + bool inverse_map_follower_tool_to_master_tool( + const Eigen::Isometry3d & follower_pose_in_follower_base, + Eigen::Isometry3d & mapped_follower_pose) const; + + /* Additional accessors (should not be useful) */ + /** + * @brief Compute mapping from master (tool) pose to + * Get internal (partial) mapping \f${}^{\text{follower base}} T_{\text{master base}}\f$ + * as 4X4 homogeneous matrix + */ + const Eigen::Isometry3d & get_transformation_from_master_base_to_follower_base() const + { + return master_base_to_follower_base_mapping_; + } + /** + * @brief Get internal (partial) mapping \f${}^{\text{master_tool}} T_{\text{follower tool}}\f$ + * as 4X4 homogeneous matrix + */ + const Eigen::Isometry3d & get_transformation_from_follower_tool_to_master_tool() const + { + return follower_tool_to_master_tool_axis_mapping_; + } + + // ------------- Twist mapping utils --------------------- + /** + * @brief Change the reference frame (master_base -> master_base, but using orientation only!) + * of a twist/wrench 6x1 vector + * + * @param vector_in_master_base Input twist/wrench vector expressed in master base frame + * @param vector_in_follower_base Resulting twist/wrench vector expressed in follower base frame + * + * @see map_tensor + */ + bool map_twist_or_wrench_from_master_to_follower( + const Eigen::Matrix & vector_in_master_base, + Eigen::Matrix & vector_in_follower_base) const; + + /** + * @brief Change the reference frame (master_base -> master_base, but using orientation only!) of a twist/wrench 6x1 vector + * + * @param vector_in_master_base Input twist/wrench vector expressed in master base frame + * @param vector_in_follower_base Resulting twist/wrench vector expressed in follower base frame + * + * @see map_tensor + */ + bool map_twist_or_wrench_from_follower_to_master( + const Eigen::Matrix & vector_in_follower_base, + Eigen::Matrix & vector_in_master_base) const; + + bool map_SDPD_from_master_to_follower( + const Eigen::Matrix & SDPD_in_master_base, + Eigen::Matrix & SDPD_in_follower_base); + + bool map_SDPD_from_follower_to_master( + const Eigen::Matrix & SDPD_in_follower_base, + Eigen::Matrix & SDPD_in_master_base); + + // ------------- Workspace clutching --------------------- + /// Returns the state of the workspace clutching is enabled (disable by default!) + CLUTCHING_STATE get_clutching_state() const + { + if (!use_workspace_clutching_) { + return CLUTCHING_STATE::DISABLED; + } else if (workspaces_disengaged_) { + return CLUTCHING_STATE::DISENGAGED; + } else { + return CLUTCHING_STATE::ENGAGED; + } + } + + /// Enable/disable workspace clutching (disabled by default!) + bool set_clutching_enabled(bool value) + { + // Error if 'workspaces_disengaged_' is true + if (workspaces_disengaged_) { + // TODO(tpoignonec:) Move to .cpp and add ERROR msg + return false; + } + // Update and return + use_workspace_clutching_ = value; + return true; + } + + /// Returns 'True' is workspace clutching is enabled (disable by default!) + bool is_clutching_enabled() const {return use_workspace_clutching_;} + +protected: + // ROS2 logging + rclcpp::Logger logger_; + // Supervision flags + /// False by default, use 'MappingManager::set_clutching_enabled()' method to activate + bool use_workspace_clutching_ = true; + /// false until the 'MappingManager::init()' method has been called + bool mapping_initialized_ = false; + // Current mapping state + /* When true, a master side displacement does not impact its mapping into the follower workspace + * (i.e., the two robot are independent) + */ + bool workspaces_disengaged_ = true; + /// \f$ {}^{\text{follower base}}T_{\text{master base}} \f$ + Eigen::Isometry3d master_base_to_follower_base_mapping_; + /// \f$ {}^{\text{master_tool}}T_{\text{follower tool}} \f$ + Eigen::Isometry3d follower_tool_to_master_tool_axis_mapping_; + Eigen::Matrix3d master_base_to_follower_base_axis_mapping_; + // Cached history + Eigen::Isometry3d last_master_pose_in_master_base_; ///< \f$ x_m[k-1]\f$ + Eigen::Isometry3d last_master_pose_mapped_into_follower_base_; ///< \f$ \mathcal{M}(x_m[k-1])\f$ + + Eigen::Matrix tmp_6x6_matrix_; + + // ------------- Misc. functions --------------------- + /** + * @brief Internal method to send verbose msgs to the ros logger + */ + void info_verbose(const std::string & msg) const; + + /** + * @brief Internal method used to map the twist and wrenches + * + */ + void map_tensor( + const Eigen::Matrix3d & orientation, + const Eigen::Matrix & in, + Eigen::Matrix & out) const; +}; + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__MAPPING_MANAGER_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/rules/vanilla_teleop_rule.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/rules/vanilla_teleop_rule.hpp new file mode 100644 index 0000000..b092907 --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/rules/vanilla_teleop_rule.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__RULES__VANILLA_TELEOP_RULE_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__RULES__VANILLA_TELEOP_RULE_HPP_ + +#include +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/time.hpp" + +#include "cartesian_vic_teleop_controller/teleop_rule.hpp" +#include "cartesian_vic_teleop_controller/teleop_data.hpp" + +namespace cartesian_vic_teleop_controller +{ + +class VanillaTeleopRule : public TeleopRule +{ +public: + VanillaTeleopRule(); + ~VanillaTeleopRule() = default; + + bool init( + std::shared_ptr parameters_interface, + const std::string & param_namespace = "teleoperation") override; + + bool update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + const TeleopDataInput & teleop_data_input, + TeleopDataOutput & teleop_data_output) override; +}; + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__RULES__VANILLA_TELEOP_RULE_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_data.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_data.hpp new file mode 100644 index 0000000..f18b8c9 --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_data.hpp @@ -0,0 +1,93 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_DATA_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_DATA_HPP_ + +#include +#include + +#include +#include + +#include "cartesian_control_msgs/msg/key_values.hpp" +#include "cartesian_control_msgs/msg/teleop_controller_state.hpp" + +namespace cartesian_vic_teleop_controller +{ + +/// Container for input teleop data !!!IMPORTANT!!! All in the follower frame!!! +struct TeleopDataInput +{ + bool workspace_is_engaged = false; + + Eigen::Isometry3d leader_pose; + Eigen::Matrix leader_velocity; + Eigen::Matrix leader_acceleration; + Eigen::Matrix leader_wrench; + Eigen::Matrix leader_natural_inertia; + + Eigen::Matrix leader_desired_inertia; + Eigen::Matrix leader_desired_stiffness; + Eigen::Matrix leader_desired_damping; + + Eigen::Isometry3d follower_pose; + Eigen::Matrix follower_velocity; + Eigen::Matrix follower_acceleration; + Eigen::Matrix follower_wrench; + Eigen::Matrix follower_natural_inertia; + + Eigen::Matrix follower_desired_inertia; + Eigen::Matrix follower_desired_stiffness; + Eigen::Matrix follower_desired_damping; +}; + +/// Container for output teleop data !!!IMPORTANT!!! All in the follower frame!!! +struct TeleopDataOutput +{ + Eigen::Isometry3d leader_desired_pose; + Eigen::Matrix leader_desired_velocity; + Eigen::Matrix leader_desired_acceleration; + Eigen::Matrix leader_desired_wrench; + + Eigen::Matrix leader_desired_inertia; + Eigen::Matrix leader_desired_stiffness; + Eigen::Matrix leader_desired_damping; + + Eigen::Isometry3d follower_desired_pose; + Eigen::Matrix follower_desired_velocity; + Eigen::Matrix follower_desired_acceleration; + Eigen::Matrix follower_desired_wrench; + + Eigen::Matrix follower_desired_inertia; + Eigen::Matrix follower_desired_stiffness; + Eigen::Matrix follower_desired_damping; +}; + +/// Default copy from input to output +void set_default_safe_behavior( + const TeleopDataInput & input_data, TeleopDataOutput & output_data); + +bool to_msg( + const TeleopDataInput & input_data, + const TeleopDataOutput & output_data, + const std::vector & diagnostic_keys, + const std::vector & diagnostic_values, + cartesian_control_msgs::msg::TeleopControllerState & msg); + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_DATA_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_rule.hpp b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_rule.hpp new file mode 100644 index 0000000..760868d --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/teleop_rule.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_RULE_HPP_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_RULE_HPP_ + +#include +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/time.hpp" + +#include "cartesian_vic_teleop_controller/teleop_data.hpp" + +namespace cartesian_vic_teleop_controller +{ + +class TeleopRule +{ +public: + TeleopRule(); + ~TeleopRule() = default; + + /// Initialize the rule + virtual bool init( + std::shared_ptr parameters_interface, + const std::string & param_namespace = "teleoperation") = 0; + + /// Update the rule to compute the teleoperation commands and update the state msg + virtual bool update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + const TeleopDataInput & teleop_data_input, + TeleopDataOutput & teleop_data_output) = 0; + + /// Retrieve the TeleopControllerState msg. Call update() first!!! + bool get_current_state_msg( + const TeleopDataInput & teleop_data_input, + const TeleopDataOutput & teleop_data_output, + cartesian_control_msgs::msg::TeleopControllerState & msg) const; + +protected: + /// Clear the diagnostic_data field of the TeleopControllerState msg + bool clear_diagnostic_data(); + + /// Add a new diagnostic value to the diagnostic_data field of the TeleopControllerState msg + bool add_diagnostic_value(std::string key, double value); + + /** The diagnostic_data that will be added to the diagnostic_data + * field of the TeleopControllerState msg + */ + std::vector diagnostic_keys_; + std::vector diagnostic_values_; + + /// ROS2 local logger + rclcpp::Logger logger_; + + /// ROS2 local clock for throttle-type logging + rclcpp::Clock internal_clock_; + + /// True if init() has been called successfully. + bool is_initialized_; + + /// Teleop state msg that can be retrieved with get_current_state_msg() + cartesian_control_msgs::msg::TeleopControllerState msg_teleop_state_; +}; + +} // namespace cartesian_vic_teleop_controller + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__TELEOP_RULE_HPP_ diff --git a/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/visibility_control.h b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/visibility_control.h new file mode 100644 index 0000000..4989470 --- /dev/null +++ b/cartesian_vic_teleop_controller/include/cartesian_vic_teleop_controller/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2023 ICUBE Laboratory, University of Strasbourg +// +// 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. + +#ifndef CARTESIAN_VIC_TELEOP_CONTROLLER__VISIBILITY_CONTROL_H_ +#define CARTESIAN_VIC_TELEOP_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define CARTESIAN_VIC_TELEOP_CONTROLLER_EXPORT __attribute__ ((dllexport)) + #define CARTESIAN_VIC_TELEOP_CONTROLLER_IMPORT __attribute__ ((dllimport)) + #else + #define CARTESIAN_VIC_TELEOP_CONTROLLER_EXPORT __declspec(dllexport) + #define CARTESIAN_VIC_TELEOP_CONTROLLER_IMPORT __declspec(dllimport) + #endif + #ifdef CARTESIAN_VIC_TELEOP_CONTROLLER_BUILDING_LIBRARY + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC CARTESIAN_VIC_TELEOP_CONTROLLER_EXPORT + #else + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC CARTESIAN_VIC_TELEOP_CONTROLLER_IMPORT + #endif + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC_TYPE CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + #define CARTESIAN_VIC_TELEOP_CONTROLLER_LOCAL +#else + #define CARTESIAN_VIC_TELEOP_CONTROLLER_EXPORT __attribute__ ((visibility("default"))) + #define CARTESIAN_VIC_TELEOP_CONTROLLER_IMPORT + #if __GNUC__ >= 4 + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC __attribute__ ((visibility("default"))) + #define CARTESIAN_VIC_TELEOP_CONTROLLER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC + #define CARTESIAN_VIC_TELEOP_CONTROLLER_LOCAL + #endif + #define CARTESIAN_VIC_TELEOP_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // CARTESIAN_VIC_TELEOP_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/cartesian_vic_teleop_controller/package.xml b/cartesian_vic_teleop_controller/package.xml new file mode 100644 index 0000000..bbe7c24 --- /dev/null +++ b/cartesian_vic_teleop_controller/package.xml @@ -0,0 +1,41 @@ + + + + cartesian_vic_teleop_controller + 0.0.1 + TODO: Package description + tpoignonec + Apache License 2.0 + + ament_cmake + + cartesian_control_msgs + cartesian_vic_controller + + control_msgs + control_toolbox + controller_interface + filters + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_kdl + tf2_ros + trajectory_msgs + + ament_lint_auto + ament_cmake_gmock + controller_manager + kinematics_interface_kdl + hardware_interface_testing + ros2_control_test_assets + + + ament_cmake + + diff --git a/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_controller.cpp b/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_controller.cpp new file mode 100644 index 0000000..fc60bed --- /dev/null +++ b/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_controller.cpp @@ -0,0 +1,385 @@ +// Copyright 2023 ICUBE Laboratory, University of Strasbourg +// +// 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 "cartesian_vic_teleop_controller/cartesian_vic_teleop_controller.hpp" + +#include "rclcpp/logging.hpp" +#include "rcutils/logging_macros.h" + +namespace cartesian_vic_teleop_controller +{ + +CartesianVicTeleopController::CartesianVicTeleopController() +: Base::CartesianVicController() +{ + // TODO(tpoignonec) : check if this is needed + // Actually, same for most overridden methods... +} + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_init() +{ + auto ret = Base::on_init(); + return ret; +} + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + auto ret = Base::on_configure(previous_state); + if (ret != controller_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Parent Base::on_configure() failed!"); + return ret; + } + // Unsubscribe the existing subscriber (declare by base and only used in update) + input_compliant_frame_trajectory_subscriber_.reset(); + + // setup new subscribers and publishers used for VIC-based teleoperation + auto follower_vic_state_callback = + [this](const std::shared_ptr msg) + {input_follower_vic_state_msg_.writeFromNonRT(msg);}; + follower_vic_state_subscriber_ = + get_node()->create_subscription( + "/vic_controller_follower/status", + rclcpp::SystemDefaultsQoS(), + follower_vic_state_callback); + + auto teleop_compliance_callback = + [this](const std::shared_ptr msg) + {input_teleop_compliance_msg_.writeFromNonRT(msg);}; + teleop_compliance_subscriber_ = + get_node()->create_subscription( + "~/teleoperation_compliance_reference", + rclcpp::SystemDefaultsQoS(), + teleop_compliance_callback); + + auto is_clutched_callback = + [this](const std::shared_ptr msg) + {input_is_clutched_msg_.writeFromNonRT(msg);}; + is_clutched_subscriber_ = + get_node()->create_subscription( + "fd_clutch", + rclcpp::SystemDefaultsQoS(), + is_clutched_callback); + + // Follower VIC ref publisher + non_rt_follower_vic_ref_publisher_ = \ + get_node()->create_publisher( + "/vic_controller_follower/reference_compliant_frame_trajectory", + rclcpp::SystemDefaultsQoS()); + follower_vic_ref_publisher_ = + std::make_unique>(non_rt_follower_vic_ref_publisher_); + + // Teleop state publisher + non_rt_teleop_state_publisher_ = \ + get_node()->create_publisher( + "~/teleop_controller_state", + rclcpp::SystemDefaultsQoS()); + teleop_state_publisher_ = + std::make_unique>(non_rt_teleop_state_publisher_); + + return ret; +} + +controller_interface::return_type +CartesianVicTeleopController::update(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + (void) time; + + auto execute_fallback_policy = [this] () + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Fallback policy triggered! Setting velocity / force controls to zero!"); + joint_command_ = last_commanded_joint_state_; + std::fill( + joint_command_.velocities.begin(), joint_command_.velocities.end(), 0.0); + std::fill( + joint_command_.accelerations.begin(), joint_command_.accelerations.end(), 0.0); + std::fill( + joint_command_.effort.begin(), joint_command_.effort.end(), 0.0); + write_state_to_hardware(joint_command_); + }; + + // Realtime constraints are required in this function + if (!vic_) { + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + // get all controller inputs + bool is_state_valid = read_state_from_hardware(measurement_data_); + + // update compliant frame(s) reference from ros subscriber message + follower_vic_state_msg_ptr_ = \ + *input_follower_vic_state_msg_.readFromRT(); + if (!follower_vic_state_msg_ptr_.get()) { + auto clock = get_node()->get_clock(); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *clock, + 5000, + "Waiting for follower VicStateMsg..."); + // Exit and wait for valid data... + is_state_valid = false; + // execute_fallback_policy(); + return controller_interface::return_type::OK; + } + + // update compliance reference from ros subscriber message + teleop_compliance_msg_ptr_ = \ + *input_teleop_compliance_msg_.readFromRT(); + if (!teleop_compliance_msg_ptr_.get()) { + auto clock = get_node()->get_clock(); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *clock, + 1000, + "WARNING: No compliance reference msg received..."); + is_state_valid = false; + // execute_fallback_policy(); + return controller_interface::return_type::OK; + } + + // retrieve clutch state + bool workspace_clutch_disengaged = true; + is_clutched_msg_ptr_ = \ + *input_is_clutched_msg_.readFromRT(); + if (is_clutched_msg_ptr_.get()) { + // Only engage workspace if clutched + workspace_clutch_disengaged = !is_clutched_msg_ptr_->data; + } else { + auto clock = get_node()->get_clock(); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *clock, + 1000, + "Could not retrieve clutched state from topic, assuming false in the mean time..."); + } + // TODO(tpoignonec): add subscriber to get clutch state + + // make sure impedance was initialized + bool all_ok = true; + if (!is_vic_initialized_ && !is_state_valid) { + // Exit and wait for valid data... + return controller_interface::return_type::OK; + } else if (!is_vic_initialized_ && is_state_valid) { + // Init current desired pose from current joint position + if (!initialize_vic_rule(measurement_data_)) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to initialize the VIC rule!"); + return controller_interface::return_type::ERROR; + } + } else if (!is_state_valid) { + // TODO(tpoignonec): return ERROR? + all_ok &= false; + joint_command_ = last_commanded_joint_state_; + RCLCPP_ERROR( + get_node()->get_logger(), + "Invalid data from hardware interface!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + // Control logic + if (all_ok) { + // Check follower VicStateMsg availability + // TODO(tpoignonec) : check timestamps also! + if (!follower_vic_state_msg_ptr_.get()) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to retrieve follower VicStateMsg!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } else { + follower_vic_state_msg_ = *follower_vic_state_msg_ptr_.get(); + } + + // Update vic input data + RCLCPP_DEBUG(get_node()->get_logger(), "vic_->update_input_data()"); + auto ret_vic = vic_->update_input_data(period, measurement_data_); + if (ret_vic != controller_interface::return_type::OK) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to update VIC rule input data!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + // Update teleop logic + if (!teleop_logic_.is_initialized()) { + std::string teleop_parameters_namespace = "teleoperation"; + // Init teleop logic if not yet OK + RCLCPP_INFO( + get_node()->get_logger(), + "Initializing the teleop logic..."); + try { + if (!teleop_logic_.init( + time, + get_node()->get_node_parameters_interface(), + vic_->get_input_data(), + follower_vic_state_msg_, + teleop_parameters_namespace)) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to initialize the teleop logic!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during update stage with message: %s \n", + e.what()); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + } + + RCLCPP_DEBUG(get_node()->get_logger(), "teleop_logic_.setTeleoperationCompliance()"); + if (!teleop_logic_.setTeleoperationCompliance( + *teleop_compliance_msg_ptr_.get())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to set the teleop compliance!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + RCLCPP_DEBUG(get_node()->get_logger(), "teleop_logic_.update()"); + if (!teleop_logic_.update( + time, period, workspace_clutch_disengaged, vic_->get_input_data(), follower_vic_state_msg_)) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to update the teleop logic!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + RCLCPP_DEBUG(get_node()->get_logger(), "teleop_logic_.get_*_vic_ref()"); + all_ok &= teleop_logic_.get_leader_vic_ref(leader_vic_ref_); + all_ok &= teleop_logic_.get_follower_vic_ref(follower_vic_ref_); + + if (all_ok) { + RCLCPP_DEBUG(get_node()->get_logger(), "vic_->update_compliant_frame_trajectory()"); + // Update leader ref + vic_->update_compliant_frame_trajectory(leader_vic_ref_); + // Publish follower ref + follower_vic_ref_publisher_->lock(); + follower_vic_ref_publisher_->msg_ = follower_vic_ref_; + follower_vic_ref_publisher_->unlockAndPublish(); + } else { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to retrieve compliant frame trajectories!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + // Get leader VIC commands + RCLCPP_DEBUG(get_node()->get_logger(), "vic_->compute_controls()"); + ret_vic = vic_->compute_controls(period, joint_command_); + if (ret_vic != controller_interface::return_type::OK) { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to compute VIC rule commands!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } + + if (ret_vic != controller_interface::return_type::OK) { + return controller_interface::return_type::ERROR; + } + } + + // write calculated values to joint interfaces + write_state_to_hardware(joint_command_); + + + // Publish teleop controller state + if (!teleop_logic_.get_current_state_msg(teleop_state_msg_)) { + // NOT a critical error, but we want to know if it happens + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to retrieve teleop rule state as a ros2 msg!"); + } else { + teleop_state_msg_.header.stamp = get_node()->get_clock()->now(); + teleop_state_publisher_->lock(); + teleop_state_publisher_->msg_ = teleop_state_msg_; + teleop_state_publisher_->unlockAndPublish(); + } + + // Publish controller state + if (vic_->controller_state_to_msg(controller_state_msg_) != \ + controller_interface::return_type::OK) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to retrieve VIC rule state!"); + execute_fallback_policy(); + return controller_interface::return_type::ERROR; + } else { + controller_state_msg_.header.stamp = get_node()->get_clock()->now(); + state_publisher_->lock(); + state_publisher_->msg_ = controller_state_msg_; + state_publisher_->unlockAndPublish(); + } + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + auto ret = Base::on_activate(previous_state); + return ret; +} + + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + auto ret = Base::on_deactivate(previous_state); + return ret; +} + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_cleanup(const rclcpp_lifecycle::State & previous_state) +{ + auto ret = Base::on_cleanup(previous_state); + return ret; +} + + +controller_interface::CallbackReturn +CartesianVicTeleopController::on_error(const rclcpp_lifecycle::State & previous_state) +{ + auto ret = Base::on_error(previous_state); + return ret; +} + +} // namespace cartesian_vic_teleop_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_vic_teleop_controller::CartesianVicTeleopController, + controller_interface::ControllerInterface +) diff --git a/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_logic.cpp b/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_logic.cpp new file mode 100644 index 0000000..5131c5b --- /dev/null +++ b/cartesian_vic_teleop_controller/src/cartesian_vic_teleop_logic.cpp @@ -0,0 +1,575 @@ +// Copyright 2023 ICUBE Laboratory, University of Strasbourg +// +// 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 "cartesian_vic_teleop_controller/cartesian_vic_teleop_logic.hpp" + +#include "rclcpp/logging.hpp" +#include "rcutils/logging_macros.h" + +#include "cartesian_vic_controller/utils.hpp" +#include "cartesian_vic_controller/vic_msgs_utils.hpp" + +namespace cartesian_vic_teleop_controller +{ + +using cartesian_vic_controller::VicInputData; +using cartesian_vic_controller::matrixEigenToMsg; +using cartesian_vic_controller::AccelToMsg; +using cartesian_vic_controller::WrenchToMsg; + + +PassiveVicTeleopLogic::PassiveVicTeleopLogic() +: is_initialized_(false), + has_update_been_called_(false), + logger_(rclcpp::get_logger("cartesian_vic_teleop_logic")) +{ + // anything? +} + +// Internal logic +// --------------------- + +bool PassiveVicTeleopLogic::internal_init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const std::string & param_namespace) +{ + parameters_interface_ = parameters_interface; + is_initialized_ = false; + has_update_been_called_ = false; + + RCLCPP_INFO(logger_, "init(): initializing the mapping manager..."); + Eigen::Matrix3d master_base_to_follower_base_axis_mapping = \ + Eigen::Matrix3d::Identity(); + // Axis mapping is chosen so as to align the follower motion with the leader motion + master_base_to_follower_base_axis_mapping = \ + Eigen::AngleAxis(static_cast(EIGEN_PI), Eigen::Vector3d::UnitZ()); + + // Init the mapping manager + bool all_ok = mapping_manager_.init(master_base_to_follower_base_axis_mapping); + all_ok &= mapping_manager_.reset_mapping( + follower_pose_in_follower_base_frame_, + leader_pose_in_leader_base_frame_); + all_ok &= mapping_manager_.update(leader_pose_in_leader_base_frame_); + + if (!all_ok) { + RCLCPP_ERROR(logger_, "init(): Failed to initialize the mapping manager!"); + return false; + } + + // Init the VIC references + RCLCPP_INFO(logger_, "init(): initializing the data structures..."); + leader_vic_ref_ = CompliantFrameTrajectoryMsg(); + leader_vic_ref_.header.stamp = time; + auto leader_cartesian_traj_point = cartesian_control_msgs::msg::CartesianTrajectoryPoint(); + auto leader_compliance = cartesian_control_msgs::msg::CartesianCompliance(); + leader_cartesian_traj_point.time_from_start = rclcpp::Duration::from_seconds(0.0); + leader_cartesian_traj_point.pose = Eigen::toMsg(leader_pose_in_leader_base_frame_); + leader_vic_ref_.cartesian_trajectory_points.push_back(leader_cartesian_traj_point); + leader_vic_ref_.compliance_at_points.push_back(leader_compliance); + + follower_vic_ref_ = CompliantFrameTrajectoryMsg(); + follower_vic_ref_.header.stamp = time; + auto follower_cartesian_traj_point = cartesian_control_msgs::msg::CartesianTrajectoryPoint(); + auto follower_compliance = cartesian_control_msgs::msg::CartesianCompliance(); + follower_cartesian_traj_point.time_from_start = rclcpp::Duration::from_seconds(0.0); + follower_cartesian_traj_point.pose = Eigen::toMsg(follower_pose_in_follower_base_frame_); + follower_vic_ref_.cartesian_trajectory_points.push_back(follower_cartesian_traj_point); + follower_vic_ref_.compliance_at_points.push_back(follower_compliance); + + // Reset the teleoperation input data + if(!mapping_manager_.map_master_tool_to_follower_tool( + leader_pose_in_leader_base_frame_, teleop_data_input_.leader_pose)) + { + RCLCPP_ERROR(logger_, "init(): Failed to map the leader pose!"); + all_ok = false; + } + teleop_data_input_.leader_velocity.setZero(); + teleop_data_input_.leader_acceleration.setZero(); + teleop_data_input_.leader_wrench.setZero(); + teleop_data_input_.leader_desired_inertia = leader_natural_inertia_in_leader_base_frame_; + teleop_data_input_.leader_desired_stiffness = 50. * Eigen::Matrix::Identity(); + teleop_data_input_.leader_desired_damping = 10. * Eigen::Matrix::Identity(); + + teleop_data_input_.follower_pose = follower_pose_in_follower_base_frame_; + teleop_data_input_.follower_velocity.setZero(); + teleop_data_input_.follower_acceleration.setZero(); + teleop_data_input_.follower_wrench.setZero(); + teleop_data_input_.follower_desired_inertia = follower_natural_inertia_in_follower_base_frame_; + teleop_data_input_.follower_desired_stiffness = 50. * Eigen::Matrix::Identity(); + teleop_data_input_.follower_desired_damping = 10 * Eigen::Matrix::Identity(); + + // Reset the teleoperation output data + set_default_safe_behavior(teleop_data_input_, teleop_data_output_); + + // Reset cache + leader_velocity_in_leader_base_frame_.setZero(); + leader_acceleration_in_leader_base_frame_.setZero(); + leader_wrench_in_leader_base_frame_.setZero(); + + follower_velocity_in_follower_base_frame_.setZero(); + follower_acceleration_in_follower_base_frame_.setZero(); + follower_wrench_in_follower_base_frame_.setZero(); + + leader_reference_pose_in_leader_base_frame_ = leader_pose_in_leader_base_frame_; + leader_reference_velocity_in_leader_base_frame_.setZero(); + leader_reference_acceleration_in_leader_base_frame_.setZero(); + leader_reference_wrench_in_leader_base_frame_.setZero(); + leader_reference_inertia_in_leader_base_frame_ = teleop_data_output_.leader_desired_inertia; + leader_reference_stiffness_in_leader_base_frame_ = teleop_data_output_.leader_desired_stiffness; + leader_reference_damping_in_leader_base_frame_ = teleop_data_output_.leader_desired_damping; + + // Initialize the teleoperation rule plugin + RCLCPP_INFO(logger_, "init(): initializing the rule plugin..."); + + auto rule_plugin_name_param_name = param_namespace + ".rule_plugin_name"; + auto rule_plugin_package_param_name = param_namespace + ".rule_plugin_package"; + + #if CARTESIAN_VIC_TELEOP_CONTROLLER_JAZZY || CARTESIAN_VIC_TELEOP_CONTROLLER_ROLLING + // Does not work seem to work humble + rcl_interfaces::msg::ParameterDescriptor rule_plugin_param_desc; + rule_plugin_param_desc.name = "rule_plugin_name"; + rule_plugin_param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + rule_plugin_param_desc.description = "Name of the teleoperation rule plugin"; + + rcl_interfaces::msg::ParameterDescriptor rule_plugin_package_param_desc; + rule_plugin_package_param_desc.name = "rule_plugin_package"; + rule_plugin_package_param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + rule_plugin_package_param_desc.description = "Package of the teleoperation rule plugin"; + + parameters_interface->declare_parameter( + rule_plugin_name_param_name, rclcpp::ParameterValue(""), rule_plugin_param_desc); + parameters_interface->declare_parameter( + rule_plugin_package_param_name, rclcpp::ParameterValue(""), rule_plugin_package_param_desc); + #endif + + auto rule_plugin_name_param = rclcpp::Parameter(); + auto rule_plugin_package_param = rclcpp::Parameter(); + if (!parameters_interface->get_parameter( + rule_plugin_name_param_name, rule_plugin_name_param)) + { + RCLCPP_ERROR(logger_, "parameter %s not set", rule_plugin_name_param_name.c_str()); + return false; + } + if (!parameters_interface->get_parameter( + rule_plugin_package_param_name, rule_plugin_package_param)) + { + RCLCPP_ERROR(logger_, "parameter %s not set", rule_plugin_package_param_name.c_str()); + return false; + } + + std::string rule_plugin_name = rule_plugin_name_param.as_string(); + std::string rule_plugin_package = rule_plugin_package_param.as_string(); + + RCLCPP_INFO( + logger_, "init(): loading rule plugin '%s' from pkg '%s'...", + rule_plugin_name.c_str(), rule_plugin_package.c_str()); + + try { + if (!rule_plugin_name.empty() && !rule_plugin_package.empty()) { + teleop_rule_loader_ = + std::make_shared>( + rule_plugin_package, + "cartesian_vic_teleop_controller::TeleopRule" + ); + teleop_rule_ = std::unique_ptr( + teleop_rule_loader_->createUnmanagedInstance(rule_plugin_name)); + } else { + RCLCPP_FATAL( + logger_, + "Please provide 'rule_plugin_package' and 'rule_plugin_name' parameters!"); + return false; + } + // Initialize teleop rule plugin + is_teleop_rule_initialized_ = teleop_rule_->init(parameters_interface_, param_namespace); + if (!is_teleop_rule_initialized_) { + RCLCPP_ERROR(logger_, "Failed to initialize the teleop rule plugin!"); + all_ok = false; + } + } catch (const std::exception & e) { + RCLCPP_ERROR(logger_, "Exception thrown during initialization with message: %s \n", + e.what()); + return false; + } + + // Set the initialized flag + is_initialized_ = all_ok; + RCLCPP_INFO(logger_, "init(): all done."); + return is_initialized_; +} + +bool PassiveVicTeleopLogic::internal_update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged) +{ + if (!teleop_rule_) { + RCLCPP_ERROR(logger_, "update(): the teleop rule plugin is not loaded!"); + return false; + } + + if (!is_teleop_rule_initialized_) { + RCLCPP_ERROR(logger_, "update(): the teleop rule plugin is not initialized!"); + return false; + } + + if (!mapping_manager_.is_initialized()) { + RCLCPP_ERROR(logger_, "update(): Mapping manager is not initialized!"); + return false; + } + + if (!mapping_manager_.update( + leader_pose_in_leader_base_frame_, + workspace_clutch_disengaged)) + { + RCLCPP_ERROR(logger_, "update(): Failed to update the mapping manager!"); + return false; + } + + // Copy the follower data + teleop_data_input_.follower_pose = follower_pose_in_follower_base_frame_; + teleop_data_input_.follower_velocity = follower_velocity_in_follower_base_frame_; + teleop_data_input_.follower_acceleration = follower_acceleration_in_follower_base_frame_; + teleop_data_input_.follower_wrench = follower_wrench_in_follower_base_frame_; + teleop_data_input_.follower_natural_inertia = follower_natural_inertia_in_follower_base_frame_; + + // Map the leader data into the follower frame + bool all_ok = mapping_manager_.map_master_tool_to_follower_tool( + leader_pose_in_leader_base_frame_, teleop_data_input_.leader_pose); + all_ok &= mapping_manager_.map_twist_or_wrench_from_master_to_follower( + leader_velocity_in_leader_base_frame_, teleop_data_input_.leader_velocity); + all_ok &= mapping_manager_.map_twist_or_wrench_from_master_to_follower( + leader_acceleration_in_leader_base_frame_, teleop_data_input_.leader_acceleration); + all_ok &= mapping_manager_.map_twist_or_wrench_from_master_to_follower( + leader_wrench_in_leader_base_frame_, teleop_data_input_.leader_wrench); + all_ok &= mapping_manager_.map_SDPD_from_master_to_follower( + leader_natural_inertia_in_leader_base_frame_, teleop_data_input_.leader_natural_inertia); + + if (!all_ok) { + RCLCPP_ERROR(logger_, "update(): Failed to map leader data into follower frame!"); + return false; + } + + // Set the workspace clutch flag + teleop_data_input_.workspace_is_engaged = ( + mapping_manager_.get_clutching_state() == MappingManager::CLUTCHING_STATE::ENGAGED); + + // Compute the reference trajectory using teleop rule plugin + all_ok &= teleop_rule_->update(time, period, teleop_data_input_, teleop_data_output_); + if (!all_ok) { + RCLCPP_ERROR(logger_, "update(): Failed to compute reference VIC profiles!"); + return false; + } + + // Disengage the follower motion forcefully if the workspace is disengaged + if (!teleop_data_input_.workspace_is_engaged) { + // Disengage the follower when needed + teleop_data_output_.follower_desired_velocity.setZero(); + teleop_data_output_.follower_desired_acceleration.setZero(); + teleop_data_output_.follower_desired_wrench.setZero(); + // TODO(anyone): Is this right? Doesn't seem so... + } + + // Map the leader reference trajectory into the leader frame + all_ok &= mapping_manager_.inverse_map_follower_tool_to_master_tool( + teleop_data_output_.leader_desired_pose, leader_reference_pose_in_leader_base_frame_); + all_ok &= mapping_manager_.map_twist_or_wrench_from_follower_to_master( + teleop_data_output_.leader_desired_velocity, leader_reference_velocity_in_leader_base_frame_); + all_ok &= mapping_manager_.map_twist_or_wrench_from_follower_to_master( + teleop_data_output_.leader_desired_acceleration, + leader_reference_acceleration_in_leader_base_frame_); + all_ok &= mapping_manager_.map_twist_or_wrench_from_follower_to_master( + teleop_data_output_.leader_desired_wrench, + leader_reference_wrench_in_leader_base_frame_); + all_ok &= mapping_manager_.map_SDPD_from_follower_to_master( + teleop_data_output_.leader_desired_inertia, + leader_reference_inertia_in_leader_base_frame_); + all_ok &= mapping_manager_.map_SDPD_from_follower_to_master( + teleop_data_output_.leader_desired_stiffness, + leader_reference_stiffness_in_leader_base_frame_); + all_ok &= mapping_manager_.map_SDPD_from_follower_to_master( + teleop_data_output_.leader_desired_damping, + leader_reference_damping_in_leader_base_frame_); + + if (!all_ok) { + RCLCPP_ERROR(logger_, "update(): Failed to map leader reference trajectory into leader frame!"); + return false; + } + + // Fill the leader VIC ref trajectory msg + leader_vic_ref_.header.stamp = time; + auto & leader_cartesian_traj_point = leader_vic_ref_.cartesian_trajectory_points[0]; + auto & leader_compliance = leader_vic_ref_.compliance_at_points[0]; + + leader_cartesian_traj_point.time_from_start = rclcpp::Duration::from_seconds(0.0); + leader_cartesian_traj_point.pose = Eigen::toMsg(leader_reference_pose_in_leader_base_frame_); + leader_cartesian_traj_point.velocity = \ + Eigen::toMsg(leader_reference_velocity_in_leader_base_frame_); + leader_cartesian_traj_point.acceleration = \ + AccelToMsg(leader_reference_acceleration_in_leader_base_frame_); + leader_cartesian_traj_point.wrench = \ + WrenchToMsg(leader_reference_wrench_in_leader_base_frame_); + matrixEigenToMsg( + leader_reference_inertia_in_leader_base_frame_, leader_compliance.inertia); + matrixEigenToMsg( + leader_reference_stiffness_in_leader_base_frame_, leader_compliance.stiffness); + matrixEigenToMsg( + leader_reference_damping_in_leader_base_frame_, leader_compliance.damping); + + // Fill the follower VIC ref trajectory msg + follower_vic_ref_.header.stamp = time; + auto & follower_cartesian_traj_point = follower_vic_ref_.cartesian_trajectory_points[0]; + auto & follower_compliance = follower_vic_ref_.compliance_at_points[0]; + follower_cartesian_traj_point.time_from_start = rclcpp::Duration::from_seconds(0.0); + follower_cartesian_traj_point.pose = Eigen::toMsg(teleop_data_output_.follower_desired_pose); + follower_cartesian_traj_point.velocity = \ + Eigen::toMsg(teleop_data_output_.follower_desired_velocity); + follower_cartesian_traj_point.acceleration = \ + AccelToMsg(teleop_data_output_.follower_desired_acceleration); + follower_cartesian_traj_point.wrench = \ + WrenchToMsg(teleop_data_output_.follower_desired_wrench); + matrixEigenToMsg( + teleop_data_output_.follower_desired_inertia, follower_compliance.inertia); + matrixEigenToMsg( + teleop_data_output_.follower_desired_stiffness, follower_compliance.stiffness); + matrixEigenToMsg( + teleop_data_output_.follower_desired_damping, follower_compliance.damping); + + has_update_been_called_ = all_ok; + return all_ok; +} + +// Convenience functions +// --------------------- +bool PassiveVicTeleopLogic::init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg, + const std::string & param_namespace) +{ + RCLCPP_DEBUG(logger_, "Initializing the teleop logic..."); + + bool success = extract_input_data(leader_vic_state_msg, follower_vic_state_msg); + if (!success) { + RCLCPP_ERROR(logger_, "init(): Failed to extract data from VicStateMsg!"); + return false; + } + RCLCPP_DEBUG(logger_, "Call to internal_init()..."); + return internal_init(time, parameters_interface, param_namespace); +} + +bool PassiveVicTeleopLogic::init( + const rclcpp::Time & time, + std::shared_ptr parameters_interface, + const VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg, + const std::string & param_namespace) +{ + RCLCPP_DEBUG(logger_, "Initializing the teleop logic..."); + + bool success = extract_input_data(leader_input_data, follower_vic_state_msg); + if (!success) { + RCLCPP_ERROR( + logger_, "init(): Failed to extract data from VicInputData and VicStateMsg!"); + return false; + } + RCLCPP_DEBUG(logger_, "Call to internal_init()..."); + return internal_init(time, parameters_interface, param_namespace); +} + +bool PassiveVicTeleopLogic::update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged, + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg) +{ + RCLCPP_DEBUG(logger_, "Update()"); + bool success = extract_input_data(leader_vic_state_msg, follower_vic_state_msg); + if (!success) { + RCLCPP_ERROR(logger_, "update(): Failed to extract data from VicStateMsg!"); + return false; + } + + return internal_update(time, period, workspace_clutch_disengaged); +} + +bool PassiveVicTeleopLogic::update( + const rclcpp::Time & time, + const rclcpp::Duration & period, + bool workspace_clutch_disengaged, + const VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg) +{ + RCLCPP_DEBUG(logger_, "Update()"); + bool success = extract_input_data(leader_input_data, follower_vic_state_msg); + if (!success) { + RCLCPP_ERROR( + logger_, "init(): Failed to extract data from VicInputData and VicStateMsg!"); + return false; + } + return internal_update(time, period, workspace_clutch_disengaged); +} + +bool PassiveVicTeleopLogic::setTeleoperationCompliance( + const cartesian_control_msgs::msg::TeleopCompliance & msg) +{ + bool all_ok = true; + all_ok &= cartesian_vic_controller::fromMsg( + msg.leader_desired_inertia, teleop_data_input_.leader_desired_inertia); + all_ok &= cartesian_vic_controller::fromMsg( + msg.leader_desired_stiffness, teleop_data_input_.leader_desired_stiffness); + all_ok &= cartesian_vic_controller::fromMsg( + msg.leader_desired_damping, teleop_data_input_.leader_desired_damping); + + all_ok &= cartesian_vic_controller::fromMsg( + msg.follower_desired_inertia, teleop_data_input_.follower_desired_inertia); + all_ok &= cartesian_vic_controller::fromMsg( + msg.follower_desired_stiffness, teleop_data_input_.follower_desired_stiffness); + all_ok &= cartesian_vic_controller::fromMsg( + msg.follower_desired_damping, teleop_data_input_.follower_desired_damping); + + return all_ok; +} + + +bool +PassiveVicTeleopLogic::get_current_state_msg( + cartesian_control_msgs::msg::TeleopControllerState & msg) +{ + if (!is_initialized_ || !has_update_been_called_) { + RCLCPP_ERROR(logger_, "get_current_state_msg(): not yet initialized and/or updated!"); + return false; + } + return teleop_rule_->get_current_state_msg(teleop_data_input_, teleop_data_output_, msg); +} + +// Utils +// --------------------- + +bool PassiveVicTeleopLogic::extract_input_data( + const VicStateMsg & leader_vic_state_msg, + const VicStateMsg & follower_vic_state_msg) +{ + if (!extract_follower_input_data(follower_vic_state_msg)) { + RCLCPP_ERROR(logger_, "extract_input_data(): failed to extract follower data!"); + return false; + } + bool all_ok = true; + tf2::fromMsg(leader_vic_state_msg.pose, leader_pose_in_leader_base_frame_); + all_ok &= cartesian_vic_controller::get_robot_velocity( + leader_vic_state_msg, leader_velocity_in_leader_base_frame_); + all_ok &= cartesian_vic_controller::get_robot_acceleration( + leader_vic_state_msg, leader_acceleration_in_leader_base_frame_); + + if (follower_vic_state_msg.has_valid_wrench) { + all_ok &= cartesian_vic_controller::get_robot_wrench( + leader_vic_state_msg, leader_wrench_in_leader_base_frame_); + } else { + leader_wrench_in_leader_base_frame_.setZero(); + all_ok = false; + RCLCPP_WARN_THROTTLE( + logger_, + internal_clock_, + 5000, + "No leader force / torque sensor available!" + ); + } + all_ok &= cartesian_vic_controller::get_natural_robot_inertia( + leader_vic_state_msg, leader_natural_inertia_in_leader_base_frame_); + return all_ok; +} + +bool PassiveVicTeleopLogic::extract_input_data( + const cartesian_vic_controller::VicInputData & leader_input_data, + const VicStateMsg & follower_vic_state_msg) +{ + if (!extract_follower_input_data(follower_vic_state_msg)) { + RCLCPP_ERROR(logger_, "extract_input_data(): failed to extract follower data!"); + return false; + } + bool all_ok = true; + leader_pose_in_leader_base_frame_ = leader_input_data.robot_current_pose; + leader_velocity_in_leader_base_frame_ = leader_input_data.robot_current_velocity; + leader_acceleration_in_leader_base_frame_ = leader_input_data.robot_estimated_acceleration; + if (leader_input_data.has_external_torque_sensor()) { + leader_wrench_in_leader_base_frame_ = leader_input_data.get_ft_sensor_wrench(); + } else { + leader_wrench_in_leader_base_frame_.setZero(); + RCLCPP_WARN_THROTTLE( + logger_, + internal_clock_, + 5000, + "No leader force / torque sensor available!" + ); + } + leader_natural_inertia_in_leader_base_frame_ = leader_input_data.natural_cartesian_inertia; + return all_ok; +} + +bool PassiveVicTeleopLogic::extract_follower_input_data( + const VicStateMsg & follower_vic_state_msg) +{ + bool all_ok = true; + tf2::fromMsg(follower_vic_state_msg.pose, follower_pose_in_follower_base_frame_); + all_ok &= cartesian_vic_controller::get_robot_velocity( + follower_vic_state_msg, follower_velocity_in_follower_base_frame_); + all_ok &= cartesian_vic_controller::get_robot_acceleration( + follower_vic_state_msg, follower_acceleration_in_follower_base_frame_); + if (follower_vic_state_msg.has_valid_wrench) { + all_ok &= cartesian_vic_controller::get_robot_wrench( + follower_vic_state_msg, follower_wrench_in_follower_base_frame_); + } else { + follower_wrench_in_follower_base_frame_.setZero(); + all_ok = false; + RCLCPP_WARN_THROTTLE( + logger_, + internal_clock_, + 5000, + "No follower force / torque sensor available!" + ); + } + if (!cartesian_vic_controller::get_natural_robot_inertia( + follower_vic_state_msg, follower_natural_inertia_in_follower_base_frame_)) + { + RCLCPP_ERROR(logger_, "Failed to extract follower inertia!"); + all_ok = false; + } + return all_ok; +} + +bool PassiveVicTeleopLogic::get_leader_vic_ref( + CompliantFrameTrajectoryMsg & leader_vic_ref_msg) +{ + if(!is_initialized_ || !has_update_been_called_) { + RCLCPP_ERROR(logger_, "get_leader_vic_ref(): not yet initialized and/or updated!"); + return false; + } + leader_vic_ref_msg = leader_vic_ref_; + return true; +} + +bool PassiveVicTeleopLogic::get_follower_vic_ref( + CompliantFrameTrajectoryMsg & follower_vic_ref_msg) +{ + if(!is_initialized_ || !has_update_been_called_) { + RCLCPP_ERROR(logger_, "get_follower_vic_ref(): not yet initialized and/or updated!"); + return false; + } + follower_vic_ref_msg = follower_vic_ref_; + return true; +} +} // namespace cartesian_vic_teleop_controller diff --git a/cartesian_vic_teleop_controller/src/mapping_manager.cpp b/cartesian_vic_teleop_controller/src/mapping_manager.cpp new file mode 100644 index 0000000..b152b84 --- /dev/null +++ b/cartesian_vic_teleop_controller/src/mapping_manager.cpp @@ -0,0 +1,263 @@ +// Copyright 2022, ICube Laboratory, University of Strasbourg +// +// 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. + + +//----------------------------------------------------------------------------- +/*!\file mapping_manager.cpp + * + * \author thibault Poignonec + * \date 2022/06/24 + * + */ +//----------------------------------------------------------------------------- + +#include "cartesian_vic_teleop_controller/mapping_manager.hpp" +#include +#include "rclcpp/logging.hpp" + +namespace cartesian_vic_teleop_controller +{ +// ----------------------------------------- +// Constructor/destructor +// ----------------------------------------- +MappingManager::MappingManager() +: logger_(rclcpp::get_logger("mapping_manager")) +{ + mapping_initialized_ = false; +} + +MappingManager::~MappingManager() {} + +// ----------------------------------------- +// Initialization +// ----------------------------------------- +bool MappingManager::init( + const Eigen::Matrix3d & master_base_to_follower_base_axis_mapping) +{ + // Retrieve mapping parameters + // TODO(tpoignonec): Dynamic parameter + master_base_to_follower_base_axis_mapping_ = master_base_to_follower_base_axis_mapping; + + // All ok + mapping_initialized_ = false; + workspaces_disengaged_ = false; + return true; +} + + +// ----------------------------------------- +// Update mapping +// ----------------------------------------- +bool MappingManager::update( + const Eigen::Isometry3d & master_pose_in_master_base, + bool clutch_is_on) +{ + // Security check(s) + bool all_ok = true; + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() before update()!"); + return false; + } + // Process data + if ((!workspaces_disengaged_ && !clutch_is_on) || !use_workspace_clutching_) { + // !use_workspace_clutching_ --> "always engaged" + // Save current mapping + map_master_tool_to_follower_tool( + master_pose_in_master_base, + last_master_pose_mapped_into_follower_base_); + } else if (!workspaces_disengaged_ && clutch_is_on) { + // Toggle clutch + workspaces_disengaged_ = true; + info_verbose("Clutch is on: you can now reposition the master robot."); + } else if (workspaces_disengaged_ && !clutch_is_on) { + // Toggle clutch + workspaces_disengaged_ = false; + info_verbose( + "Be carefult! Clutch is off, any master displacement will cause the robot to move."); + } else { + // Update mapping such that no follower side motion are generated + all_ok &= reset_mapping( + last_master_pose_mapped_into_follower_base_, + master_pose_in_master_base, + master_base_to_follower_base_axis_mapping_ + ); + } + return all_ok; +} + +bool MappingManager::reset_mapping( + const Eigen::Isometry3d & follower_pose_in_follower_base, + const Eigen::Isometry3d & master_pose_in_master_base) +{ + return reset_mapping( + follower_pose_in_follower_base, master_pose_in_master_base, + master_base_to_follower_base_axis_mapping_); +} + +bool MappingManager::reset_mapping( + const Eigen::Isometry3d & follower_pose_in_follower_base, + const Eigen::Isometry3d & master_pose_in_master_base, + const Eigen::Matrix3d & master_base_to_follower_base_axis_mapping) +{ + /* Compute 'master_base_to_follower_base_mapping' */ + // Apply chosen axis mapping + master_base_to_follower_base_mapping_.linear() = master_base_to_follower_base_axis_mapping; + + // Retrieve frames relative translation offset + master_base_to_follower_base_mapping_.translation() = + follower_pose_in_follower_base.translation() - + master_base_to_follower_base_axis_mapping * master_pose_in_master_base.translation(); + + /* Compute 'master_base_to_follower_base_mapping' (only a rotation) */ + follower_tool_to_master_tool_axis_mapping_ = Eigen::Isometry3d::Identity(); + follower_tool_to_master_tool_axis_mapping_.linear() = + master_pose_in_master_base.linear().transpose() * + master_base_to_follower_base_mapping_.linear().transpose() * + follower_pose_in_follower_base.linear(); + + // Check the resulting mapping is consistent + // TODO(tpoignonec): remove this (or make it optional) once tested + mapping_initialized_ = true; + map_master_tool_to_follower_tool( + master_pose_in_master_base, + last_master_pose_mapped_into_follower_base_); + if (!last_master_pose_mapped_into_follower_base_.isApprox(follower_pose_in_follower_base)) { + mapping_initialized_ = false; + RCLCPP_ERROR(logger_, "Mapping manager: The computed mapping is invalid!"); + return false; + } + return true; +} + +// ----------------------------------------- +// Direct/inverse mappings +// ----------------------------------------- +bool MappingManager::map_master_tool_to_follower_tool( + const Eigen::Isometry3d & master_pose_in_master_base, + Eigen::Isometry3d & mapped_master_pose) const +{ + // Security check(s) + bool all_ok = true; + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() and reset() before using mapping!"); + return false; + } + // Map master pose + mapped_master_pose = \ + master_base_to_follower_base_mapping_ * + master_pose_in_master_base * + follower_tool_to_master_tool_axis_mapping_; + return all_ok; +} + +bool MappingManager::inverse_map_follower_tool_to_master_tool( + const Eigen::Isometry3d & follower_pose_in_follower_base, + Eigen::Isometry3d & mapped_follower_pose) const +{ + // Security check(s) + bool all_ok = true; + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() and reset() before using mapping!"); + return false; + } + // Map master pose + mapped_follower_pose = + master_base_to_follower_base_mapping_.inverse() * + follower_pose_in_follower_base * + follower_tool_to_master_tool_axis_mapping_.inverse(); + return all_ok; +} + + +bool MappingManager::map_twist_or_wrench_from_master_to_follower( + const Eigen::Matrix & vector_in_master_base, + Eigen::Matrix & vector_in_follower_base) const +{ + // Security check(s) + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() and reset() before using mapping!"); + return false; + } + map_tensor( + master_base_to_follower_base_mapping_.linear(), vector_in_master_base, + vector_in_follower_base); + return true; +} + +bool MappingManager::map_twist_or_wrench_from_follower_to_master( + const Eigen::Matrix & vector_in_follower_base, + Eigen::Matrix & vector_in_master_base) const +{ + // Security check(s) + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() and reset() before using mapping!"); + return false; + } + map_tensor( + master_base_to_follower_base_mapping_.linear().transpose(), vector_in_follower_base, + vector_in_master_base); + return true; +} + +bool MappingManager::map_SDPD_from_master_to_follower( + const Eigen::Matrix & SDPD_in_master_base, + Eigen::Matrix & SDPD_in_follower_base) +{ + // Security check(s) + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() and reset() before using mapping!"); + return false; + } + tmp_6x6_matrix_.setZero(); + tmp_6x6_matrix_.block<3, 3>(0, 0) = master_base_to_follower_base_mapping_.linear(); + tmp_6x6_matrix_.block<3, 3>(3, 3) = master_base_to_follower_base_mapping_.linear(); + + SDPD_in_follower_base = tmp_6x6_matrix_ * SDPD_in_master_base * tmp_6x6_matrix_.transpose(); + + return true; +} + +bool MappingManager::map_SDPD_from_follower_to_master( + const Eigen::Matrix & SDPD_in_follower_base, + Eigen::Matrix & SDPD_in_master_base) +{ + // Security check(s) + if (!mapping_initialized_) { + RCLCPP_ERROR(logger_, "Mapping manager: Call init() before using mapping!"); + return false; + } + tmp_6x6_matrix_.setZero(); + tmp_6x6_matrix_.block<3, 3>(0, 0) = master_base_to_follower_base_mapping_.linear().transpose(); + tmp_6x6_matrix_.block<3, 3>(3, 3) = master_base_to_follower_base_mapping_.linear().transpose(); + + SDPD_in_master_base = tmp_6x6_matrix_ * SDPD_in_follower_base * tmp_6x6_matrix_.transpose(); + return true; +} + + +void MappingManager::map_tensor( + const Eigen::Matrix3d & orientation, + const Eigen::Matrix & in, + Eigen::Matrix & out) const +{ + out.head(3) = orientation * in.head(3); + out.tail(3) = orientation * in.tail(3); +} + +void MappingManager::info_verbose(const std::string & msg) const +{ + RCLCPP_INFO(logger_, "%s", msg.c_str()); +} + +} // namespace cartesian_vic_teleop_controller diff --git a/cartesian_vic_teleop_controller/src/rules/vanilla_teleop_rule.cpp b/cartesian_vic_teleop_controller/src/rules/vanilla_teleop_rule.cpp new file mode 100644 index 0000000..8650aa5 --- /dev/null +++ b/cartesian_vic_teleop_controller/src/rules/vanilla_teleop_rule.cpp @@ -0,0 +1,108 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#include +#include + +#include + +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/time.hpp" + +#include "cartesian_vic_teleop_controller/rules/vanilla_teleop_rule.hpp" + +namespace cartesian_vic_teleop_controller +{ + +VanillaTeleopRule::VanillaTeleopRule() +: TeleopRule() +{ + logger_ = rclcpp::get_logger("VanillaTeleopRule"); + // Nothing else to do, see init(). +} + +bool VanillaTeleopRule::init( + std::shared_ptr/* parameters_interface */, + const std::string & /* param_namespace */) +{ + is_initialized_ = true; + return true; +} + +bool VanillaTeleopRule::update( + const rclcpp::Time & /* time */, + const rclcpp::Duration & period, + const TeleopDataInput & teleop_data_input, + TeleopDataOutput & teleop_data_output) +{ + // Clear the diagnostic_data field of the TeleopControllerState msg + clear_diagnostic_data(); + + // Nominal behavior: + // Copy the leader and follower pose, velocity, acceleration, wrench + teleop_data_output.leader_desired_pose = teleop_data_input.follower_pose; + teleop_data_output.leader_desired_velocity = teleop_data_input.follower_velocity; + teleop_data_output.leader_desired_acceleration = teleop_data_input.follower_acceleration; + teleop_data_output.leader_desired_wrench = teleop_data_input.follower_wrench; + + teleop_data_output.leader_desired_inertia = teleop_data_input.leader_desired_inertia; + teleop_data_output.leader_desired_stiffness = teleop_data_input.leader_desired_stiffness; + teleop_data_output.leader_desired_damping = teleop_data_input.leader_desired_damping; + + teleop_data_output.follower_desired_pose = teleop_data_input.leader_pose; + teleop_data_output.follower_desired_velocity = teleop_data_input.leader_velocity; + teleop_data_output.follower_desired_acceleration = teleop_data_input.leader_acceleration; + teleop_data_output.follower_desired_wrench = teleop_data_input.leader_wrench; + + teleop_data_output.follower_desired_inertia = teleop_data_input.follower_desired_inertia; + teleop_data_output.follower_desired_stiffness = teleop_data_input.follower_desired_stiffness; + teleop_data_output.follower_desired_damping = teleop_data_input.follower_desired_damping; + + teleop_data_output.follower_desired_velocity.tail(3).setZero(); + teleop_data_output.follower_desired_acceleration.tail(3).setZero(); + + // Logging + double real_Ts = period.seconds(); + add_diagnostic_value("real_Ts", real_Ts); + + // Set the desired velocity to zero if the workspace is disengaged + if (!teleop_data_input.workspace_is_engaged) { + // At minima... + teleop_data_output.leader_desired_velocity.setZero(); + teleop_data_output.leader_desired_acceleration.setZero(); + teleop_data_output.leader_desired_wrench.setZero(); + teleop_data_output.follower_desired_velocity.setZero(); + teleop_data_output.follower_desired_acceleration.setZero(); + teleop_data_output.follower_desired_wrench.setZero(); + // TODO(anyone): Is this right? Doesn't seem so... + + // To increase comfort, set K = 0 and D ~ 0 + teleop_data_output.leader_desired_stiffness.setZero(); + teleop_data_output.leader_desired_damping = 1.0 * Eigen::Matrix::Identity(); + } + return true; +} + +} // namespace cartesian_vic_teleop_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + cartesian_vic_teleop_controller::VanillaTeleopRule, + cartesian_vic_teleop_controller::TeleopRule +) diff --git a/cartesian_vic_teleop_controller/src/teleop_data.cpp b/cartesian_vic_teleop_controller/src/teleop_data.cpp new file mode 100644 index 0000000..5570281 --- /dev/null +++ b/cartesian_vic_teleop_controller/src/teleop_data.cpp @@ -0,0 +1,114 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#include "cartesian_vic_teleop_controller/teleop_data.hpp" + +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_kdl/tf2_kdl.hpp" + +#include "cartesian_vic_controller/utils.hpp" + + +namespace cartesian_vic_teleop_controller +{ + +using cartesian_vic_controller::matrixEigenToMsg; +using cartesian_vic_controller::AccelToMsg; +using cartesian_vic_controller::WrenchToMsg; + + +void set_default_safe_behavior(const TeleopDataInput & input_data, TeleopDataOutput & output_data) +{ + output_data.leader_desired_pose = input_data.follower_pose; + output_data.leader_desired_velocity.setZero(); + output_data.leader_desired_acceleration.setZero(); + output_data.leader_desired_wrench.setZero(); + + output_data.leader_desired_inertia = input_data.leader_desired_inertia; + output_data.leader_desired_stiffness = input_data.leader_desired_stiffness; + output_data.leader_desired_damping = input_data.leader_desired_damping; + + output_data.follower_desired_pose = input_data.leader_pose; + output_data.follower_desired_velocity.setZero(); + output_data.follower_desired_acceleration.setZero(); + output_data.follower_desired_wrench.setZero(); + + output_data.follower_desired_inertia = input_data.follower_desired_inertia; + output_data.follower_desired_stiffness = input_data.follower_desired_stiffness; + output_data.follower_desired_damping = input_data.follower_desired_damping; +} + +bool to_msg( + const TeleopDataInput & input_data, + const TeleopDataOutput & output_data, + const std::vector & diagnostic_keys, + const std::vector & diagnostic_values, + cartesian_control_msgs::msg::TeleopControllerState & msg) +{ + // Fill input data + msg.workspace_is_engaged.data = input_data.workspace_is_engaged; + + msg.x_1 = Eigen::toMsg(input_data.leader_pose); + msg.x_1_dot = Eigen::toMsg(input_data.leader_velocity); + msg.x_1_ddot = AccelToMsg(input_data.leader_acceleration); + msg.f_1 = WrenchToMsg(input_data.leader_wrench); + + msg.x_2 = Eigen::toMsg(input_data.follower_pose); + msg.x_2_dot = Eigen::toMsg(input_data.follower_velocity); + msg.x_2_ddot = AccelToMsg(input_data.follower_acceleration); + msg.f_2 = WrenchToMsg(input_data.follower_wrench); + + matrixEigenToMsg(input_data.leader_desired_inertia, msg.desired_inertia_1); + matrixEigenToMsg(input_data.leader_desired_stiffness, msg.desired_stiffness_1); + matrixEigenToMsg(input_data.leader_desired_damping, msg.desired_damping_1); + matrixEigenToMsg(input_data.follower_desired_inertia, msg.desired_inertia_2); + matrixEigenToMsg(input_data.follower_desired_stiffness, msg.desired_stiffness_2); + matrixEigenToMsg(input_data.follower_desired_damping, msg.desired_damping_2); + + // Fill output data + msg.x_1_d = Eigen::toMsg(output_data.leader_desired_pose); + msg.x_1_dot_d = Eigen::toMsg(output_data.leader_desired_velocity); + msg.x_1_ddot_d = AccelToMsg(output_data.leader_desired_acceleration); + msg.f_1_d = WrenchToMsg(output_data.leader_desired_wrench); + + msg.x_2_d = Eigen::toMsg(output_data.follower_desired_pose); + msg.x_2_dot_d = Eigen::toMsg(output_data.follower_desired_velocity); + msg.x_2_ddot_d = AccelToMsg(output_data.follower_desired_acceleration); + msg.f_2_d = WrenchToMsg(output_data.follower_desired_wrench); + + matrixEigenToMsg(output_data.leader_desired_inertia, msg.rendered_inertia_1); + matrixEigenToMsg(output_data.leader_desired_stiffness, msg.rendered_stiffness_1); + matrixEigenToMsg(output_data.leader_desired_damping, msg.rendered_damping_1); + matrixEigenToMsg(output_data.follower_desired_inertia, msg.rendered_inertia_2); + matrixEigenToMsg(output_data.follower_desired_stiffness, msg.rendered_stiffness_2); + matrixEigenToMsg(output_data.follower_desired_damping, msg.rendered_damping_2); + + // Fill diagnostic data + if (diagnostic_keys.size() != diagnostic_values.size()) { + std::cerr << "Diagnostic keys and values have different sizes!" << std::endl; + return false; + } + msg.diagnostic_data.keys.clear(); + msg.diagnostic_data.values.clear(); + for (size_t idx = 0; idx < diagnostic_keys.size(); ++idx) { + msg.diagnostic_data.keys.push_back(diagnostic_keys[idx]); + msg.diagnostic_data.values.push_back(diagnostic_values[idx]); + } + + return true; +} + +} // namespace cartesian_vic_teleop_controller diff --git a/cartesian_vic_teleop_controller/src/teleop_rule.cpp b/cartesian_vic_teleop_controller/src/teleop_rule.cpp new file mode 100644 index 0000000..7024199 --- /dev/null +++ b/cartesian_vic_teleop_controller/src/teleop_rule.cpp @@ -0,0 +1,73 @@ +// Copyright 2024 ICUBE Laboratory, University of Strasbourg +// +// 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. +// +/// \authors: Thibault Poignonec + +#include "cartesian_vic_teleop_controller/teleop_rule.hpp" + +#include "rclcpp/logging.hpp" +#include "rcutils/logging_macros.h" + +#include "cartesian_vic_teleop_controller/teleop_data.hpp" + + +namespace cartesian_vic_teleop_controller +{ + +TeleopRule::TeleopRule() +: is_initialized_(false), + logger_(rclcpp::get_logger("TeleopRule")) +{ + // Nothing to do, see init() in specialized rules. +} + +bool TeleopRule::get_current_state_msg( + const TeleopDataInput & teleop_data_input, + const TeleopDataOutput & teleop_data_output, + cartesian_control_msgs::msg::TeleopControllerState & msg) const +{ + if (!is_initialized_) { + RCLCPP_ERROR( + logger_, "Failed to retrieve TeleopControllerState: TeleopRule is not initialized"); + return false; + } + return to_msg(teleop_data_input, teleop_data_output, diagnostic_keys_, diagnostic_values_, msg); +} + +bool TeleopRule::clear_diagnostic_data() +{ + diagnostic_keys_.clear(); + diagnostic_values_.clear(); + return true; +} + +bool TeleopRule::add_diagnostic_value(std::string key, double value) +{ + // Checking if the map already contains this specific key + if (std::find(diagnostic_keys_.begin(), diagnostic_keys_.end(), key) != diagnostic_keys_.end()) { + // the key already exists! + RCLCPP_ERROR( + logger_, + "Failed to add diagnostic value with key '%s': key already exists!", + key.c_str() + ); + return false; + } + + diagnostic_keys_.push_back(key); + diagnostic_values_.push_back(value); + return true; +} + +} // namespace cartesian_vic_teleop_controller