From fac3db693e17097c37569096102f1e100842e2cc Mon Sep 17 00:00:00 2001
From: Juancho <juanjqogm@gmail.com>
Date: Tue, 27 Jun 2023 15:09:17 +0900
Subject: [PATCH] Update the interface to use the new motion generation class

---
 include/custom_motion_generation.h |  7 ++++
 src/custom_motion_generation.cpp   | 57 ++++++++++++++++++++++++++++++
 src/robot_interface_franka.cpp     | 11 +++++-
 3 files changed, 74 insertions(+), 1 deletion(-)

diff --git a/include/custom_motion_generation.h b/include/custom_motion_generation.h
index 69d1029..1696310 100644
--- a/include/custom_motion_generation.h
+++ b/include/custom_motion_generation.h
@@ -83,14 +83,21 @@ private:
     int _get_dimensions(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
     void _check_speed_factor(const double& speed_factor) const;
 
+
+
 public:
     CustomMotionGeneration(const double &speed_factor,
                            const Eigen::VectorXd &q_initial,
                            const Eigen::VectorXd &q_dot_initial,
                            const Eigen::VectorXd &q_goal);
 
+    CustomMotionGeneration(const double &speed_factor,
+                           const Eigen::VectorXd &q_dot_initial,
+                           const Eigen::VectorXd &q_dot_goal);
+
     void set_proportional_gain(const double& gain);
 
     VectorXd compute_new_configuration(const VectorXd& q_goal, const double& T);
+    VectorXd compute_new_configuration_velocities(const VectorXd& q_dot_goal, const double& T);
 };
 
diff --git a/src/custom_motion_generation.cpp b/src/custom_motion_generation.cpp
index f2fdcc2..b6c84b8 100644
--- a/src/custom_motion_generation.cpp
+++ b/src/custom_motion_generation.cpp
@@ -59,6 +59,34 @@ CustomMotionGeneration::CustomMotionGeneration(const double &speed_factor,
     q_dot_dot_max_ *= speed_factor_;
 }
 
+
+/**
+ * @brief CustomMotionGeneration::CustomMotionGeneration
+ * @param speed_factor
+ * @param q_dot_initial
+ * @param q_dot_goal
+ */
+CustomMotionGeneration::CustomMotionGeneration(const double &speed_factor, const VectorXd &q_dot_initial, const VectorXd &q_dot_goal)
+{
+    n_links_ = _get_dimensions(q_dot_initial, q_dot_goal, q_dot_goal);
+    solver_ = std::make_unique<DQ_QPOASESSolver>();
+    constraints_manager_ = std::make_unique<ConstraintsManager>(n_links_);
+    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_;
+
+}
+
 /**
  * @brief CustomMotionGeneration::set_proportional_gain
  * @param gain
@@ -100,6 +128,7 @@ int CustomMotionGeneration::_get_dimensions(const Eigen::VectorXd &q1,
 }
 
 
+
 /**
  * @brief CustomMotionGeneration::_check_speed_factor
  * @param speed_factor
@@ -179,3 +208,31 @@ VectorXd CustomMotionGeneration::compute_new_configuration(const VectorXd &q_goa
 
     return q_;
 }
+
+/**
+ * @brief CustomMotionGeneration::compute_new_configuration_velocities
+ * @param q_dot_goal
+ * @param T
+ * @return
+ */
+VectorXd CustomMotionGeneration::compute_new_configuration_velocities(const VectorXd &q_dot_goal, const double &T)
+{
+    f_ = 2*gain_*(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_;
+}
diff --git a/src/robot_interface_franka.cpp b/src/robot_interface_franka.cpp
index 0e285e6..f0239fe 100644
--- a/src/robot_interface_franka.cpp
+++ b/src/robot_interface_franka.cpp
@@ -473,6 +473,8 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
     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);
 
@@ -481,6 +483,12 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
     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);
+    */
+
+    custom_generator_sptr_ = std::make_unique<CustomMotionGeneration>(0.8, q_dot_initial, q_dot);
+    custom_generator_sptr_->set_proportional_gain(15.0);
+
+
     _update_status_message("Starting joint velocity control mode EXPERIMENTAL",verbose_);
     try {
         double time = 0.0;
@@ -490,7 +498,8 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
                 time += period.toSec();
                 double T = period.toSec();
 
-                auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T);
+                //auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T);
+                auto new_u = custom_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],