task-joint-posVelAcc-bounds.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#ifndef __invdyn_task_joint_posVelAcc_bounds_hpp__
#define __invdyn_task_joint_posVelAcc_bounds_hpp__

#include <tsid/tasks/task-motion.hpp>
#include <tsid/math/constraint-bound.hpp>
#include <tsid/math/constraint-inequality.hpp>
#include <tsid/deprecated.hh>

namespace tsid {
namespace tasks {

class TaskJointPosVelAccBounds : public TaskMotion {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef math::Vector Vector;
 typedef math::ConstraintBound ConstraintBound;
 typedef math::ConstraintInequality ConstraintInequality;
 typedef math::VectorXi VectorXi;
 typedef pinocchio::Data Data;

 TaskJointPosVelAccBounds(const std::string& name, RobotWrapper& robot,
 double dt, bool verbose = true);

 int dim() const override;

 const ConstraintBase& compute(double t, ConstRefVector q, ConstRefVector v,
 Data& data) override;

 const ConstraintBase& getConstraint() const override;

 void setTimeStep(double dt);
 void setPositionBounds(ConstRefVector lower, ConstRefVector upper);
 void setVelocityBounds(ConstRefVector upper);
 void setAccelerationBounds(ConstRefVector upper);
 const Vector& getAccelerationBounds() const;
 const Vector& getVelocityBounds() const;
 const Vector& getPositionLowerBounds() const;
 const Vector& getPositionUpperBounds() const;

 void setVerbose(bool verbose);

 void setImposeBounds(bool impose_position_bounds, bool impose_velocity_bounds,
 bool impose_viability_bounds,
 bool impose_acceleration_bounds);

 void isStateViable(ConstRefVector q, ConstRefVector dq, bool verbose = true);

 void computeAccLimitsFromPosLimits(ConstRefVector q, ConstRefVector dq,
 bool verbose = true);

 void computeAccLimitsFromViability(ConstRefVector q, ConstRefVector dq,
 bool verbose = true);

 void computeAccLimits(ConstRefVector q, ConstRefVector dq,
 bool verbose = true);

 TSID_DEPRECATED const Vector& mask() const; // deprecated
 TSID_DEPRECATED void mask(const Vector& mask); // deprecated
 virtual void setMask(math::ConstRefVector mask) override;

 protected:
 ConstraintInequality m_constraint;
 double m_dt;
 bool m_verbose;
 int m_nv, m_na;

 Vector m_mask;
 VectorXi m_activeAxes;

 Vector m_qa; // actuated part of q
 Vector m_dqa; // actuated part of dq

 double m_eps; // tolerance used to check violations

 Vector m_qMin; // joints position limits
 Vector m_qMax; // joints position limits
 Vector m_dqMax; // joints max velocity limits
 Vector m_ddqMax; // joints max acceleration limits

 Vector m_dqMinViab; // velocity lower limits from viability
 Vector m_dqMaxViab; // velocity upper limits from viability

 Vector m_ddqLBPos; // acceleration lower bound from position bounds
 Vector m_ddqUBPos; // acceleration upper bound from position bounds
 Vector m_ddqLBVia; // acceleration lower bound from viability bounds
 Vector m_ddqUBVia; // acceleration upper bound from viability bounds
 Vector m_ddqLBVel; // acceleration lower bound from velocity bounds
 Vector m_ddqUBVel; // acceleration upper bound from velocity bounds
 Vector m_ddqLBAcc; // acceleration lower bound from acceleration bounds
 Vector m_ddqUBAcc; // acceleration upper bound from acceleration bounds

 Vector m_ddqLB; // final acceleration bounds
 Vector m_ddqUB; // final acceleration bounds

 bool m_impose_position_bounds;
 bool m_impose_velocity_bounds;
 bool m_impose_viability_bounds;
 bool m_impose_acceleration_bounds;

 Vector m_viabViol; // 0 if the state is viable, error otherwise

 // Used in computeAccLimitsFromPosLimits
 double m_two_dt_sq;
 Vector m_ddqMax_q3;
 Vector m_ddqMin_q3;
 Vector m_ddqMax_q2;
 Vector m_ddqMin_q2;
 Vector m_minus_dq_over_dt;

 // Used in computeAccLimitsFromViability
 double m_dt_square;
 Vector m_dt_dq;
 Vector m_dt_two_dq;
 Vector m_two_ddqMax;
 Vector m_dt_ddqMax_dt;
 Vector m_dq_square;
 Vector m_q_plus_dt_dq;
 double m_two_a;
 Vector m_b_1;
 Vector m_b_2;
 Vector m_ddq_1;
 Vector m_ddq_2;
 Vector m_c_1;
 Vector m_delta_1;
 Vector m_c_2;
 Vector m_delta_2;

 // Used in computeAccLimits
 Vector m_ub;
 Vector m_lb;
};

} // namespace tasks
} // namespace tsid

#endif // ifndef __invdyn_task_joint_bounds_hpp__