Added source files

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();
+    
+};
+}