diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..9039d2e9 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,72 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h index dab0986f..83498b56 100644 --- a/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h +++ b/cartesian_compliance_controller/include/cartesian_compliance_controller/cartesian_compliance_controller.h @@ -112,6 +112,7 @@ class CartesianComplianceController : public cartesian_motion_controller::Cartes ctrl::Vector6D computeComplianceError(); ctrl::Matrix6D m_stiffness; + ctrl::Matrix6D m_damping; std::string m_compliance_ref_link; }; diff --git a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp index 6865e3fb..eecefa52 100644 --- a/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp +++ b/cartesian_compliance_controller/src/cartesian_compliance_controller.cpp @@ -189,12 +189,14 @@ ctrl::Vector6D CartesianComplianceController::computeComplianceError() tmp[5] = get_node()->get_parameter("stiffness.rot_z").as_double(); m_stiffness = tmp.asDiagonal(); - + m_damping = 2.0 * m_stiffness.cwiseSqrt(); + ctrl::Vector6D net_force = - // Spring force in base orientation Base::displayInBaseLink(m_stiffness, m_compliance_ref_link) * MotionBase::computeMotionError() - + // Damping force in base orientation + - Base::displayInBaseLink(m_damping, m_compliance_ref_link) * + Base::m_ik_solver->getEndEffectorVel() // Sensor and target force in base orientation + ForceBase::computeForceError(); diff --git a/end_effector_controller/include/end_effector_controller/end_effector_control.h b/end_effector_controller/include/end_effector_controller/end_effector_control.h index 862b225e..0501ffc1 100644 --- a/end_effector_controller/include/end_effector_controller/end_effector_control.h +++ b/end_effector_controller/include/end_effector_controller/end_effector_control.h @@ -40,14 +40,6 @@ #ifndef END_EFFECTOR_CONTROL_H_INCLUDED #define END_EFFECTOR_CONTROL_H_INCLUDED -#include "cartesian_controller_base/ROS2VersionConfig.h" -#include "geometry_msgs/msg/wrench_stamped.hpp" -#include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "std_msgs/msg/float64_multi_array.hpp" -#include "rclcpp/publisher.hpp" -#include "visualization_msgs/msg/interactive_marker.hpp" -#include "visualization_msgs/msg/interactive_marker_feedback.hpp" #include #include #include @@ -57,9 +49,16 @@ #include #include +#include "cartesian_controller_base/ROS2VersionConfig.h" +#include "geometry_msgs/msg/detail/pose_stamped__struct.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/publisher.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "visualization_msgs/msg/interactive_marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" + #define _USE_MATH_DEFINES -#include -#include #include #include "SystemModelF.hpp" @@ -67,7 +66,6 @@ #include #include - #include #include #include @@ -100,130 +98,129 @@ namespace end_effector_controller */ class EndEffectorControl : public controller_interface::ControllerInterface { - public: - EndEffectorControl(); - ~EndEffectorControl(); +public: + EndEffectorControl(); + ~EndEffectorControl(); #if defined CARTESIAN_CONTROLLERS_GALACTIC || defined CARTESIAN_CONTROLLERS_HUMBLE - virtual LifecycleNodeInterface::CallbackReturn on_init() override; + virtual LifecycleNodeInterface::CallbackReturn on_init() override; #elif defined CARTESIAN_CONTROLLERS_FOXY - virtual controller_interface::return_type init(const std::string & controller_name) override; + virtual controller_interface::return_type init(const std::string & controller_name) override; #endif - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; - controller_interface::InterfaceConfiguration command_interface_configuration() const override; - controller_interface::InterfaceConfiguration state_interface_configuration() const override; - - controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + controller_interface::return_type update(const rclcpp::Time & time, + const rclcpp::Duration & period) override; - - private: - /** +private: + /** * @brief Move visual marker in RViz according to user interaction * * This function also stores the marker pose internally. * * @param feedback The message containing the current pose of the marker */ - void updateMotionControlCallback(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr& feedback); + void updateMotionControlCallback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); - /** + /** * @brief Get the current pose of the specified end-effector * * @return The current end-effector pose with respect to the specified base link */ - geometry_msgs::msg::PoseStamped getEndEffectorPose(); + geometry_msgs::msg::PoseStamped getEndEffectorPose(); - geometry_msgs::msg::PoseStamped getEndEffectorVel(); - /** + geometry_msgs::msg::PoseStamped getEndEffectorVel(); + /** * @brief Proportional controller to set the orientation perpendicular to the surfae * * @param currentPos The end effector current pose * * @return The new orientation of the end effector */ - geometry_msgs::msg::Quaternion setEndEffectorOrientation(geometry_msgs::msg::Quaternion pos); - - void gridPosition(); - void surfaceApproach(); - void tissuePalpation(const rclcpp::Time &time); - void startingHigh(); - void newStartingPosition(); - - void ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); - void targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); - - // Handles to the joints - std::vector > - m_joint_state_pos_handles; - std::vector > - m_joint_state_vel_handles; - - std::vector m_joint_names; - std::vector m_state_interface_types; - - // Kinematics - std::string m_robot_base_link; - std::string m_end_effector_link; - std::string m_target_frame_topic; - KDL::Chain m_robot_chain; - std::shared_ptr< - KDL::ChainFkSolverVel_recursive> m_fk_solver; - - geometry_msgs::msg::PoseStamped m_current_pose; - geometry_msgs::msg::PoseStamped m_target_pose; - geometry_msgs::msg::WrenchStamped m_sinusoidal_force; - rclcpp::Publisher::SharedPtr m_pose_publisher; - rclcpp::Publisher::SharedPtr m_data_publisher; - rclcpp::Publisher::SharedPtr m_estimator_publisher; - // rclcpp::Publisher::SharedPtr m_elasticity_publisher; - // rclcpp::Publisher::SharedPtr m_position_publisher; - // rclcpp::Publisher::SharedPtr m_force_publisher; - rclcpp::Subscription::SharedPtr m_target_wrench_subscriber; - rclcpp::Subscription::SharedPtr m_ft_sensor_wrench_subscriber; - Eigen::Vector3d m_target_wrench; - Eigen::Vector3d m_ft_sensor_wrench; - Eigen::Vector3d errorOrientation; - Eigen::Vector3d cartVel; - geometry_msgs::msg::Point m_starting_position; - geometry_msgs::msg::Point m_grid_position; - uint m_phase; - uint m_palpation_number; - double m_surface; - double m_prev_force; - - public: - // Simulated (true) system state - State x; - - // System - SystemModel sys; - - // Measurement models - ForceModel fm; - - Kalman::ExtendedKalmanFilter ekf; - - Kalman::Covariance cov; - - // Previos position - float prev_pos; - rclcpp::Time initial_time; - rclcpp::Time prec_time; - - std::queue msgs_queue; - + geometry_msgs::msg::Quaternion setEndEffectorOrientation(geometry_msgs::msg::Quaternion pos); + + void gridPosition(); + void surfaceApproach(); + void tissuePalpation(const rclcpp::Time & time); + void startingHigh(); + void newStartingPosition(); + void publishDataEE(const rclcpp::Time & time); + + void ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); + void targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench); + + // Handles to the joints + std::vector > + m_joint_state_pos_handles; + std::vector > + m_joint_state_vel_handles; + + std::vector m_joint_names; + std::vector m_state_interface_types; + + // Kinematics + std::string m_robot_base_link; + std::string m_end_effector_link; + std::string m_target_frame_topic; + KDL::Chain m_robot_chain; + std::shared_ptr m_fk_solver; + + geometry_msgs::msg::PoseStamped m_current_pose; + geometry_msgs::msg::PoseStamped m_target_pose; + geometry_msgs::msg::WrenchStamped m_sinusoidal_force; + rclcpp::Publisher::SharedPtr m_pose_publisher; + rclcpp::Publisher::SharedPtr m_data_publisher; + rclcpp::Publisher::SharedPtr m_estimator_publisher; + // rclcpp::Publisher::SharedPtr m_elasticity_publisher; + // rclcpp::Publisher::SharedPtr m_position_publisher; + // rclcpp::Publisher::SharedPtr m_force_publisher; + rclcpp::Subscription::SharedPtr m_target_wrench_subscriber; + rclcpp::Subscription::SharedPtr m_ft_sensor_wrench_subscriber; + Eigen::Vector3d m_target_wrench; + Eigen::Vector3d m_ft_sensor_wrench; + Eigen::Vector3d errorOrientation; + Eigen::Vector3d cartVel; + geometry_msgs::msg::Point m_starting_position; + geometry_msgs::msg::Point m_grid_position; + uint m_phase; + uint m_palpation_number; + double m_surface; + double m_prev_force; + +public: + // Simulated (true) system state + State x; + + // System + SystemModel sys; + + // Measurement models + ForceModel fm; + + Kalman::ExtendedKalmanFilter ekf; + + Kalman::Covariance cov; + + // Previos position + float prev_pos; + rclcpp::Time initial_time; + rclcpp::Time prec_time; + + std::queue msgs_queue; }; -} // cartesian_controller_handles +} // namespace end_effector_controller #endif diff --git a/end_effector_controller/src/end_effector_control.cpp b/end_effector_controller/src/end_effector_control.cpp index 9b2114e1..cf6bfb24 100644 --- a/end_effector_controller/src/end_effector_control.cpp +++ b/end_effector_controller/src/end_effector_control.cpp @@ -37,159 +37,154 @@ */ //----------------------------------------------------------------------------- +#include +#include + +#include +#include + #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/node.hpp" #include "rclcpp/time.hpp" #include "visualization_msgs/msg/detail/interactive_marker_feedback__struct.hpp" -#include -#include -#include -#include namespace end_effector_controller { - EndEffectorControl::EndEffectorControl() {} +EndEffectorControl::EndEffectorControl() {} - EndEffectorControl::~EndEffectorControl() {} +EndEffectorControl::~EndEffectorControl() {} - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - EndEffectorControl::on_activate(const rclcpp_lifecycle::State &previous_state) +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +EndEffectorControl::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + // Get state handles. + // if (!controller_interface::get_ordered_interfaces( + // state_interfaces_, m_joint_names, hardware_interface::HW_IF_POSITION, m_joint_handles)) + // { + // RCLCPP_ERROR(get_node()->get_logger(), + // "Expected %zu '%s' state interfaces, got %zu.", + // m_joint_names.size(), + // hardware_interface::HW_IF_POSITION, + // m_joint_handles.size()); + // return CallbackReturn::ERROR; + // } + + if (!controller_interface::get_ordered_interfaces(state_interfaces_, m_joint_names, + hardware_interface::HW_IF_POSITION, + m_joint_state_pos_handles)) { - // Get state handles. - // if (!controller_interface::get_ordered_interfaces( - // state_interfaces_, m_joint_names, hardware_interface::HW_IF_POSITION, m_joint_handles)) - // { - // RCLCPP_ERROR(get_node()->get_logger(), - // "Expected %zu '%s' state interfaces, got %zu.", - // m_joint_names.size(), - // hardware_interface::HW_IF_POSITION, - // m_joint_handles.size()); - // return CallbackReturn::ERROR; - // } - - if (!controller_interface::get_ordered_interfaces(state_interfaces_, - m_joint_names, - hardware_interface::HW_IF_POSITION, - m_joint_state_pos_handles)) - { - RCLCPP_ERROR(get_node()->get_logger(), - "Expected %zu '%s' state interfaces, got %zu.", - m_joint_names.size(), - hardware_interface::HW_IF_POSITION, - m_joint_state_pos_handles.size()); - return CallbackReturn::ERROR; - } + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + m_joint_names.size(), hardware_interface::HW_IF_POSITION, + m_joint_state_pos_handles.size()); + return CallbackReturn::ERROR; + } - // Velocity - if (!controller_interface::get_ordered_interfaces(state_interfaces_, - m_joint_names, - hardware_interface::HW_IF_VELOCITY, - m_joint_state_vel_handles)) - { - RCLCPP_ERROR(get_node()->get_logger(), - "Expected %zu '%s' state interfaces, got %zu.", - m_joint_names.size(), - hardware_interface::HW_IF_VELOCITY, - m_joint_state_vel_handles.size()); - return CallbackReturn::ERROR; - } + // Velocity + if (!controller_interface::get_ordered_interfaces(state_interfaces_, m_joint_names, + hardware_interface::HW_IF_VELOCITY, + m_joint_state_vel_handles)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", + m_joint_names.size(), hardware_interface::HW_IF_VELOCITY, + m_joint_state_vel_handles.size()); + return CallbackReturn::ERROR; + } - m_current_pose = getEndEffectorPose(); - prev_pos = m_current_pose.pose.position.z; - m_starting_position = m_current_pose.pose.position; - m_starting_position.x = 0.036; - m_starting_position.y = -0.36; - m_grid_position = m_starting_position; + m_current_pose = getEndEffectorPose(); + prev_pos = m_current_pose.pose.position.z; + m_starting_position = m_current_pose.pose.position; + m_grid_position = m_starting_position; - m_ft_sensor_wrench(0) = 0.0; - m_ft_sensor_wrench(1) = 0.0; - m_ft_sensor_wrench(2) = 0.0; + m_ft_sensor_wrench(0) = 0.0; + m_ft_sensor_wrench(1) = 0.0; + m_ft_sensor_wrench(2) = 0.0; - m_target_wrench(0) = 0.0; - m_target_wrench(1) = 0.0; - m_target_wrench(2) = 0.0; + m_target_wrench(0) = 0.0; + m_target_wrench(1) = 0.0; + m_target_wrench(2) = 0.0; - initial_time = get_node()->now(); + initial_time = get_node()->now(); - // Set the initial state of the estimator - State x; - x.x1() = 0.0; - x.x2() = -cartVel(2); - x.x3() = 1000.0; - x.x4() = 1000.0; - - // Init filters with true system state - ekf.init(x); - - // Save covariance for later - cov = ekf.getCovariance(); - cov.setZero(); - - // Set initial values for the covariance - cov(0, 0) = 1.0; - cov(1, 1) = 1.0; - cov(2, 2) = 1000.0; - cov(3, 3) = 1000.0; - - // Set covariance - ekf.setCovariance(cov); - - // Set data for the kalman filter estimation - sys.setModelData(0.05, 0.002, 1.35); - - - m_current_pose = getEndEffectorPose(); - prev_pos = m_current_pose.pose.position.z; - prec_time = get_node()->now(); - - m_phase = 1; - m_palpation_number = 0; - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } + // Set the initial state of the estimator + State x; + x.x1() = 0.0; + x.x2() = -cartVel(2); + x.x3() = 1000.0; + x.x4() = 1000.0; + + // Init filters with true system state + ekf.init(x); + + // Save covariance for later + cov = ekf.getCovariance(); + cov.setZero(); + + // Set initial values for the covariance + cov(0, 0) = 1.0; + cov(1, 1) = 1.0; + cov(2, 2) = 1000.0; + cov(3, 3) = 1000.0; + + // Set covariance + ekf.setCovariance(cov); + + // Set data for the kalman filter estimation + sys.setModelData(0.05, 0.002, 1.35); + + m_current_pose = getEndEffectorPose(); + prev_pos = m_current_pose.pose.position.z; + prec_time = get_node()->now(); + + m_phase = 1; + m_palpation_number = 0; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - EndEffectorControl::on_deactivate(const rclcpp_lifecycle::State &previous_state) + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +EndEffectorControl::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + m_joint_state_pos_handles.clear(); + m_joint_state_vel_handles.clear(); + this->release_interfaces(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type EndEffectorControl::update(const rclcpp::Time & time, + const rclcpp::Duration & period) +{ + // Control for the palpation + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "Phase: " << m_phase); + // Print the phase every half a second + if (time.nanoseconds() % 500000000 == 0) { - m_joint_state_pos_handles.clear(); - m_joint_state_vel_handles.clear(); - this->release_interfaces(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Phase: " << m_phase); } - controller_interface::return_type EndEffectorControl::update(const rclcpp::Time &time, - const rclcpp::Duration &period) + // Get the end effector pose + m_current_pose = getEndEffectorPose(); + + m_target_pose.pose.orientation.x = 1; + m_target_pose.pose.orientation.y = 0; + m_target_pose.pose.orientation.z = 0; + m_target_pose.pose.orientation.w = 0; + // m_current_pose.pose.position.x = m_starting_position.x; + // m_current_pose.pose.position.y = m_starting_position.y; + + // The controller will palpate the tissue in a square of 0.12x0.12 m starting from the bottom left corner, + // every palpation is done with a distance of 0.03 m from the previous one. The palpation is done in the z direction + // and will least 3 seconds. The palpation is done with with a sinusoidal movement of 2hz, a bias of -0.015 m and an amplitude of 0.008 m. + + // Switch case will be used to divide the process in 4 phases: + // 1. Move the end effector to the position of the palpation + // 2. Move the end effector in order to touch the surface of the tissue + // 3. Move the end effector in order to palpate the tCL_issue + // 4. Move the end effector in order to go back to the initial high + + switch (m_phase) { - if (prec_time == time) - { - return controller_interface::return_type::OK; - } - // Control for the palpation - - // Get the end effector pose - m_current_pose = getEndEffectorPose(); - - m_target_pose.pose.orientation.x = 1; - m_target_pose.pose.orientation.y = 0; - m_target_pose.pose.orientation.z = 0; - m_target_pose.pose.orientation.w = 0; - // m_current_pose.pose.position.x = m_starting_position.x; - // m_current_pose.pose.position.y = m_starting_position.y; - - // The controller will palpate the tissue in a square of 0.12x0.12 m starting from the bottom left corner, - // every palpation is done with a distance of 0.03 m from the previous one. The palpation is done in the z direction - // and will least 3 seconds. The palpation is done with with a sinusoidal movement of 2hz, a bias of -0.015 m and an amplitude of 0.008 m. - - // Switch case will be used to divide the process in 4 phases: - // 1. Move the end effector to the position of the palpation - // 2. Move the end effector in order to touch the surface of the tissue - // 3. Move the end effector in order to palpate the tissue - // 4. Move the end effector in order to go back to the initial high - - switch (m_phase) - { case 1: gridPosition(); break; @@ -198,469 +193,385 @@ namespace end_effector_controller break; case 3: tissuePalpation(time); - break; + break; case 4: startingHigh(); - break; + break; default: break; - } - - - return controller_interface::return_type::OK; } - void EndEffectorControl::gridPosition() - { - // The end effector will move to the position of the palpation - m_target_pose.pose.position.x = m_grid_position.x; - m_target_pose.pose.position.y = m_grid_position.y; - m_target_pose.pose.position.z = m_starting_position.z; - - m_target_pose.header.stamp = get_node()->now(); - m_target_pose.header.frame_id = m_robot_base_link; + // Publish the data + publishDataEE(time); - m_pose_publisher->publish(m_target_pose); + return controller_interface::return_type::OK; +} - // If the end effector is in the position of the palpation the phase is finished - if (abs(m_current_pose.pose.position.x - m_grid_position.x) < 0.005 && abs(m_current_pose.pose.position.y - m_grid_position.y) < 0.005) - { - m_phase = 2; - m_prev_force = 0.0; - } - } +void EndEffectorControl::gridPosition() +{ + // The end effector will move to the position of the palpation + m_target_pose.pose.position.x = m_grid_position.x; + m_target_pose.pose.position.y = m_grid_position.y; + m_target_pose.pose.position.z = m_starting_position.z; - void EndEffectorControl::surfaceApproach() - { - // If the detected force in the z direction is greater than 0.1 N the phase is finished - if (m_grid_position.z < 0.085 && m_current_pose.pose.position.z < 0.0982) - { - m_phase = 3; - m_surface = m_current_pose.pose.position.z; - m_target_pose.pose.position.x = m_grid_position.x; - m_target_pose.pose.position.y = m_grid_position.y; - m_target_pose.pose.position.z = m_current_pose.pose.position.z; - m_grid_position.z = m_current_pose.pose.position.z; - initial_time = get_node()->now(); - } - // The end effector will move in the z direction until it touches the surface of the tissue - else - { - m_target_pose.pose.position.x = m_grid_position.x; - m_target_pose.pose.position.y = m_grid_position.y; - m_grid_position.z -= 0.01/500; - m_target_pose.pose.position.z = m_grid_position.z; - m_prev_force = m_ft_sensor_wrench(2); - } + m_target_pose.header.stamp = get_node()->now(); + m_target_pose.header.frame_id = m_robot_base_link; - m_target_pose.header.stamp = get_node()->now(); - m_target_pose.header.frame_id = m_robot_base_link; + m_pose_publisher->publish(m_target_pose); - m_pose_publisher->publish(m_target_pose); + // If the end effector is in the position of the palpation the phase is finished + if (abs(m_current_pose.pose.position.x - m_grid_position.x) < 0.005 && + abs(m_current_pose.pose.position.y - m_grid_position.y) < 0.005) + { + m_phase = 2; + m_prev_force = 0.0; + std::cout << "Phase 2" << std::endl; } +} - void EndEffectorControl::tissuePalpation(const rclcpp::Time &time) +void EndEffectorControl::surfaceApproach() +{ + // If the detected force in the z direction is greater than 5 N the phase is finished + if (m_ft_sensor_wrench(2) < -5.0) { - m_target_pose.pose.position.x = m_starting_position.x - 0.01 * ( - initial_time.nanoseconds() * 1e-9 + time.nanoseconds() * 1e-9); + std::cout << "Phase 3" << std::endl; + m_phase = 3; + m_surface = m_current_pose.pose.position.z; + m_target_pose.pose.position.x = m_grid_position.x; m_target_pose.pose.position.y = m_grid_position.y; - m_target_pose.pose.position.z = 0.085; - // prova di carico - // std::cout << "x: " << m_target_pose.pose.position.x << std::endl; - // std::cout << "time: " << time.nanoseconds() * 1e-9 - initial_time.nanoseconds()*1e-9<< std::endl; - // z position is a sine wave with frequency 10 Hz and amplitude 0.02 m - //m_target_pose.pose.position.z = m_surface - 0.02 - 0.005 * sin(2 * M_PI * (time.nanoseconds() * 1e-9) * 2.5); - - m_target_pose.header.stamp = get_node()->now(); - m_target_pose.header.frame_id = m_robot_base_link; - - m_pose_publisher->publish(m_target_pose); - - - // Publish state - std_msgs::msg::Float64MultiArray msg; - if (msgs_queue.size() < 28) - { - msg.data = { - (time.nanoseconds() * 1e-9), - m_surface, - m_current_pose.pose.position.z, - m_target_pose.pose.position.z, - cartVel(2), - 0, - (double)m_palpation_number, - m_current_pose.pose.position.x, - m_current_pose.pose.position.y - }; - msgs_queue.push(msg); - } - else - { - msgs_queue.front().data[5] = -m_ft_sensor_wrench(2); - m_data_publisher->publish(msgs_queue.front()); - - msgs_queue.pop(); - msg.data = { - (time.nanoseconds() * 1e-9), - m_surface, - m_current_pose.pose.position.z, - m_target_pose.pose.position.z, - cartVel(2), - 0, - (double)m_palpation_number, - m_current_pose.pose.position.x, - m_current_pose.pose.position.y - }; - msgs_queue.push(msg); - } - - // If the time is greater than 5 seconds the phase is finished - if (time.nanoseconds() * 1e-9 - initial_time.nanoseconds() * 1e-9 > 20) - { - m_phase = 4; - m_palpation_number++; - while(!msgs_queue.empty()) - { - msgs_queue.pop(); - } - } + m_target_pose.pose.position.z = m_current_pose.pose.position.z; + m_grid_position.z = m_current_pose.pose.position.z; + initial_time = get_node()->now(); } - - void EndEffectorControl::startingHigh() + // The end effector will move in the z direction until it touches the surface of the tissue + else { - // m_target_pose.pose.position.x = m_starting_position.x; - // m_target_pose.pose.position.y = m_starting_position.y; - m_target_pose.pose.position.z = m_starting_position.z; - - m_target_pose.pose.orientation.x = 1; - m_target_pose.pose.orientation.y = 0; - m_target_pose.pose.orientation.z = 0; - m_target_pose.pose.orientation.w = 0; - m_target_pose.header.stamp = get_node()->now(); - m_target_pose.header.frame_id = m_robot_base_link; - - m_pose_publisher->publish(m_target_pose); - - // If the end effector is the starting high the phase is finished - // if (abs(m_current_pose.pose.position.z - m_starting_position.z) < 0.005) - // { - // m_phase = 4; - // newStartingPosition(); - // m_grid_position.z = m_starting_position.z; - // } + m_target_pose.pose.position.x = m_grid_position.x; + m_target_pose.pose.position.y = m_grid_position.y; + m_grid_position.z -= 0.01 / 500; + m_target_pose.pose.position.z = m_grid_position.z; + m_prev_force = m_ft_sensor_wrench(2); + initial_time = get_node()->now(); } - void EndEffectorControl::newStartingPosition(){ - m_grid_position.x = m_starting_position.x + 0.01 * (int)(m_palpation_number/11); - m_grid_position.y = m_starting_position.y + 0.01 * (m_palpation_number%11); - } + m_target_pose.header.stamp = get_node()->now(); + m_target_pose.header.frame_id = m_robot_base_link; - controller_interface::InterfaceConfiguration - EndEffectorControl::command_interface_configuration() const - { - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::NONE; - return conf; - } + m_pose_publisher->publish(m_target_pose); +} + +void EndEffectorControl::tissuePalpation(const rclcpp::Time & time) +{ + m_target_pose.pose.position.x = m_grid_position.x; + m_target_pose.pose.position.y = m_grid_position.y; + m_target_pose.pose.position.z = + m_grid_position.z - + 0.008 * sin(2 * M_PI * (time.nanoseconds() * 1e-9 - initial_time.nanoseconds() * 1e-9) * 2); + // prova di carico + // std::cout << "x: " << m_target_pose.pose.position.x << std::endl; + // std::cout << "time: " << time.nanoseconds() * 1e-9 - initial_time.nanoseconds()*1e-9<< std::endl; + // z position is a sine wave with frequency 10 Hz and amplitude 0.02 m + //m_target_pose.pose.position.z = m_surface - 0.02 - 0.005 * sin(2 * M_PI * (time.nanoseconds() * 1e-9) * 2.5); + + m_target_pose.header.stamp = get_node()->now(); + m_target_pose.header.frame_id = m_robot_base_link; + + m_pose_publisher->publish(m_target_pose); + + // Publish state + // std_msgs::msg::Float64MultiArray msg; + // if (msgs_queue.size() < 28) + // { + // msg.data = {(time.nanoseconds() * 1e-9), + // m_surface, + // m_current_pose.pose.position.z, + // m_target_pose.pose.position.z, + // cartVel(2), + // 0, + // (double)m_palpation_number, + // m_current_pose.pose.position.x, + // m_current_pose.pose.position.y}; + // msgs_queue.push(msg); + // } + // else + // { + // msgs_queue.front().data[5] = -m_ft_sensor_wrench(2); + // m_data_publisher->publish(msgs_queue.front()); + + // msgs_queue.pop(); + // msg.data = {(time.nanoseconds() * 1e-9), + // m_surface, + // m_current_pose.pose.position.z, + // m_target_pose.pose.position.z, + // cartVel(2), + // 0, + // (double)m_palpation_number, + // m_current_pose.pose.position.x, + // m_current_pose.pose.position.y}; + // msgs_queue.push(msg); + // } - controller_interface::InterfaceConfiguration - EndEffectorControl::state_interface_configuration() const + // If the time is greater than 5 seconds the phase is finished + if (time.nanoseconds() * 1e-9 - initial_time.nanoseconds() * 1e-9 > 20) { - controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - conf.names.reserve(m_joint_names.size() * 2); - for (const auto& type : m_state_interface_types) + std::cout << "Phase 4" << std::endl; + m_phase = 4; + m_palpation_number++; + while (!msgs_queue.empty()) { - for (const auto & joint_name : m_joint_names) - { - conf.names.push_back(joint_name + std::string("/").append(type)); - } + msgs_queue.pop(); } - return conf; } +} - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - EndEffectorControl::on_init() - { - - auto_declare("robot_description", ""); - auto_declare("robot_base_link", ""); - auto_declare("end_effector_link", ""); - auto_declare>("joints", std::vector()); +void EndEffectorControl::startingHigh() +{ + // m_target_pose.pose.position.x = m_starting_position.x; + // m_target_pose.pose.position.y = m_starting_position.y; + m_target_pose.pose.position.z = m_starting_position.z; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } + m_target_pose.pose.orientation.x = 1; + m_target_pose.pose.orientation.y = 0; + m_target_pose.pose.orientation.z = 0; + m_target_pose.pose.orientation.w = 0; + m_target_pose.header.stamp = get_node()->now(); + m_target_pose.header.frame_id = m_robot_base_link; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn - EndEffectorControl::on_configure(const rclcpp_lifecycle::State &previous_state) - { - // Get kinematics specific configuration - urdf::Model robot_model; - KDL::Tree robot_tree; - - std::string robot_description = get_node()->get_parameter("robot_description").as_string(); - if (robot_description.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "robot_description is empty"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - m_robot_base_link = get_node()->get_parameter("robot_base_link").as_string(); - if (m_robot_base_link.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "robot_base_link is empty"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - m_end_effector_link = get_node()->get_parameter("end_effector_link").as_string(); - if (m_end_effector_link.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "end_effector_link is empty"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } + m_pose_publisher->publish(m_target_pose); - // Build a kinematic chain of the robot - if (!robot_model.initString(robot_description)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf model from 'robot_description'"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from urdf model"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) - { - const std::string error = "" - "Failed to parse robot chain from urdf model. " - "Do robot_base_link and end_effector_link exist?"; - RCLCPP_ERROR(get_node()->get_logger(), "%s", error.c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } + // If the end effector is the starting high the phase is finished + // if (abs(m_current_pose.pose.position.z - m_starting_position.z) < 0.005) + // { + // m_phase = 4; + // newStartingPosition(); + // m_grid_position.z = m_starting_position.z; + // } +} - // Get names of the joints - m_joint_names = get_node()->get_parameter("joints").as_string_array(); - if (m_joint_names.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } +void EndEffectorControl::newStartingPosition() +{ + m_grid_position.x = m_starting_position.x + 0.01 * (int)(m_palpation_number / 11); + m_grid_position.y = m_starting_position.y + 0.01 * (m_palpation_number % 11); +} - // Publishers - m_pose_publisher = get_node()->create_publisher( - get_node()->get_name() + std::string("/target_frame"), 10); - - m_data_publisher = get_node()->create_publisher( - std::string("/data_control"), 10); - - m_estimator_publisher = get_node()->create_publisher( - std::string("/data_estimation"), 10); - - // m_elasticity_publisher = get_node()->create_publisher( - // std::string("/position_desired"), 10); - - // m_position_publisher = get_node()->create_publisher( - // std::string("/force_z"), 10); - - // m_force_publisher = get_node()->create_publisher( - // std::string("/surface"), 10); - - // m_time_publisher = get_node()->create_publisher( - // std::string("/time"), 10); - m_state_interface_types.push_back("position"); - m_state_interface_types.push_back("velocity"); - - // Subscriber - m_target_wrench_subscriber = get_node()->create_subscription( - get_node()->get_name() + std::string("/target_wrench"), - 10, - std::bind(&EndEffectorControl::targetWrenchCallback, this, std::placeholders::_1)); - - m_ft_sensor_wrench_subscriber = - get_node()->create_subscription( - get_node()->get_name() + std::string("/ft_sensor_wrench"), - 10, - std::bind(&EndEffectorControl::ftSensorWrenchCallback, this, std::placeholders::_1)); - - // Initialize kinematics - m_fk_solver.reset(new KDL::ChainFkSolverVel_recursive(m_robot_chain)); - m_current_pose = getEndEffectorPose(); - Eigen::Quaterniond current_quat(m_current_pose.pose.orientation.w, m_current_pose.pose.orientation.x, m_current_pose.pose.orientation.y, m_current_pose.pose.orientation.z); - Eigen::AngleAxisd current_aa(current_quat); - m_sinusoidal_force.wrench.force.x = 0.0; - m_sinusoidal_force.wrench.force.y = 0.0; - m_sinusoidal_force.wrench.force.z = 15.0; - m_sinusoidal_force.wrench.torque.x = 0.0; - m_sinusoidal_force.wrench.torque.y = 0.0; - m_sinusoidal_force.wrench.torque.z = 0.0; - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } +controller_interface::InterfaceConfiguration EndEffectorControl::command_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::NONE; + return conf; +} - geometry_msgs::msg::Quaternion EndEffectorControl::setEndEffectorOrientation(geometry_msgs::msg::Quaternion pos) +void EndEffectorControl::publishDataEE(const rclcpp::Time & time) +{ + // Publish state + // time, current position, target position, velocity, force + std_msgs::msg::Float64MultiArray msg; + msg.data = {(time.nanoseconds() * 1e-9), m_current_pose.pose.position.z, + m_target_pose.pose.position.z, cartVel(2), m_ft_sensor_wrench(2)}; + m_data_publisher->publish(msg); +} + +controller_interface::InterfaceConfiguration EndEffectorControl::state_interface_configuration() + const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + conf.names.reserve(m_joint_names.size() * 2); + for (const auto & type : m_state_interface_types) { - - // if no target force is not published the controller do nothing - if (m_target_wrench[0] == 0.0 && m_target_wrench[1] == 0.0 && m_target_wrench[2] == 0.0) - { - return pos; - } - // if no target force is not published the controller do nothing - if (abs(m_ft_sensor_wrench[0]) <= 0.1 && abs(m_ft_sensor_wrench[1]) <= 0.1 && abs(m_ft_sensor_wrench[2]) <= 0.1) - { - return pos; - } - // Normalize the vectors - m_ft_sensor_wrench.normalize(); - m_target_wrench.normalize(); - - // Calculate the cross product of the normalized vectors - errorOrientation = m_ft_sensor_wrench.cross(m_target_wrench); - - // Calculate the dot product of the normalized vectors - double cos_theta = m_ft_sensor_wrench.dot(m_target_wrench); - if (cos_theta > 1.0) - { - cos_theta = 1.0; - } - if (cos_theta < -1.0) + for (const auto & joint_name : m_joint_names) { - cos_theta = -1.0; + conf.names.push_back(joint_name + std::string("/").append(type)); } + } + return conf; +} - // Calculate the angle between the vectors using the inverse cosine function - double theta = std::acos(cos_theta); - - // Create a quaternion with the angle-axis representation - Eigen::Quaterniond q_new(Eigen::AngleAxisd(theta, errorOrientation)); - - // Define a previous quaternion rotation - Eigen::Quaterniond q_current(pos.w, pos.x, pos.y, pos.z); - - // Multiply the two quaternions together to get the new rotation - Eigen::Quaterniond q_final = q_new * q_current; - q_final.normalize(); - - geometry_msgs::msg::Point pt; - pt.x = errorOrientation[0]; - pt.y = errorOrientation[1]; - pt.z = errorOrientation[2]; - - // m_error_publisher->publish(pt); - - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q_current" << q_current); - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q_new" << q_new); - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "m_ft_sensor_wrench" << m_ft_sensor_wrench); - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "m_target_wrench" << m_target_wrench); - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "errorOrientation" << errorOrientation); - - // Multiply the two quaternions together to get the new rotation - Eigen::Quaterniond q_interp = q_current.slerp(1, q_final); +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +EndEffectorControl::on_init() +{ + auto_declare("robot_description", ""); + auto_declare("robot_base_link", ""); + auto_declare("end_effector_link", ""); + auto_declare>("joints", std::vector()); - pos.x = (double)q_interp.x(); - pos.y = (double)q_interp.y(); - pos.z = (double)q_interp.z(); - pos.w = (double)q_interp.w(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q_interp" << q_interp); +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +EndEffectorControl::on_configure(const rclcpp_lifecycle::State & previous_state) +{ + // Get kinematics specific configuration + urdf::Model robot_model; + KDL::Tree robot_tree; - return pos; + std::string robot_description = get_node()->get_parameter("robot_description").as_string(); + if (robot_description.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "robot_description is empty"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - - geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorPose() + m_robot_base_link = get_node()->get_parameter("robot_base_link").as_string(); + if (m_robot_base_link.empty()) { - KDL::JntArray positions(m_joint_state_pos_handles.size()); - KDL::JntArray velocities(m_joint_state_pos_handles.size()); - for (size_t i = 0; i < m_joint_state_pos_handles.size(); ++i) - { - positions(i) = m_joint_state_pos_handles[i].get().get_value(); - velocities(i) = m_joint_state_vel_handles[i].get().get_value(); - } - - KDL::JntArrayVel joint_data(positions,velocities); - KDL::FrameVel tmp; - m_fk_solver->JntToCart(joint_data, tmp); - - geometry_msgs::msg::PoseStamped current; - current.pose.position.x = tmp.p.p.x(); - current.pose.position.y = tmp.p.p.y(); - current.pose.position.z = tmp.p.p.z(); - tmp.M.R.GetQuaternion(current.pose.orientation.x, - current.pose.orientation.y, - current.pose.orientation.z, - current.pose.orientation.w); - - // Publish state - cartVel(0) = tmp.p.v.x(); - cartVel(1) = tmp.p.v.y(); - cartVel(2) = tmp.p.v.z(); - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "vel arrr: " << cartVel); - return current; + RCLCPP_ERROR(get_node()->get_logger(), "robot_base_link is empty"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - - // geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorVel() - // { - // KDL::JntArray velocities(m_joint_state_pos_handles.size()); - // for (size_t i = 0; i < m_joint_state_pos_handles.size(); ++i) - // { - // velocities(i) = m_joint_state_vel_handles[i].get().get_value(); - // } - // geometry_msgs::msg::PoseStamped current; - - // RCLCPP_INFO_STREAM(get_node()->get_logger(), "vel arrr: " << velocities.data); - // return current; - // } - - void EndEffectorControl::ftSensorWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) + m_end_effector_link = get_node()->get_parameter("end_effector_link").as_string(); + if (m_end_effector_link.empty()) { - KDL::Wrench tmp; - tmp[0] = wrench->wrench.force.x; - tmp[1] = wrench->wrench.force.y; - tmp[2] = wrench->wrench.force.z; - - m_ft_sensor_wrench(0) = tmp[0]; - m_ft_sensor_wrench(1) = tmp[1]; - m_ft_sensor_wrench(2) = tmp[2]; + RCLCPP_ERROR(get_node()->get_logger(), "end_effector_link is empty"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + // Build a kinematic chain of the robot + if (!robot_model.initString(robot_description)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf model from 'robot_description'"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + if (!kdl_parser::treeFromUrdfModel(robot_model, robot_tree)) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse KDL tree from urdf model"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + if (!robot_tree.getChain(m_robot_base_link, m_end_effector_link, m_robot_chain)) + { + const std::string error = + "" + "Failed to parse robot chain from urdf model. " + "Do robot_base_link and end_effector_link exist?"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", error.c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + // Get names of the joints + m_joint_names = get_node()->get_parameter("joints").as_string_array(); + if (m_joint_names.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "joints array is empty"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - void EndEffectorControl::targetWrenchCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) + // Publishers + m_pose_publisher = get_node()->create_publisher( + get_node()->get_name() + std::string("/target_frame"), 10); + + m_data_publisher = get_node()->create_publisher( + std::string("/data_control"), 10); + + m_estimator_publisher = get_node()->create_publisher( + std::string("/data_estimation"), 10); + + // m_elasticity_publisher = get_node()->create_publisher( + // std::string("/position_desired"), 10); + + // m_position_publisher = get_node()->create_publisher( + // std::string("/force_z"), 10); + + // m_force_publisher = get_node()->create_publisher( + // std::string("/surface"), 10); + + // m_time_publisher = get_node()->create_publisher( + // std::string("/time"), 10); + m_state_interface_types.push_back("position"); + m_state_interface_types.push_back("velocity"); + + // Subscriber + m_target_wrench_subscriber = get_node()->create_subscription( + get_node()->get_name() + std::string("/target_wrench"), 10, + std::bind(&EndEffectorControl::targetWrenchCallback, this, std::placeholders::_1)); + + m_ft_sensor_wrench_subscriber = + get_node()->create_subscription( + get_node()->get_name() + std::string("/ft_sensor_wrench"), 10, + std::bind(&EndEffectorControl::ftSensorWrenchCallback, this, std::placeholders::_1)); + + // Initialize kinematics + m_fk_solver.reset(new KDL::ChainFkSolverVel_recursive(m_robot_chain)); + m_current_pose = getEndEffectorPose(); + Eigen::Quaterniond current_quat( + m_current_pose.pose.orientation.w, m_current_pose.pose.orientation.x, + m_current_pose.pose.orientation.y, m_current_pose.pose.orientation.z); + Eigen::AngleAxisd current_aa(current_quat); + m_sinusoidal_force.wrench.force.x = 0.0; + m_sinusoidal_force.wrench.force.y = 0.0; + m_sinusoidal_force.wrench.force.z = 15.0; + m_sinusoidal_force.wrench.torque.x = 0.0; + m_sinusoidal_force.wrench.torque.y = 0.0; + m_sinusoidal_force.wrench.torque.z = 0.0; + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorPose() +{ + KDL::JntArray positions(m_joint_state_pos_handles.size()); + KDL::JntArray velocities(m_joint_state_pos_handles.size()); + for (size_t i = 0; i < m_joint_state_pos_handles.size(); ++i) { - KDL::Wrench tmp; - tmp[0] = wrench->wrench.force.x; - tmp[1] = wrench->wrench.force.y; - tmp[2] = wrench->wrench.force.z; - - m_target_wrench(0) = -tmp[0]; - m_target_wrench(1) = -tmp[1]; - m_target_wrench(2) = -tmp[2]; + positions(i) = m_joint_state_pos_handles[i].get().get_value(); + velocities(i) = m_joint_state_vel_handles[i].get().get_value(); } - // void CartesianForceController::setFtSensorReferenceFrame(const std::string& new_ref) - // { - // // Compute static transform from the force torque sensor to the new reference - // // frame of interest. - // m_new_ft_sensor_ref = new_ref; - - // // Joint positions should cancel out, i.e. it doesn't matter as long as they - // // are the same for both transformations. - // KDL::JntArray jnts(EndEffectorControl::m_ik_solver->getPositions()); - - // KDL::Frame sensor_ref; - // Base::m_forward_kinematics_solver->JntToCart( - // jnts, - // sensor_ref, - // m_ft_sensor_ref_link); - - // KDL::Frame new_sensor_ref; - // Base::m_forward_kinematics_solver->JntToCart( - // jnts, - // new_sensor_ref, - // m_new_ft_sensor_ref); - - // m_ft_sensor_transform = new_sensor_ref.Inverse() * sensor_ref; - // } + KDL::JntArrayVel joint_data(positions, velocities); + KDL::FrameVel tmp; + m_fk_solver->JntToCart(joint_data, tmp); + + geometry_msgs::msg::PoseStamped current; + current.pose.position.x = tmp.p.p.x(); + current.pose.position.y = tmp.p.p.y(); + current.pose.position.z = tmp.p.p.z(); + tmp.M.R.GetQuaternion(current.pose.orientation.x, current.pose.orientation.y, + current.pose.orientation.z, current.pose.orientation.w); + + cartVel(0) = tmp.p.v.x(); + cartVel(1) = tmp.p.v.y(); + cartVel(2) = tmp.p.v.z(); + return current; +} + +// geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorVel() +// { +// KDL::JntArray velocities(m_joint_state_pos_handles.size()); +// for (size_t i = 0; i < m_joint_state_pos_handles.size(); ++i) +// { +// velocities(i) = m_joint_state_vel_handles[i].get().get_value(); +// } +// geometry_msgs::msg::PoseStamped current; + +// RCLCPP_INFO_STREAM(get_node()->get_logger(), "vel arrr: " << velocities.data); +// return current; +// } + +void EndEffectorControl::ftSensorWrenchCallback( + const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) +{ + KDL::Wrench tmp; + tmp[0] = wrench->wrench.force.x; + tmp[1] = wrench->wrench.force.y; + tmp[2] = wrench->wrench.force.z; + + m_ft_sensor_wrench(0) = tmp[0]; + m_ft_sensor_wrench(1) = tmp[1]; + m_ft_sensor_wrench(2) = tmp[2]; +} + +void EndEffectorControl::targetWrenchCallback( + const geometry_msgs::msg::WrenchStamped::SharedPtr wrench) +{ + KDL::Wrench tmp; + tmp[0] = wrench->wrench.force.x; + tmp[1] = wrench->wrench.force.y; + tmp[2] = wrench->wrench.force.z; + + m_target_wrench(0) = -tmp[0]; + m_target_wrench(1) = -tmp[1]; + m_target_wrench(2) = -tmp[2]; +} -} // namespace cartesian_controller_handles +} // namespace end_effector_controller // Pluginlib #include