From 3dc79ae5e6506223d497d45245042586e75260ca Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 7 Jun 2023 10:40:50 +0900 Subject: [PATCH] Added source files --- cfg/sas_operator_side_receiver.yaml | 3 + cfg/sas_patient_side_manager.yaml | 17 + cfg/sas_robot_driver_franka_1.yaml | 7 + include/motion_generator.h | 77 ++ include/quadratic_program_motion_generator.h | 74 ++ include/robot_interface_franka.h | 174 ++++ include/sas_robot_driver_franka.h | 89 +++ launch/sas_operator_side_receiver.launch | 7 + launch/sas_patient_side_manager.launch | 8 + launch/sas_robot_driver_franka_1.launch | 5 + package.xml | 84 ++ src/motion_generator.cpp | 142 ++++ src/quadratic_program_motion_generator.cpp | 159 ++++ src/robot_interface_franka.cpp | 789 +++++++++++++++++++ src/sas_robot_driver_franka.cpp | 120 +++ src/sas_robot_driver_franka_node.cpp | 90 +++ src/teleop_franka_node.cpp | 235 ++++++ 17 files changed, 2080 insertions(+) create mode 100644 cfg/sas_operator_side_receiver.yaml create mode 100644 cfg/sas_patient_side_manager.yaml create mode 100644 cfg/sas_robot_driver_franka_1.yaml create mode 100644 include/motion_generator.h create mode 100644 include/quadratic_program_motion_generator.h create mode 100644 include/robot_interface_franka.h create mode 100644 include/sas_robot_driver_franka.h create mode 100644 launch/sas_operator_side_receiver.launch create mode 100644 launch/sas_patient_side_manager.launch create mode 100644 launch/sas_robot_driver_franka_1.launch create mode 100644 package.xml create mode 100644 src/motion_generator.cpp create mode 100644 src/quadratic_program_motion_generator.cpp create mode 100644 src/robot_interface_franka.cpp create mode 100644 src/sas_robot_driver_franka.cpp create mode 100644 src/sas_robot_driver_franka_node.cpp create mode 100644 src/teleop_franka_node.cpp diff --git a/cfg/sas_operator_side_receiver.yaml b/cfg/sas_operator_side_receiver.yaml new file mode 100644 index 0000000..692b5b2 --- /dev/null +++ b/cfg/sas_operator_side_receiver.yaml @@ -0,0 +1,3 @@ +"patient_side_ips": ["10.198.113.107"] +"patient_side_ports": [2223] +"operator_side_ports": [2223] diff --git a/cfg/sas_patient_side_manager.yaml b/cfg/sas_patient_side_manager.yaml new file mode 100644 index 0000000..2717c98 --- /dev/null +++ b/cfg/sas_patient_side_manager.yaml @@ -0,0 +1,17 @@ +vrep_port: 19998 +thread_sampling_time_nsec: 8000000 + +master_manipulator_label_list: ["0_0"] +vrep_camera_list: ["CameraSmartArm"] +vrep_x_list: ["x"] +vrep_xd_list: ["xd"] +robot_kinematics_interface_prefix_list: ["/arm1_kinematics"] +robot_gripper_interface_prefix_list: ["/arm1/gripper"] +use_interpolator_list: [true] +interpolator_speed_max_list: [50.] +interpolator_speed_min_list: [10.] +interpolator_speed_decay_seconds_list: [5.] +force_feedback_type_list: ["Cartesian"] + +# This value is replaced by the enviroment variable VREP_IP +"vrep_ip": "0.0.0.0" diff --git a/cfg/sas_robot_driver_franka_1.yaml b/cfg/sas_robot_driver_franka_1.yaml new file mode 100644 index 0000000..53bbded --- /dev/null +++ b/cfg/sas_robot_driver_franka_1.yaml @@ -0,0 +1,7 @@ +"robot_ip_address": "172.16.0.2" +"robot_port": 5007 +"robot_speed": 20.0 +"thread_sampling_time_nsec": 4000000 +"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895] +"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] + diff --git a/include/motion_generator.h b/include/motion_generator.h new file mode 100644 index 0000000..55c54d9 --- /dev/null +++ b/include/motion_generator.h @@ -0,0 +1,77 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +/** + * @file examples_common.h + * Contains common types and functions for the examples. + */ + +/** + * Sets a default collision behavior, joint impedance and Cartesian impedance. + * + * @param[in] robot Robot instance to set behavior on. + */ +void setDefaultBehavior(franka::Robot& robot); + +/** + * An example showing how to generate a joint pose motion to a goal position. Adapted from: + * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots + * (Kogan Page Science Paper edition). + */ +class MotionGenerator { +public: + /** + * Creates a new MotionGenerator instance for a target q. + * + * @param[in] speed_factor General speed factor in range [0, 1]. + * @param[in] q_goal Target joint positions. + */ + MotionGenerator(const double &speed_factor, const Eigen::VectorXd &q_goal); + + /** + * Sends joint position calculations + * + * @param[in] robot_state Current state of the robot. + * @param[in] period Duration of execution. + * + * @return Joint positions for use inside a control loop. + */ + franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period); + +private: + using Vector7d = Eigen::Matrix; + using Vector7i = Eigen::Matrix; + + void _check_vector_size(const Eigen::VectorXd &q); + + bool calculateDesiredValues(double t, Vector7d* delta_q_d) const; + void calculateSynchronizedValues(); + + static constexpr double kDeltaQMotionFinished = 1e-3; //1e-6; + Vector7d q_goal_; + + Vector7d q_start_; + Vector7d delta_q_; + + Vector7d dq_max_sync_; + Vector7d t_1_sync_; + Vector7d t_2_sync_; + Vector7d t_f_sync_; + Vector7d q_1_; + + double time_ = 0.0; + + Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); + Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); + Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); +}; diff --git a/include/quadratic_program_motion_generator.h b/include/quadratic_program_motion_generator.h new file mode 100644 index 0000000..6030fe8 --- /dev/null +++ b/include/quadratic_program_motion_generator.h @@ -0,0 +1,74 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include "ConstraintsManager.h" + + +using namespace DQ_robotics; + +class QuadraticProgramMotionGenerator +{ +private: + + using Vector7d = Eigen::Matrix; + + double speed_factor_; + VectorXd q_; + VectorXd q_dot_; + + int n_links_ = 7; + Vector7d q_dot_max_ = (Vector7d() << 2.0, 1.0, 1.5, 1.25, 3, 1.5, 3).finished(); + Vector7d q_dot_dot_max_ = (Vector7d() << 8, 8, 8, 8, 8, 8, 8).finished(); + double n2_ = 1.0; + double n1_ = 2000*std::sqrt(n2_); + + VectorXd N1_ = (VectorXd(7)< solver_; + std::unique_ptr constraints_manager_; + + void _check_speed_factor(const double& speed_factor) const; + + +public: + QuadraticProgramMotionGenerator(const double &speed_factor, + const Eigen::VectorXd &q_initial, + const Eigen::VectorXd &q_dot_initial, + const Eigen::VectorXd &q_goal); + + QuadraticProgramMotionGenerator(const double &speed_factor, + const Eigen::VectorXd &q_dot_initial, + const Eigen::VectorXd &q_dot_goal); + + VectorXd compute_new_configuration(const VectorXd& q_goal, const double& T); + VectorXd compute_new_configuration_velocities(const VectorXd& q_dot_goal, const double& T); + + void set_gains(const double& n1, const double& n2); + + void set_diagonal_gains(const VectorXd& K1, const VectorXd& K2); + +}; + + diff --git a/include/robot_interface_franka.h b/include/robot_interface_franka.h new file mode 100644 index 0000000..9c87636 --- /dev/null +++ b/include/robot_interface_franka.h @@ -0,0 +1,174 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "MotionGenerator.h" +#include +#include "QuadraticProgramMotionGenerator.h" +#include +#include + +using namespace DQ_robotics; +using namespace Eigen; + +class RobotInterfaceFranka +{ +public: enum class MODE{ + None=0, + PositionControl, + VelocityControl, + ForceControl, + Homing, + ClearPositions, + }; + +private: + using Vector7d = Eigen::Matrix; + std::string ip_ = "172.16.0.2"; + double speed_factor_joint_position_controller_ = 0.2; + double speed_gripper_ = 0.02; + VectorXd desired_joint_positions_; + VectorXd desired_joint_velocities_ = VectorXd::Zero(7); + + VectorXd current_joint_positions_; + std::array current_joint_positions_array_; + + VectorXd current_joint_velocities_; + std::array current_joint_velocities_array_; + + VectorXd current_joint_forces_; + std::array current_joint_forces_array_; + + franka::RobotMode robot_mode_; + + double time_ = 0; + bool initialize_flag_ = false; + void _check_if_robot_is_connected(); + void _check_if_hand_is_connected(); + + RobotInterfaceFranka::MODE mode_; + void _set_driver_mode(const RobotInterfaceFranka::MODE& mode); + + void _restart_default_parameters(); + + + void _update_robot_state(const franka::RobotState& robot_state, const double& time); + + std::shared_ptr robot_sptr_; + std::shared_ptr gripper_sptr_; + std::unique_ptr trajectory_generator_sptr_; + + void _setDefaultRobotBehavior(); + //bool hand_enabled_ = false; + const VectorXd q_home_configuration_ =(VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished(); + + const VectorXd q_max_ = ((VectorXd(7) << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished()); + const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished()); + const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished()); + const VectorXd q_max_dot_ = ((VectorXd(7) << 2, 1, 1.5, 1.25, 3, 1.5, 3).finished()); + + const double samples_ = 20; + + VectorXd desired_mean_joint_positions_; + + bool verbose_ = true; + std::string status_message_ = " "; + void _update_status_message(const std::string& message, const bool& verbose = true); + + + //-------To handle the threads----------------- + void _start_joint_position_control_mode(); + std::thread joint_position_control_mode_thread_; + void _start_joint_position_control_thread(); + std::atomic finish_motion_; + + void _start_echo_robot_state_mode(); + std::thread echo_robot_state_mode_thread_; + void _start_echo_robot_state_mode_thread(); + std::atomic finish_echo_robot_state_; + void _finish_echo_robot_state(); + + void _start_joint_velocity_control_mode(); + std::thread joint_velocity_control_mode_thread_; + void _start_joint_velocity_control_thread(); + //---------------------------------------------- + + + + VectorXd _compute_recursive_mean(const double& samples, const VectorXd& q); + VectorXd _read_once_smooth_initial_positions(const double& samples); + + +public: + + + enum HAND{ON=0, OFF}; + enum CONNECTION{AUTOMATIC}; + enum GRIPPER_STATES{WIDTH=0, MAX_WIDTH=1}; + RobotInterfaceFranka() = delete; + RobotInterfaceFranka(const RobotInterfaceFranka&) = delete; + RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete; + RobotInterfaceFranka(const std::string &ROBOT_IP, const MODE& mode, const HAND& hand = ON); + + + + VectorXd read_once_initial_positions(); + + VectorXd get_joint_target_positions(); + + void move_robot_to_target_joint_positions(const VectorXd& q_target); + + + + + double read_gripper(const GRIPPER_STATES& gripper_state = WIDTH); + bool read_grasped_status(); + void set_gripper(const double& width); + void gripper_homing(); + VectorXd get_home_robot_configuration(); + + std::string get_status_message(); + + std::string get_robot_mode(); + + + void finish_motion(); + + std::shared_ptr get_robot_pointer(); + + + //--------sas compatible methods----------// + VectorXd get_joint_positions(); + VectorXd get_joint_velocities(); + VectorXd get_joint_forces(); + + double get_time(); + + void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad); + void connect(); + void initialize(); + void deinitialize(); + void disconnect(); + + void set_target_joint_velocities(const VectorXd& target_joint_velocities); + + + + + + + /* To be implemented + std::tuple get_joint_limits() const; + void set_joint_limits(const std::tuple& joint_limits); + */ + + +}; + + diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h new file mode 100644 index 0000000..b7738e4 --- /dev/null +++ b/include/sas_robot_driver_franka.h @@ -0,0 +1,89 @@ +#pragma once + + +#include +#include +#include +#include +#include + +#include + +#include +#include "robot_interface_franka.h" + +using namespace DQ_robotics; +using namespace Eigen; + +namespace sas +{ + + +struct RobotDriverFrankaConfiguration +{ + std::string ip_address; + int port; + double speed; +}; + + +class RobotDriverFranka: public RobotDriver +{ +private: + RobotDriverFrankaConfiguration configuration_; + + std::shared_ptr robot_driver_interface_sptr_ = nullptr; + + //Joint positions + VectorXd joint_positions_; + //VectorXd joint_velocities_; + VectorXd end_effector_pose_; + std::vector joint_positions_buffer_; + std::vector end_effector_pose_euler_buffer_; + std::vector end_effector_pose_homogenous_transformation_buffer_; + + //DQ _homogenous_vector_to_dq(const VectorXd& homogenousvector) const; + //VectorXd _dq_to_homogenous_vector(const DQ& pose) const; + //VectorXd _get_end_effector_pose_homogenous_transformation(); + //double _sign(const double &a) const; + + // void _connect(); //Throws std::runtime_error() + + // void _motor_on(); //Throws std::runtime_error() + // void _motor_off() noexcept; //No exceptions should be thrown in the path to turn off the robot + + // void _set_speed(const float& speed, const float& acceleration, const float& deacceleration); //Throws std::runtime_error() + + // void _slave_mode_on(int mode); //Throws std::runtime_error() + //void _slave_mode_off() noexcept; //No exceptions should be thrown in the path to turn off the robot + +public: + const static int SLAVE_MODE_JOINT_CONTROL; + const static int SLAVE_MODE_END_EFFECTOR_CONTROL; + + RobotDriverFranka(const RobotDriverFranka&)=delete; + RobotDriverFranka()=delete; + ~RobotDriverFranka(); + + RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops); + + + VectorXd get_joint_positions() override; + void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override; + + VectorXd get_joint_velocities() override; + void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override; + + VectorXd get_joint_forces() override; + + void connect() override; + void disconnect() override; + + void initialize() override; + void deinitialize() override; + + bool set_end_effector_pose_dq(const DQ& pose); + DQ get_end_effector_pose_dq(); + +}; +} diff --git a/launch/sas_operator_side_receiver.launch b/launch/sas_operator_side_receiver.launch new file mode 100644 index 0000000..10bd709 --- /dev/null +++ b/launch/sas_operator_side_receiver.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/launch/sas_patient_side_manager.launch b/launch/sas_patient_side_manager.launch new file mode 100644 index 0000000..7f1b83a --- /dev/null +++ b/launch/sas_patient_side_manager.launch @@ -0,0 +1,8 @@ + + + + + vrep_ip: $(env VREP_IP) + + + diff --git a/launch/sas_robot_driver_franka_1.launch b/launch/sas_robot_driver_franka_1.launch new file mode 100644 index 0000000..ad661c2 --- /dev/null +++ b/launch/sas_robot_driver_franka_1.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..def03b9 --- /dev/null +++ b/package.xml @@ -0,0 +1,84 @@ + + + sas_robot_driver_franka + 0.0.0 + The sas_driver_franka package + + + + + moonshot + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + roscpp + rospy + std_msgs + sas_clock + sas_robot_driver + sas_common + sas_patient_side_manager + + roscpp + rospy + std_msgs + sas_clock + sas_robot_driver + sas_common + sas_patient_side_manager + + + roscpp + rospy + std_msgs + sas_clock + sas_robot_driver + sas_common + sas_patient_side_manager + + + + + + + + diff --git a/src/motion_generator.cpp b/src/motion_generator.cpp new file mode 100644 index 0000000..351225c --- /dev/null +++ b/src/motion_generator.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include "MotionGenerator.h" +#include +#include +#include +#include +#include + + +void setDefaultBehavior(franka::Robot& robot) { + robot.setCollisionBehavior( + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); + robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); +} + + +MotionGenerator::MotionGenerator(const double &speed_factor, const Eigen::VectorXd &q_goal) + { + _check_vector_size(q_goal); + q_goal_ = q_goal; + dq_max_ *= speed_factor; + ddq_max_start_ *= speed_factor; + ddq_max_goal_ *= speed_factor; + dq_max_sync_.setZero(); + q_start_.setZero(); + delta_q_.setZero(); + t_1_sync_.setZero(); + t_2_sync_.setZero(); + t_f_sync_.setZero(); + q_1_.setZero(); +} + +bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const { + Vector7i sign_delta_q; + sign_delta_q << delta_q_.cwiseSign().cast(); + Vector7d t_d = t_2_sync_ - t_1_sync_; + Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_; + std::array joint_motion_finished{}; + + for (size_t i = 0; i < 7; i++) { + if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) { + (*delta_q_d)[i] = 0; + joint_motion_finished[i] = true; + } else { + if (t < t_1_sync_[i]) { + (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * + (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0); + } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) { + (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i]; + } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) { + (*delta_q_d)[i] = + delta_q_[i] + 0.5 * + (1.0 / std::pow(delta_t_2_sync[i], 3.0) * + (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) * + std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) + + (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) * + dq_max_sync_[i] * sign_delta_q[i]; + } else { + (*delta_q_d)[i] = delta_q_[i]; + joint_motion_finished[i] = true; + } + } + } + return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), + [](bool x) { return x; }); +} + +void MotionGenerator::calculateSynchronizedValues() { + Vector7d dq_max_reach(dq_max_); + Vector7d t_f = Vector7d::Zero(); + Vector7d delta_t_2 = Vector7d::Zero(); + Vector7d t_1 = Vector7d::Zero(); + Vector7d delta_t_2_sync = Vector7d::Zero(); + Vector7i sign_delta_q; + sign_delta_q << delta_q_.cwiseSign().cast(); + + for (size_t i = 0; i < 7; i++) { + if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { + if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) + + 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) { + dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] * + (ddq_max_start_[i] * ddq_max_goal_[i]) / + (ddq_max_start_[i] + ddq_max_goal_[i])); + } + t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i]; + delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i]; + t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i]; + } + } + double max_t_f = t_f.maxCoeff(); + for (size_t i = 0; i < 7; i++) { + if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { + double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]); + double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i]; + double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i]; + double delta = b * b - 4.0 * a * c; + if (delta < 0.0) { + delta = 0.0; + } + dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); + t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i]; + delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i]; + t_f_sync_[i] = + (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]); + t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i]; + q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]); + } + } +} + +franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state, + franka::Duration period) { + time_ += period.toSec(); + + if (time_ == 0.0) { + q_start_ = Vector7d(robot_state.q_d.data()); + delta_q_ = q_goal_ - q_start_; + calculateSynchronizedValues(); + } + + Vector7d delta_q_d; + bool motion_finished = calculateDesiredValues(time_, &delta_q_d); + + std::array joint_positions; + Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d); + franka::JointPositions output(joint_positions); + output.motion_finished = motion_finished; + return output; +} + +void MotionGenerator::_check_vector_size(const Eigen::VectorXd &q) +{ + if(q.size() != 7) + { + throw std::runtime_error(std::string("Error in MotionGenerator. Input vector must have size 7")); + } +} diff --git a/src/quadratic_program_motion_generator.cpp b/src/quadratic_program_motion_generator.cpp new file mode 100644 index 0000000..a5016a0 --- /dev/null +++ b/src/quadratic_program_motion_generator.cpp @@ -0,0 +1,159 @@ +#include "QuadraticProgramMotionGenerator.h" + + +QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor, + const Eigen::VectorXd &q_initial, + const Eigen::VectorXd &q_dot_initial, + const Eigen::VectorXd &q_goal) +{ + n_links_ = _get_dimensions(q_initial, q_dot_initial, q_goal); + solver_ = std::make_unique(DQ_QPOASESSolver()); + constraints_manager_ = std::make_unique(ConstraintsManager(n_links_)); + _check_gains(); + q_ = q_initial; + q_dot_ = q_dot_initial; + + H_ = MatrixXd::Identity(n_links_, n_links_); + lb_ = VectorXd::Zero(n_links_); + ub_ = VectorXd::Zero(n_links_); + I_ = MatrixXd::Identity(n_links_, n_links_); + + _check_speed_factor(speed_factor); + speed_factor_ = speed_factor; + + q_dot_max_ *= speed_factor_; + q_dot_dot_max_ *= speed_factor_; + + K1_ = N1_.asDiagonal(); + K2_ = N2_.asDiagonal(); +} + +QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor, const VectorXd &q_dot_initial, const VectorXd &q_dot_goal) +{ + n_links_ = _get_dimensions(q_dot_initial, q_dot_initial, q_dot_goal); + solver_ = std::make_unique(DQ_QPOASESSolver()); + constraints_manager_ = std::make_unique(ConstraintsManager(n_links_)); + _check_gains(); + q_dot_ = q_dot_initial; + + H_ = MatrixXd::Identity(n_links_, n_links_); + lb_ = VectorXd::Zero(n_links_); + ub_ = VectorXd::Zero(n_links_); + I_ = MatrixXd::Identity(n_links_, n_links_); + + _check_speed_factor(speed_factor); + speed_factor_ = speed_factor; + + q_dot_max_ *= speed_factor_; + q_dot_dot_max_ *= speed_factor_; + + K1_ = N1_.asDiagonal(); + K2_ = N2_.asDiagonal(); +} + + +void QuadraticProgramMotionGenerator::_check_speed_factor(const double& speed_factor) const +{ + if (speed_factor > 1.0) + { + throw std::runtime_error("Speed factor must be <= 1.0"); + } + if (speed_factor <= 0.0) + { + throw std::runtime_error("Speed factor must be > 0.0"); + } +} + +int QuadraticProgramMotionGenerator::_get_dimensions(const Eigen::VectorXd &q1, + const Eigen::VectorXd &q2, + const Eigen::VectorXd &q3) const +{ + _check_sizes(q1, q2, q3); + return q1.size(); +} + + +void QuadraticProgramMotionGenerator::set_gains(const double& n1, const double& n2) +{ + n1_ = n1; + n2_ = n2; + _check_gains() ; + N1_ < 0. ")); + } +} + + +VectorXd QuadraticProgramMotionGenerator::compute_new_configuration(const VectorXd& q_goal, + const double& T) +{ + + f_ = K1_*(q_dot_ + K2_*(q_-q_goal)); //f_ = n1_*(q_dot_ + n2_*(q_-q_goal)); + for (int i=0; iadd_inequality_constraint(-I_, -lb_); + constraints_manager_->add_inequality_constraint( I_, ub_); + + + std::tie(A_, b_) = constraints_manager_->get_inequality_constraints(); + + auto u = solver_->solve_quadratic_program(H_, f_, A_, b_, Aeq_, beq_); + q_ = q_ +T*q_dot_ +0.5*std::pow(T,2)*u; + q_dot_ = q_dot_ + T*u; + + return q_; +} + +VectorXd QuadraticProgramMotionGenerator::compute_new_configuration_velocities(const VectorXd &q_dot_goal, const double &T) +{ + + f_ = 2*K1_*(q_dot_-q_dot_goal); + for (int i=0; iadd_inequality_constraint(-I_, -lb_); + constraints_manager_->add_inequality_constraint( I_, ub_); + + + std::tie(A_, b_) = constraints_manager_->get_inequality_constraints(); + + auto u = solver_->solve_quadratic_program(H_, f_, A_, b_, Aeq_, beq_); + q_dot_ = q_dot_ + T*u; + + return q_dot_; +} + diff --git a/src/robot_interface_franka.cpp b/src/robot_interface_franka.cpp new file mode 100644 index 0000000..3021e66 --- /dev/null +++ b/src/robot_interface_franka.cpp @@ -0,0 +1,789 @@ +#include "robot_interface_franka.h" + + +/** + * @brief robot_driver_franka::robot_driver_franka + * @param ROBOT_IP The IP address of the FCI + * @param mode The operation mode {None, PositionControl}. + * @param hand The hand option {ONFinished, OFF}. + */ +RobotInterfaceFranka::RobotInterfaceFranka(const std::string &ROBOT_IP, + const MODE& mode, + const HAND& hand):ip_(ROBOT_IP),mode_(mode) +{ + _set_driver_mode(mode); + if (hand == RobotInterfaceFranka::HAND::ON) + { + gripper_sptr_ = std::make_shared(ip_); + } + robot_sptr_ = std::make_shared(ip_); + _setDefaultRobotBehavior(); + /* + std::cout<<"---------------------------------------------------------------"< RobotInterfaceFranka::get_robot_pointer() +{ + return robot_sptr_; +} + + +/** + * @brief robot_driver_franka::_finish_echo_robot_state + */ +void RobotInterfaceFranka::_finish_echo_robot_state() +{ + _update_status_message("Finishing echo robot state.",verbose_); + finish_echo_robot_state_ = true; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); +} + + +/** + * @brief robot_driver_franka::deinitialize + */ +void RobotInterfaceFranka::deinitialize() +{ + _update_status_message("Deinitializing....",verbose_); + initialize_flag_ = false; + switch (mode_) + { + case RobotInterfaceFranka::MODE::None: + break; + case RobotInterfaceFranka::MODE::PositionControl: + finish_motion(); + break; + case RobotInterfaceFranka::MODE::VelocityControl: + finish_motion(); + break; + case RobotInterfaceFranka::MODE::ClearPositions: + throw std::runtime_error(std::string("The robot_driver_franka::ClearPositions is not available. ")); + break; + case RobotInterfaceFranka::MODE::Homing: + throw std::runtime_error(std::string("The robot_driver_franka::Homing is not available. ")); + break; + case RobotInterfaceFranka::MODE::ForceControl: + throw std::runtime_error(std::string("The robot_driver_franka::ForceControl is not available. ")); + break; + } + _update_status_message("Deinitialized.",verbose_); +} + + +/** + * @brief robot_driver_franka::initialize + */ +void RobotInterfaceFranka::initialize() +{ + _update_status_message("Initializing....",verbose_); + + initialize_flag_ = true; + + + switch (mode_) + { + case RobotInterfaceFranka::MODE::None: + break; + case RobotInterfaceFranka::MODE::PositionControl: + _finish_echo_robot_state(); + _start_joint_position_control_thread(); + break; + case RobotInterfaceFranka::MODE::VelocityControl: + _finish_echo_robot_state(); + _start_joint_velocity_control_thread(); + break; + case RobotInterfaceFranka::MODE::ClearPositions: + throw std::runtime_error(std::string("The robot_driver_franka::ClearPositions is not available. ")); + break; + case RobotInterfaceFranka::MODE::Homing: + throw std::runtime_error(std::string("The robot_driver_franka::Homing is not available. ")); + break; + case RobotInterfaceFranka::MODE::ForceControl: + throw std::runtime_error(std::string("The robot_driver_franka::ForceControl is not available. ")); + break; + } + _update_status_message("Initialized.",verbose_); +} + + +/** + * @brief robot_driver_franka::_update_robot_state + * @param robot_state + * @param time + */ +void RobotInterfaceFranka::_update_robot_state(const franka::RobotState &robot_state, const double& time) +{ + current_joint_positions_array_ = robot_state.q_d; + current_joint_positions_ = Eigen::Map(current_joint_positions_array_.data(), 7); + + current_joint_velocities_array_ = robot_state.dq_d; + current_joint_velocities_ = Eigen::Map(current_joint_velocities_array_.data(), 7); + + current_joint_forces_array_ = robot_state.tau_J; + current_joint_forces_ = Eigen::Map(current_joint_forces_array_.data(), 7); + + robot_mode_ = robot_state.robot_mode; + time_ = time; +} + + +/** + * @brief robot_driver_franka::_start_echo_robot_state_mode + */ +void RobotInterfaceFranka::_start_echo_robot_state_mode(){ + double time = 0.0; + size_t count = 0; + double t1 = 0.0; + + try { + robot_sptr_->read([&t1, &count, &time, this](const franka::RobotState& robot_state) { + if (count == 0) + { + t1 = robot_state.time.toSec(); + } + time = robot_state.time.toSec() - t1; + _update_robot_state(robot_state, time); + count++; + return !finish_echo_robot_state_; + }); + _update_status_message("Finished echo_robot_state.",verbose_); + } + catch (franka::Exception const& e) { + std::cout << e.what() << std::endl; + } +} + + +/** + * @brief robot_driver_franka::_start_joint_position_control_mode + */ +void RobotInterfaceFranka::_start_joint_position_control_mode() +{ + std::array initial_position; + VectorXd target_position; + double time = 0.0; + + VectorXd q = VectorXd::Zero(7); + franka::RobotState state = robot_sptr_->readOnce(); + q = Map(state.q_d.data(), 7); + desired_joint_positions_ = q; + + VectorXd q_dot = VectorXd::Zero(7); + _update_status_message("Starting joint position control mode EXPERIMENTAL",verbose_); + trajectory_generator_sptr_ = + std::make_unique(1.0, q, q_dot, q); + + double n2 = 2; + double n1 = 1000*std::sqrt(n2); + VectorXd K2 = (VectorXd(7)<set_diagonal_gains(K1, K2); + + finish_motion_ = false; + + int retry_counter = 0; + int TIMEOUT_IN_MILISECONDS = 5000; + + try { + robot_sptr_->control( //------------------------------------------------------------ + [&initial_position, &time, this](const franka::RobotState& robot_state, + franka::Duration period) -> franka::JointPositions { + time += period.toSec(); + double T = period.toSec(); + + auto new_q = trajectory_generator_sptr_->compute_new_configuration(desired_joint_positions_, T); + + + if (time == 0.0) { + initial_position = robot_state.q_d; + new_q = Eigen::Map(initial_position.data(), 7); + } + + + franka::JointPositions output = {{new_q[0], new_q[1], + new_q[2], new_q[3], + new_q[4], new_q[5], + new_q[6]}}; + + if (time == 0.0) { + _update_status_message("joint position control mode running! ",verbose_); + } + + if (finish_motion_) { + _update_status_message("Motion finished",verbose_); + finish_motion_ = false; + return franka::MotionFinished(output); + }else + { + _update_robot_state(robot_state, time); + } + return output; + } + );//------------------------------------------------------------ + + } + catch (franka::Exception const& e) { + std::cout << e.what() << std::endl; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(TIMEOUT_IN_MILISECONDS)); + retry_counter++; + +} + + +/** + * @brief RobotInterfaceFranka::_start_joint_velocity_control_mode + */ +void RobotInterfaceFranka::_start_joint_velocity_control_mode() +{ + finish_motion_ = false; + + std::array initial_velocity; + + VectorXd q_dot_initial = VectorXd::Zero(7); + VectorXd q_dot = VectorXd::Zero(7); + desired_joint_velocities_ = VectorXd::Zero(7); + trajectory_generator_sptr_ = + std::make_unique(0.8, q_dot_initial, q_dot); + + double n2 = 1; + double n1 = 10*std::sqrt(n2); + VectorXd K2 = (VectorXd(7)<set_diagonal_gains(K1, K2); + _update_status_message("Starting joint velocity control mode EXPERIMENTAL",verbose_); + try { + double time = 0.0; + + robot_sptr_->control( + [&initial_velocity, &time, this](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointVelocities { + time += period.toSec(); + double T = period.toSec(); + + auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T); + + franka::JointVelocities velocities = {{new_u[0], new_u[1], + new_u[2], new_u[3], + new_u[4], new_u[5], + new_u[6]}}; + + if (time == 0.0) { + _update_status_message("joint velocity control mode running! ",verbose_); + } + + if (finish_motion_) { + _update_status_message("Motion finished",verbose_); + finish_motion_ = false; + return franka::MotionFinished(velocities); + }else + { + _update_robot_state(robot_state, time); + initial_velocity = robot_state.dq_d; + } + return velocities; + }); + } + catch (franka::Exception const& e) { + std::cout << e.what() << std::endl; + } +} + + + +/** + * @brief robot_driver_franka::_setDefaultRobotBehavior + */ +void RobotInterfaceFranka::_setDefaultRobotBehavior() +{ + robot_sptr_->setCollisionBehavior( + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + + robot_sptr_->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); + robot_sptr_->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); +} + + +/** + * @brief robot_driver_franka::_update_status_message + * @param message + * @param verbose + */ +void RobotInterfaceFranka::_update_status_message(const std::string &message, const bool& verbose) +{ + status_message_ = message; + std::string base = "RobotInterfaceFranka::"; + if(verbose) + { + std::cout<readOnce(); + q = Map(state.q_d.data(), 7); + q = _compute_recursive_mean(samples, q); + } + return q; +} + + +/** + * @brief robot_driver_franka::read_once_initial_positions + * @return + */ +VectorXd RobotInterfaceFranka::read_once_initial_positions() +{ + _check_if_robot_is_connected(); + VectorXd q = VectorXd::Zero(7); + _update_status_message("Reading initial joints...",verbose_); + franka::RobotState state = robot_sptr_->readOnce(); + q = Map(state.q_d.data(), 7); + return q; +} + + +/** + * @brief robot_driver_franka::move_robot_to_target_joint_positions + * @param q_target + */ +void RobotInterfaceFranka::move_robot_to_target_joint_positions(const VectorXd& q_target) +{ + _finish_echo_robot_state(); + _check_if_robot_is_connected(); + + if (!initialize_flag_) + { + throw std::runtime_error(std::string("Error in set_initial_robot_configuration. Driver not initialized. Use initialize(); ")); + } + MotionGenerator motion_generator(speed_factor_joint_position_controller_, q_target); + + try { + _update_status_message("Moving robot...", verbose_); + robot_sptr_->control(motion_generator); + } + catch (franka::Exception const& e) { + std::cout << e.what() << std::endl; + } + + _start_echo_robot_state_mode_thread(); + +} + + + +/** + * @brief robot_driver_franka::set_joint_target_positions + * @param set_target_joint_positions_rad + */ +void RobotInterfaceFranka::set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) +{ + desired_joint_positions_ = set_target_joint_positions_rad; +} + + +/** + * @brief robot_driver_franka::get_joint_target_positions + * @return + */ +VectorXd RobotInterfaceFranka::get_joint_target_positions() +{ + return desired_joint_positions_; +} + + +/** + * @brief robot_driver_franka::read_gripper + * @param gripper_state + * @return + */ +double RobotInterfaceFranka::read_gripper(const GRIPPER_STATES& gripper_state) +{ + _check_if_hand_is_connected(); + franka::GripperState state = gripper_sptr_->readOnce(); + switch (gripper_state) { + case RobotInterfaceFranka::GRIPPER_STATES::WIDTH: + return state.width; + break; + case RobotInterfaceFranka::GRIPPER_STATES::MAX_WIDTH: + return state.max_width; + break; + default: + throw std::runtime_error(std::string("Wrong argument in sas_robot_driver_franka::read_gripper. ")); + break; + } +} + +/** + * @brief robot_driver_franka::read_grasped_status + * @return + */ +bool RobotInterfaceFranka::read_grasped_status() +{ + _check_if_hand_is_connected(); + franka::GripperState state = gripper_sptr_->readOnce(); + return state.is_grasped; +} + + +/** + * @brief robot_driver_franka::set_gripper + * @param width + */ +void RobotInterfaceFranka::set_gripper(const double& width) +{ + _check_if_hand_is_connected(); + auto gripper_max_width = read_gripper(RobotInterfaceFranka::GRIPPER_STATES::MAX_WIDTH); + if (width > gripper_max_width) + { + throw std::runtime_error( + std::string("You used a width = ") + + std::to_string(width) + + std::string(". Maximum width allowed is ") + + std::to_string(gripper_max_width) + ); + } + gripper_sptr_->move(width, speed_gripper_); +} + + +/** + * @brief robot_driver_franka::gripper_homing + */ +void RobotInterfaceFranka::gripper_homing() +{ + _check_if_hand_is_connected(); + gripper_sptr_->homing(); +} + + +/** + * @brief robot_driver_franka::get_home_robot_configuration + * @return + */ +VectorXd RobotInterfaceFranka::get_home_robot_configuration() +{ + return q_home_configuration_; +} + + diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp new file mode 100644 index 0000000..e61d4f5 --- /dev/null +++ b/src/sas_robot_driver_franka.cpp @@ -0,0 +1,120 @@ +#include "sas_robot_driver_franka.h" +#include "sas_clock/sas_clock.h" +#include + +namespace sas +{ + RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops): + RobotDriver(break_loops), + configuration_(configuration) + { + joint_positions_.resize(7); + joint_velocities_.resize(7); + joint_forces_.resize(7); + end_effector_pose_.resize(7); + joint_positions_buffer_.resize(8,0); + end_effector_pose_euler_buffer_.resize(7,0); + end_effector_pose_homogenous_transformation_buffer_.resize(10,0); + std::cout<(configuration.ip_address, + RobotInterfaceFranka::MODE::VelocityControl, //None, PositionControl + RobotInterfaceFranka::HAND::ON); + } + + RobotDriverFranka::~RobotDriverFranka()=default; + + + /** + * @brief + * + */ + void RobotDriverFranka::connect() + { + robot_driver_interface_sptr_ ->connect(); + } + + + /** + * @brief + * + */ + void RobotDriverFranka::initialize() + { + robot_driver_interface_sptr_->initialize(); + } + + + /** + * @brief + * + */ + void RobotDriverFranka::deinitialize() + { + robot_driver_interface_sptr_->deinitialize(); + } + + + /** + * @brief + * + */ + void RobotDriverFranka::disconnect() + { + robot_driver_interface_sptr_->disconnect(); + } + + + /** + * @brief + * + * @return VectorXd + */ + VectorXd RobotDriverFranka::get_joint_positions() + { + return robot_driver_interface_sptr_->get_joint_positions(); + } + + + /** + * @brief + * + * @param desired_joint_positions_rad + */ + void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad) + { + robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad); + } + + /** + * @brief RobotDriverFranka::get_joint_velocities + * @return + */ + VectorXd RobotDriverFranka::get_joint_velocities() + { + joint_velocities_ = robot_driver_interface_sptr_->get_joint_velocities(); + return joint_velocities_; + } + + + /** + * @brief RobotDriverFranka::set_target_joint_velocities + * @param desired_joint_velocities + */ + void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities) + { + desired_joint_velocities_ = desired_joint_velocities; + robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities); + } + + /** + * @brief RobotDriverFranka::get_joint_forces + * @return + */ + VectorXd RobotDriverFranka::get_joint_forces() + { + joint_forces_ = robot_driver_interface_sptr_->get_joint_forces(); + return joint_forces_; + } + +} diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp new file mode 100644 index 0000000..7167746 --- /dev/null +++ b/src/sas_robot_driver_franka_node.cpp @@ -0,0 +1,90 @@ +//#include "ros/ros.h" +//#include "std_msgs/String.h" + +#include + +#include +#include +//#include +#include +#include +#include +#include "sas_robot_driver_franka.h" + + +/********************************************* + * SIGNAL HANDLER + * *******************************************/ +#include +static std::atomic_bool kill_this_process(false); +void sig_int_handler(int) +{ + kill_this_process = true; +} + + +int main(int argc, char **argv) +{ + if(signal(SIGINT, sig_int_handler) == SIG_ERR) + { + throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler."); + } + + ros::init(argc, argv, "sas_robot_driver_franka_node", ros::init_options::NoSigintHandler); + ROS_WARN("====================================================================="); + ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------"); + ROS_WARN("====================================================================="); + ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server."); + + + ros::NodeHandle nh; + sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration; + + sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address); + // sas::get_ros_param(nh,"/robot_port", robot_driver_franka_configuration.port); + //sas::get_ros_param(nh,"/robot_speed", robot_driver_franka_configuration.speed); + + //robot_driver_franka_configuration.ip_address = "172.16.0.2"; + //robot_driver_franka_configuration.port = 0; + //robot_driver_franka_configuration.speed = 0.0; + + sas::RobotDriverROSConfiguration robot_driver_ros_configuration; + + sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec); + sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min); + sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max); + + //auto qmin = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_min); + //auto qmax = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_max); + + + //std::vector q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895}; + //std::vector q_max = { 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895}; + + //robot_driver_ros_configuration.thread_sampling_time_nsec = 4000000; + //robot_driver_ros_configuration.q_min = q_min; + //robot_driver_ros_configuration.q_max = q_max; + + robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName(); + + try + { + ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot."); + sas::RobotDriverFranka robot_driver_franka(robot_driver_franka_configuration, + &kill_this_process); + //robot_driver_franka.set_joint_limits({qmin, qmax}); + ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS."); + sas::RobotDriverROS robot_driver_ros(nh, + &robot_driver_franka, + robot_driver_ros_configuration, + &kill_this_process); + robot_driver_ros.control_loop(); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what()); + } + + + return 0; +} diff --git a/src/teleop_franka_node.cpp b/src/teleop_franka_node.cpp new file mode 100644 index 0000000..1f63143 --- /dev/null +++ b/src/teleop_franka_node.cpp @@ -0,0 +1,235 @@ +#include + +#include +#include +#include +#include +#include +#include +//#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "franka_ros_interface.h" +#include + + +std::string config_vfi_path = "/home/moonshot/Documents/git/franka_emika/real_franka_developments/real_franka_developments/cfg/vfi_constraints.yaml"; + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "teleop_franka_node"); + auto nodehandle = ros::NodeHandle(); + sas::RobotKinematicsProvider pose1_provider(nodehandle, "/arm1_kinematics"); + + //auto franka1_ros = FrankaRosInterface(nodehandle, 50, "/franka_1"); + sas::RobotDriverInterface franka1_ros(nodehandle, "/franka_1"); + //########################################################################################## + + DQ_VrepInterface vi; + vi.connect("10.198.113.159", 19997,100,10); + std::cout << "Starting V-REP simulation..." << std::endl; + vi.start_simulation(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto franka_sim = FrankaEmikaPandaVrepRobot("Franka", std::make_shared(vi)); + auto robot = franka_sim.kinematics(); + DQ xoffset = (1 + E_*0.5*0.21*k_)*(cos((M_PI/4)/2)-k_*sin((M_PI/4)/2)); + robot.set_effector(xoffset); + + + + //########################################################################################### + auto manager = RobotConstraintsManager(std::make_shared(vi), + (std::static_pointer_cast(std::make_shared(robot))), + (std::static_pointer_cast(std::make_shared(franka_sim))), + config_vfi_path, + RobotConstraintsManager::ORDER::FIRST_ORDER); + //########################################################################################## + //#######------------------------Controller------------------------------------------####### + DQ_QPOASESSolver solver; + DQ_ClassicQPController controller + (std::static_pointer_cast(std::make_shared(robot)), + std::static_pointer_cast(std::make_shared(solver))); + controller.set_gain(20.0); + controller.set_damping(0.01); + controller.set_control_objective(DQ_robotics::Translation); //DistanceToPlane Translation + controller.set_stability_threshold(0.0001); + //########################################################################################## + + //######################################################################################### + DQ xd; + DQ x; + MatrixXd A; + VectorXd b; + VectorXd u; + VectorXd q_u = VectorXd::Zero(7); + VectorXd q; + + VectorXd q_dot_initial = VectorXd::Zero(7); + VectorXd q_dot = VectorXd::Zero(7); + + + + while (ros::ok()) { + if (franka1_ros.is_enabled()) + { + q = franka1_ros.get_joint_positions(); + break; + + } + ros::spinOnce(); + } + + + q_u = q; + + franka_sim.set_target_configuration_space_positions(q); + vi.set_object_pose("xd", robot.fkm(q)); + + /* + DQ_QPOASESSolver solver2; + MatrixXd H = MatrixXd::Identity(7, 7); + VectorXd f ; + VectorXd qvrep; + MatrixXd A_; + VectorXd b_; + + for (int i=0;i<3000;i++) + { + qvrep = franka_sim.get_configuration_space_positions(); + f = 2*0.1*(qvrep-q); + auto u_ = solver2.solve_quadratic_program(H, f, A_, b_, A_, b_); + franka_sim.set_target_configuration_space_velocities(u_); + std::cout<<"initialiazing..."<compute_new_configuration_velocities(u, T); + //franka_sim.set_target_configuration_space_velocities(new_u); + //franka1_ros.send_target_joint_positions(new_u); + + //auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(u, T); + //franka1_ros.send_target_joint_positions(q); + franka1_ros.send_target_joint_velocities(u); + + + + + + } + + ros::spinOnce(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::cout << "Stopping V-REP simulation..." << std::endl; + vi.stop_simulation(); + vi.disconnect(); + + return 0; + } + catch (const std::exception& e) { + vi.stop_simulation(); + vi.disconnect(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::cout << e.what() << std::endl; + return -1; + } +}