Remove log not used

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