From 39b67d26e7a190e0890ef0f58f4e0afc49db1336 Mon Sep 17 00:00:00 2001
From: Lucas Joseph <lucas.joseph@inria.fr>
Date: Tue, 4 Apr 2023 16:23:40 +0200
Subject: [PATCH] Remove log not used
---
src/controller.cpp | 37 -------------------------------------
1 file changed, 37 deletions(-)
diff --git a/src/controller.cpp b/src/controller.cpp
index 029b8af..7c8dc0d 100755
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -68,49 +68,12 @@ Eigen::VectorXd Acceleration::update(Eigen::VectorXd q, Eigen::VectorXd qdot, Ei
( (-model.velocityLimit - qdot_d)/horizon_dt ).cwiseMax
( 2.0 * ( model.lowerPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt) );
- // qp.ub = q2dot_max;
- // qp.lb = -q2dot_max;
-
- // qp.a_constraints.block(0, 0, model.nv, model.nv) = Eigen::MatrixXd::Identity(model.nv,model.nv);
- // qp.a_constraints.block(model.nv, 0, model.nv, model.nv) = Eigen::MatrixXd::Identity(model.nv,model.nv);
- // qp.a_constraints.block(2*model.nv, 0, model.nv, model.nv) = Eigen::MatrixXd::Identity(model.nv,model.nv);
-
- // qp.lb_a.block(0, 0, model.nv, 1) = -q3dot_max * horizon_dt + q2dot_d;
- // qp.lb_a.block(model.nv, 0, model.nv, 1) = (-model.velocityLimit - qdot_d)/horizon_dt;
- // qp.lb_a.block(2*model.nv, 0, model.nv, 1) = 2.0 * ( model.lowerPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt);
-
- // qp.ub_a.block(0, 0, model.nv, 1) = q3dot_max * horizon_dt + q2dot_d;
- // qp.ub_a.block(model.nv, 0, model.nv, 1) = (model.velocityLimit - qdot_d)/horizon_dt;
- // qp.ub_a.block(2*model.nv, 0, model.nv, 1) = 2.0 * ( model.upperPositionLimit - q - qdot_d * horizon_dt ) / (horizon_dt * horizon_dt);
-
qp_solution = qp_solver.SolveQP(qp);
- q3dot_constraint = (qp_solution - q2dot_d) / horizon_dt;
- q2dot_constraint = qp_solution;
- qdot_constraint = qdot_d + qp_solution * horizon_dt;
- q_constraint = q + qdot_d * horizon_dt + 0.5 * qp_solution * horizon_dt * horizon_dt;
return qp_solution;
}
-void Acceleration::getQPConstraints(Eigen::VectorXd& q3dot_constraint, Eigen::VectorXd& q2dot_constraint, Eigen::VectorXd& qdot_constraint, Eigen::VectorXd& q_constraint )
-{
- q3dot_constraint = this->q3dot_constraint;
- q2dot_constraint = this->q2dot_constraint;
- qdot_constraint = this->qdot_constraint;
- q_constraint = this->q_constraint;
-}
-
-void Acceleration::getQPLimits(Eigen::VectorXd& q3dot_lim, Eigen::VectorXd& q2dot_lim, Eigen::VectorXd& qdot_lim, Eigen::VectorXd& q_lim_lower, Eigen::VectorXd& q_lim_upper )
-{
- q3dot_lim= this->q3dot_max;
- q2dot_lim= this->q2dot_max;
- qdot_lim= this->model.velocityLimit;
- q_lim_lower = model.lowerPositionLimit;
- q_lim_upper = model.upperPositionLimit;
-}
-
-
bool Acceleration::load_robot_model(std::string robot_description, std::vector<std::string> joint_names)
{
// Load the urdf model
--
GitLab