75 lines
2.2 KiB
C++
75 lines
2.2 KiB
C++
#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);
|
|
|
|
};
|
|
|
|
|