Added source files

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