Added source files

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]

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"

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]
+

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 <array>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <franka/control_types.h>
+#include <franka/duration.h>
+#include <franka/robot.h>
+#include <franka/robot_state.h>
+
+/**
+ * @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<double, 7, 1, Eigen::ColMajor>;
+    using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;
+
+    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();
+};

include/quadratic_program_motion_generator.h

@@ -0,0 +1,74 @@
+#pragma once
+#include <array>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+#include <iostream>
+#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
+#include <memory>
+#include "ConstraintsManager.h"
+
+
+using namespace DQ_robotics;
+
+class QuadraticProgramMotionGenerator
+{
+private:
+
+    using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
+
+    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)<<n1_, n1_, n1_, n1_, n1_, n1_, n1_).finished();
+    VectorXd N2_ = (VectorXd(7)<<n2_, n2_, n2_, n2_, n2_, n2_, n2_).finished();
+
+    MatrixXd K1_;
+    MatrixXd K2_;
+
+    MatrixXd H_;
+    VectorXd lb_;
+    VectorXd ub_;
+    MatrixXd A_;
+    VectorXd b_;
+    VectorXd f_;
+    MatrixXd Aeq_;
+    VectorXd beq_;
+    MatrixXd I_;
+
+    void   _check_sizes(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
+    int _get_dimensions(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
+    void _check_gains();
+    std::unique_ptr<DQ_QPOASESSolver> solver_;
+    std::unique_ptr<ConstraintsManager> 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);
+
+};
+
+

include/robot_interface_franka.h

@@ -0,0 +1,174 @@
+#pragma once
+
+#include <dqrobotics/DQ.h>
+#include <memory.h>
+#include <tuple>
+#include <exception>
+#include <vector>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <franka/exception.h>
+#include "MotionGenerator.h"
+#include <thread>
+#include "QuadraticProgramMotionGenerator.h"
+#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
+#include <atomic>
+
+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<double, 7, 1, Eigen::ColMajor>;
+    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<double, 7> current_joint_positions_array_;
+
+    VectorXd current_joint_velocities_;
+    std::array<double, 7> current_joint_velocities_array_;
+
+    VectorXd current_joint_forces_;
+    std::array<double, 7> 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<franka::Robot> robot_sptr_;
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+    std::unique_ptr<QuadraticProgramMotionGenerator> 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<bool> finish_motion_;
+
+    void _start_echo_robot_state_mode();
+    std::thread echo_robot_state_mode_thread_;
+    void _start_echo_robot_state_mode_thread();
+    std::atomic<bool> 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<franka::Robot> 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<VectorXd, VectorXd> get_joint_limits() const;
+    void set_joint_limits(const std::tuple<VectorXd, VectorXd>& joint_limits);
+    */
+
+
+};
+
+

include/sas_robot_driver_franka.h

@@ -0,0 +1,89 @@
+#pragma once
+
+
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+
+#include <dqrobotics/DQ.h>
+
+#include <sas_robot_driver/sas_robot_driver.h>
+#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<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
+
+    //Joint positions
+    VectorXd joint_positions_;
+    //VectorXd joint_velocities_;
+    VectorXd end_effector_pose_;
+    std::vector<double> joint_positions_buffer_;
+    std::vector<double> end_effector_pose_euler_buffer_;
+    std::vector<double> 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();
+    
+};
+}

launch/sas_operator_side_receiver.launch

@@ -0,0 +1,7 @@
+<launch>
+
+<rosparam file="$(find sas_robot_driver_franka)/cfg/sas_operator_side_receiver.yaml" command="load" ns="sas_operator_side_receiver"/>
+
+<node pkg="sas_operator_side_receiver" type="sas_operator_side_receiver_udp_node" name="sas_operator_side_receiver"/>
+
+</launch>

launch/sas_patient_side_manager.launch

@@ -0,0 +1,8 @@
+<launch>
+<node pkg="sas_patient_side_manager" type="sas_patient_side_manager_node" name="sas_patient_side_manager">
+<rosparam file="$(find sas_robot_driver_franka)/cfg/sas_patient_side_manager.yaml" command="load"/>
+<rosparam subst_value="true">
+    vrep_ip: $(env VREP_IP)
+</rosparam>
+</node>
+</launch>

launch/sas_robot_driver_franka_1.launch

@@ -0,0 +1,5 @@
+<launch>
+<rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_franka_1.yaml" command="load"/>
+<node pkg="sas_robot_driver_franka" type="sas_robot_driver_franka_node" name="franka_1">
+</node>
+</launch>

package.xml

@@ -0,0 +1,84 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>sas_robot_driver_franka</name>
+  <version>0.0.0</version>
+  <description>The sas_driver_franka package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="moonshot@todo.todo">moonshot</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/sas_driver_franka</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sas_clock</build_depend>
+  <build_depend>sas_robot_driver</build_depend>
+  <build_depend>sas_common</build_depend>
+  <build_depend>sas_patient_side_manager</build_depend>
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sas_clock</build_export_depend>
+  <build_export_depend>sas_robot_driver</build_export_depend>
+  <build_export_depend>sas_common</build_export_depend>
+  <build_export_depend>sas_patient_side_manager</build_export_depend>
+
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sas_clock</exec_depend>
+  <exec_depend>sas_robot_driver</exec_depend>
+  <exec_depend>sas_common</exec_depend>
+  <exec_depend>sas_patient_side_manager</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

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 <algorithm>
+#include <array>
+#include <cmath>
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+
+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<int>();
+    Vector7d t_d = t_2_sync_ - t_1_sync_;
+    Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
+    std::array<bool, 7> 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<int>();
+
+    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<double, 7> 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"));
+    }
+}

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>(DQ_QPOASESSolver());
+    constraints_manager_ = std::make_unique<ConstraintsManager>(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>(DQ_QPOASESSolver());
+    constraints_manager_ = std::make_unique<ConstraintsManager>(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_ <<n1_, n1_, n1_, n1_, n1_, n1_, n1_;
+    N2_ <<n2_, n2_, n2_, n2_, n2_, n2_, n2_;
+    K1_ = N1_.asDiagonal();
+    K2_ = N2_.asDiagonal();
+}
+
+void QuadraticProgramMotionGenerator::set_diagonal_gains(const VectorXd& K1, const VectorXd& K2)
+{
+    K1_ = K1.asDiagonal();
+    K2_ = K2.asDiagonal();
+}
+
+void QuadraticProgramMotionGenerator::_check_sizes(const Eigen::VectorXd &q1,
+                                                   const Eigen::VectorXd &q2,
+                                                   const Eigen::VectorXd &q3) const
+{
+    if (q1.size() != q2.size() or q1.size() != q3.size() or q2.size() != q3.size())
+    {
+        throw std::runtime_error(std::string("Wrong sizes in vectors. "));
+    }
+}
+
+void QuadraticProgramMotionGenerator::_check_gains()
+{
+    if ( std::pow(n1_, 2) - 4*n2_ <= 0)
+    {
+        throw std::runtime_error(std::string("Wrong gains!!!. You need to use gains such that n1^2 - 4*n2 > 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; i<n_links_;i++)
+    {
+          lb_[i] =   std::max( -q_dot_dot_max_[i], (1/T)*(-q_dot_max_[i] - q_dot_[i]));
+          ub_[i] =   std::min(  q_dot_dot_max_[i], (1/T)*( q_dot_max_[i] - q_dot_[i]));
+    }
+
+
+    constraints_manager_->add_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; i<n_links_;i++)
+    {
+          lb_[i] =   std::max( -q_dot_dot_max_[i], (1/T)*(-q_dot_max_[i] - q_dot_[i]));
+          ub_[i] =   std::min(  q_dot_dot_max_[i], (1/T)*( q_dot_max_[i] - q_dot_[i]));
+    }
+
+
+    constraints_manager_->add_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_;
+}
+

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<franka::Gripper>(ip_);
+    }
+    robot_sptr_ = std::make_shared<franka::Robot>(ip_);
+    _setDefaultRobotBehavior();
+    /*
+    std::cout<<"---------------------------------------------------------------"<<std::endl;
+    std::cout<<"The connection is not automatic. Expected Workflow:            "<<std::endl;
+    std::cout<<"franka_driver.connect();                                       "<<std::endl;
+    std::cout<<"franka_driver.initialize();                                    "<<std::endl;
+    std::cout<<"                                                               "<<std::endl;
+    std::cout<<"franka_driver.deinitialize();                                  "<<std::endl;
+    std::cout<<"franka_driver.disconnect();                                    "<<std::endl;
+    std::cout<<"---------------------------------------------------------------"<<std::endl;
+    */
+
+
+    std::cout<<"-----------------------------------------------------------------"<<std::endl;
+    std::cout<<"RobotInterfaceFranka is brought to you by Juan Jose Quiroz Omana."<<std::endl;
+    std::cout<<"                                                                 "<<std::endl;
+    std::cout<<"-----------------------------------------------------------------"<<std::endl;
+    finish_motion_ = false;
+    finish_echo_robot_state_ = false;
+}
+
+
+/**
+ * @brief robot_driver_franka::_set_driver_mode sets the mode.
+ * @param mode
+ */
+void RobotInterfaceFranka::_set_driver_mode(const RobotInterfaceFranka::MODE& mode)
+{
+    switch (mode)
+    {
+    case RobotInterfaceFranka::MODE::None:
+        mode_ = mode;
+        break;
+    case RobotInterfaceFranka::MODE::PositionControl:
+        mode_ = mode;
+        break;
+    case RobotInterfaceFranka::MODE::VelocityControl:
+        mode_ = mode;
+        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;
+    }
+}
+
+
+/**
+ * @brief robot_driver_franka::connect starts the connection with the robot and starts the robot state thread.
+ */
+void RobotInterfaceFranka::connect()
+{
+    _update_status_message("Connecting...", verbose_);
+    current_joint_positions_ = read_once_initial_positions(); //_read_once_smooth_initial_positions(1000);
+    desired_joint_positions_ = current_joint_positions_;
+    desired_joint_velocities_ = VectorXd::Zero(7);
+    _update_status_message("Connected.", verbose_);
+    _start_echo_robot_state_mode_thread(); //  start_robot_state_loop to read joints without moving the robot;
+
+}
+
+
+/**
+ * @brief robot_driver_franka::disconnect stops all loops threads. Use this method after to run deinitialize().
+ */
+void RobotInterfaceFranka::disconnect()
+{
+    _update_status_message("Disconnecting...", verbose_);
+
+    _finish_echo_robot_state();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+    if (joint_position_control_mode_thread_.joinable())
+    {
+        joint_position_control_mode_thread_.join();
+    }
+    if (echo_robot_state_mode_thread_.joinable())
+    {
+        echo_robot_state_mode_thread_.join();
+    }
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        joint_velocity_control_mode_thread_.join();
+    }
+
+    _update_status_message("Disconnected.", verbose_);
+    _restart_default_parameters();
+
+
+}
+
+void RobotInterfaceFranka::set_target_joint_velocities(const VectorXd &target_joint_velocities)
+{
+    desired_joint_velocities_ = target_joint_velocities;
+}
+
+
+/**
+ * @brief robot_driver_franka::_restart_default_parameters
+ */
+void RobotInterfaceFranka::_restart_default_parameters()
+{
+    finish_motion_           = false;
+    finish_echo_robot_state_ = false;
+    initialize_flag_         = false;
+    time_ = 0;
+}
+
+
+/**
+ * @brief robot_driver_franka::_compute_recursive_mean computes the recursive mean of a vector
+ * @param n The number of samples
+ * @param q The vector
+ * @return The recursive mean of n samples.
+ */
+VectorXd RobotInterfaceFranka::_compute_recursive_mean(const double& n, const VectorXd& q)
+{
+    //Vector7d qm;
+    Vector7d qmean;
+    for (int i=0;i<q.size();i++)
+    {
+        //qm[i] = (1/n)*((n-1)*q[i] + q[i]);
+        qmean[i]  = (1/n)*((n-1)*q[i] + q[i]);
+    }
+    return qmean;
+}
+
+
+/**
+ * @brief robot_driver_franka::_start_joint_position_control_thread
+ */
+void RobotInterfaceFranka::_start_joint_position_control_thread()
+{
+    finish_motion_ = false;
+    _update_status_message("checking joint position control thread",verbose_);
+    if (joint_position_control_mode_thread_.joinable())
+    {
+        joint_position_control_mode_thread_.join();
+    }
+    _update_status_message("Starting joint position control thread",verbose_);
+    joint_position_control_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_joint_position_control_mode, this);
+}
+
+
+
+void RobotInterfaceFranka::_start_joint_velocity_control_thread()
+{
+    finish_motion_ = false;
+    _update_status_message("checking joint velocity control thread",verbose_);
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        joint_velocity_control_mode_thread_.join();
+    }
+    _update_status_message("Starting joint velocity control thread",verbose_);
+    joint_velocity_control_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_joint_velocity_control_mode, this);
+}
+
+/**
+ * @brief robot_driver_franka::_start_echo_robot_state_mode_thread
+ */
+void RobotInterfaceFranka::_start_echo_robot_state_mode_thread()
+{
+    finish_echo_robot_state_ = false;
+    _update_status_message("Checking echo robot state thread",verbose_);
+    if (echo_robot_state_mode_thread_.joinable())
+    {
+        echo_robot_state_mode_thread_.join();
+    }
+    _update_status_message("Starting echo robot state thread",verbose_);
+    echo_robot_state_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_echo_robot_state_mode, this);
+}
+
+
+/**
+ * @brief robot_driver_franka::finish_motion
+ */
+void RobotInterfaceFranka::finish_motion()
+{
+    _update_status_message("Ending control loop.",verbose_);
+    for (int i=0;i<1000;i++)
+    {
+        set_target_joint_positions(current_joint_positions_);
+        set_target_joint_velocities(VectorXd::Zero(7));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    finish_motion_ = true;
+}
+
+
+/**
+ * @brief robot_driver_franka::get_robot_pointer
+ * @return
+ */
+std::shared_ptr<franka::Robot> 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<VectorXd>(current_joint_positions_array_.data(), 7);
+
+    current_joint_velocities_array_ = robot_state.dq_d;
+    current_joint_velocities_ = Eigen::Map<VectorXd>(current_joint_velocities_array_.data(), 7);
+
+    current_joint_forces_array_ = robot_state.tau_J;
+    current_joint_forces_ =  Eigen::Map<VectorXd>(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<double, 7> initial_position;
+    VectorXd target_position;
+    double time = 0.0;
+
+    VectorXd q = VectorXd::Zero(7);
+    franka::RobotState state = robot_sptr_->readOnce();
+    q = Map<VectorXd>(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<QuadraticProgramMotionGenerator>(1.0, q, q_dot, q);
+
+    double n2 = 2;
+    double n1 = 1000*std::sqrt(n2);
+    VectorXd K2 = (VectorXd(7)<<n2, n2, n2, n2, n2, n2, n2).finished();
+    VectorXd K1 = (VectorXd(7)<<n1, n1, n1, n1, 3*n1, 3*n1, 3*n1).finished();
+    trajectory_generator_sptr_->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<VectorXd>(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<double, 7> 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<QuadraticProgramMotionGenerator>(0.8, q_dot_initial, q_dot);
+
+    double n2 = 1;
+    double n1 = 10*std::sqrt(n2);
+    VectorXd K2 = (VectorXd(7)<<n2, n2, n2, n2, n2, n2, n2).finished();
+    VectorXd K1 = (VectorXd(7)<<n1, n1, n1, n1, 2*n1, 2*n1, 2*n1).finished();
+    trajectory_generator_sptr_->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<<base+message<<std::endl;
+    }
+}
+
+
+/**
+ * @brief robot_driver_franka::get_status_message
+ * @return
+ */
+std::string RobotInterfaceFranka::get_status_message()
+{
+    return status_message_;
+}
+
+
+/**
+ * @brief RobotInterfaceFranka::get_robot_mode
+ * @return
+ */
+std::string RobotInterfaceFranka::get_robot_mode()
+{
+    std::string string_robot_mode;
+    switch (robot_mode_) {
+    case (franka::RobotMode::kUserStopped):
+        string_robot_mode = "User stopped";
+        break;
+    case (franka::RobotMode::kIdle):
+        string_robot_mode = "Idle";
+        break;
+    case (franka::RobotMode::kMove):
+        string_robot_mode = "Move";
+        break;
+    case (franka::RobotMode::kGuiding):
+        string_robot_mode = "Guiding";
+        break;
+    case (franka::RobotMode::kReflex):
+        string_robot_mode = "Reflex";
+        break;
+    case (franka::RobotMode::kAutomaticErrorRecovery):
+        string_robot_mode = "Automatic error recovery";
+        break;
+    case (franka::RobotMode::kOther):
+         string_robot_mode = "Other";
+        break;
+
+    }
+    return  string_robot_mode;
+}
+
+
+/**
+ * @brief robot_driver_franka::_check_if_robot_is_connected
+ */
+void RobotInterfaceFranka::_check_if_robot_is_connected()
+{
+    if(!robot_sptr_)
+        throw std::runtime_error("Invalid robot pointer. You must connect(), then initialize(). "
+                   + std::string("Example:  robot_driver_franka(IP, robot_driver_franka::MODE::None, robot_driver_franka::HAND::ON)" ));
+}
+
+
+/**
+  * @brief robot_driver_franka::_check_if_hand_is_connected
+  */
+void RobotInterfaceFranka::_check_if_hand_is_connected()
+{
+    if(!gripper_sptr_)
+        throw std::runtime_error("Invalid hand pointer. You must connect(), then initialize(). "
+                   + std::string("Example:  robot_driver_franka(IP, robot_driver_franka::MODE::None, robot_driver_franka::HAND::ON)" ));
+}
+
+
+/**
+ * @brief robot_driver_franka::get_joint_positions
+ * @return
+ */
+VectorXd RobotInterfaceFranka::get_joint_positions()
+{
+    _check_if_robot_is_connected();
+    return current_joint_positions_;
+}
+
+
+/**
+ * @brief robot_driver_franka::get_joint_velocities
+ * @return
+ */
+VectorXd RobotInterfaceFranka::get_joint_velocities()
+{
+    _check_if_robot_is_connected();
+    return current_joint_velocities_;
+}
+
+
+/**
+ * @brief RobotInterfaceFranka::get_joint_forces
+ * @return
+ */
+VectorXd RobotInterfaceFranka::get_joint_forces()
+{
+    _check_if_robot_is_connected();
+    return current_joint_forces_;
+}
+
+
+/**
+ * @brief robot_driver_franka::get_time
+ * @return
+ */
+double RobotInterfaceFranka::get_time()
+{
+    _check_if_robot_is_connected();
+    return time_;
+}
+
+
+/**
+ * @brief robot_driver_franka::_read_once_smooth_initial_positions
+ * @param samples
+ * @return
+ */
+VectorXd RobotInterfaceFranka::_read_once_smooth_initial_positions(const double& samples)
+{
+    _check_if_robot_is_connected();
+    VectorXd q = VectorXd::Zero(7);
+    _update_status_message("Reading smooth initial joints...",verbose_);
+    for (int i=0;i<samples;i++ )
+    {
+        franka::RobotState state = robot_sptr_->readOnce();
+        q = Map<VectorXd>(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<VectorXd>(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_;
+}
+
+

src/sas_robot_driver_franka.cpp

@@ -0,0 +1,120 @@
+#include "sas_robot_driver_franka.h"
+#include "sas_clock/sas_clock.h"
+#include <dqrobotics/utils/DQ_Math.h>
+
+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<<std::endl;
+
+        robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(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_;
+    }
+
+}

src/sas_robot_driver_franka_node.cpp

@@ -0,0 +1,90 @@
+//#include "ros/ros.h"
+//#include "std_msgs/String.h"
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <sas_common/sas_common.h>
+#include <sas_conversions/eigen3_std_conversions.h>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include "sas_robot_driver_franka.h"
+
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+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<double> q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895};
+    //std::vector<double> 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;
+}

src/teleop_franka_node.cpp

@@ -0,0 +1,235 @@
+#include <ros/ros.h>
+
+#include <sas_common/sas_common.h>
+#include <sas_robot_kinematics/sas_robot_kinematics_provider.h>
+#include <sas_robot_driver/sas_robot_driver_interface.h>
+#include <iostream>
+#include <Eigen/Dense>
+#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
+//#include <dqrobotics/robots/FrankaEmikaPandaMDHRobot.h>
+#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
+//#include <dqrobotics/solvers/DQ_SerialManipulatorDynamicsGaussPrincipleSolver.h>
+#include <dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h>
+#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
+#include <dqrobotics/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.h>
+#include <thread>
+#include <dqrobotics/DQ.h>
+#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
+#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
+#include <chrono>
+#include <yaml-cpp/yaml.h>
+#include <RobotConstraintsManager.h>
+#include "franka_ros_interface.h"
+#include <QuadraticProgramMotionGenerator.h>
+
+
+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<DQ_VrepInterface>(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<DQ_VrepInterface>(vi),
+                                           (std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot))),
+                                           (std::static_pointer_cast<DQ_VrepRobot>(std::make_shared<FrankaEmikaPandaVrepRobot>(franka_sim))),
+                                           config_vfi_path,
+                                           RobotConstraintsManager::ORDER::FIRST_ORDER);
+    //##########################################################################################
+    //#######------------------------Controller------------------------------------------#######
+    DQ_QPOASESSolver solver;
+    DQ_ClassicQPController controller
+        (std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot)),
+         std::static_pointer_cast<DQ_QuadraticProgrammingSolver>(std::make_shared<DQ_QPOASESSolver>(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..."<<std::endl;
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    */
+
+
+    std::cout<<"-------------------------"<<std::endl;
+    std::cout<<"Initial q: "<<std::endl;
+    std::cout<<q.transpose()<<std::endl;
+    std::cout<<"-------------------------"<<std::endl;
+    double T = 0.001;
+    try {
+
+        while(ros::ok())
+        {
+
+            if (q.size() == 7)
+            {
+                q = franka1_ros.get_joint_positions();
+                franka_sim.set_target_configuration_space_positions(q);
+                //q = franka_sim.get_configuration_space_positions();
+                x = robot.fkm(q);
+                vi.set_object_pose("effector", x);
+
+                if(not pose1_provider.is_enabled())
+                {
+                    pose1_provider.send_pose(x);
+
+                }
+                pose1_provider.send_reference_frame(robot.get_reference_frame());
+
+
+                try{
+
+                    xd =  pose1_provider.get_desired_pose();
+                    pose1_provider.send_pose(xd);
+
+                }
+                catch(const std::exception& e){
+                    xd = vi.get_object_pose("xd");
+                    std::cout<<"Nope"<<std::endl;
+                    std::cout << e.what() << std::endl;
+                }
+
+                xd = vi.get_object_pose("xd");
+
+                std::tie(A, b) = manager.get_inequality_constraints(q);
+                controller.set_inequality_constraint(A,b);
+
+
+
+                //-----------------------------------------------------------
+                switch(controller.get_control_objective())
+                {
+                case ControlObjective::Distance:
+                {
+                    VectorXd pdesired = vec4(translation(xd));
+                    VectorXd distance(1);
+                    distance(0) = pdesired.transpose()*pdesired;
+                    u = controller.compute_setpoint_control_signal(q, pdesired.transpose()*pdesired);
+                    break;
+                }
+                case ControlObjective::Translation:
+                {
+                    u = controller.compute_setpoint_control_signal(q, vec4(xd.translation()));
+                    break;
+                }
+                case ControlObjective::Pose:
+                {
+                    u = controller.compute_setpoint_control_signal(q, vec8(xd));
+                    break;
+                }
+                case ControlObjective::DistanceToPlane:
+                {
+                    DQ plane_normal = xd.P()*k_*xd.P().conj();
+                    DQ plane_point =  xd.translation();
+                    DQ xdesired_plane = (plane_normal + E_ * dot(plane_point, plane_normal));
+                    controller.set_target_primitive(xdesired_plane);
+                    u = controller.compute_setpoint_control_signal(q, VectorXd::Zero(1)); //DistanceToPlane
+                    break;
+                }
+                default:
+                    throw std::runtime_error("ControlObjective not suppported.");
+                    break;
+                }
+                //q = q + T*u;
+                //std::cout<<"error: "<<std::endl;
+                //std::cout<<controller.get_last_error_signal().norm()<<std::endl;
+
+                //franka_sim.set_target_configuration_space_positions(franka1_ros.get_joint_positions());
+                //auto new_u = trajectory_generator_sptr_->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;
+    }
+}