inverse-dynamics-formulation-acc-force.hpp

Go to the documentation of this file.

//
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
//
// 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_inverse_dynamics_formulation_acc_force_hpp__
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__

#include <vector>

#include "tsid/formulations/contact-level.hpp"
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
#include "tsid/math/constraint-equality.hpp"

namespace tsid {

class ContactTransitionInfo {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  double time_start;
  double time_end;
  double fMax_start;  
  double fMax_end;    
  std::shared_ptr<ContactLevel> contactLevel;
};

class InverseDynamicsFormulationAccForce
    : public InverseDynamicsFormulationBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef pinocchio::Data Data;
  typedef math::Vector Vector;
  typedef math::Matrix Matrix;
  typedef math::ConstRefVector ConstRefVector;
  typedef tasks::TaskBase TaskBase;
  typedef tasks::TaskMotion TaskMotion;
  typedef tasks::TaskContactForce TaskContactForce;
  typedef tasks::TaskActuation TaskActuation;
  typedef contacts::MeasuredForceBase MeasuredForceBase;
  typedef solvers::HQPOutput HQPOutput;

  InverseDynamicsFormulationAccForce(const std::string& name,
                                     RobotWrapper& robot, bool verbose = false);

  virtual ~InverseDynamicsFormulationAccForce() {}

  Data& data();

  unsigned int nVar() const;
  unsigned int nEq() const;
  unsigned int nIn() const;

  bool addMotionTask(TaskMotion& task, double weight,
                     unsigned int priorityLevel,
                     double transition_duration = 0.0);

  bool addForceTask(TaskContactForce& task, double weight,
                    unsigned int priorityLevel,
                    double transition_duration = 0.0);

  bool addActuationTask(TaskActuation& task, double weight,
                        unsigned int priorityLevel,
                        double transition_duration = 0.0);

  bool updateTaskWeight(const std::string& task_name, double weight);

  bool addRigidContact(ContactBase& contact, double force_regularization_weight,
                       double motion_weight = 1.0,
                       unsigned int motion_priority_level = 0);

  TSID_DEPRECATED bool addRigidContact(ContactBase& contact);

  bool updateRigidContactWeights(const std::string& contact_name,
                                 double force_regularization_weight,
                                 double motion_weight = -1.0);

  bool addMeasuredForce(MeasuredForceBase& measuredForce);

  bool removeTask(const std::string& taskName,
                  double transition_duration = 0.0);

  bool removeRigidContact(const std::string& contactName,
                          double transition_duration = 0.0);

  bool removeMeasuredForce(const std::string& measuredForceName);

  const HQPData& computeProblemData(double time, ConstRefVector q,
                                    ConstRefVector v);

  const Vector& getActuatorForces(const HQPOutput& sol);
  const Vector& getAccelerations(const HQPOutput& sol);
  const Vector& getContactForces(const HQPOutput& sol);
  Vector getContactForces(const std::string& name, const HQPOutput& sol);
  bool getContactForces(const std::string& name, const HQPOutput& sol,
                        RefVector f);

 public:
  template <class TaskLevelPointer>
  void addTask(TaskLevelPointer task, double weight,
               unsigned int priorityLevel);

  void resizeHqpData();

  bool removeFromHqpData(const std::string& name);

  bool decodeSolution(const HQPOutput& sol);

  Data m_data;
  HQPData m_hqpData;
  std::vector<std::shared_ptr<TaskLevel>> m_taskMotions;
  std::vector<std::shared_ptr<TaskLevelForce>> m_taskContactForces;
  std::vector<std::shared_ptr<TaskLevel>> m_taskActuations;
  std::vector<std::shared_ptr<ContactLevel>> m_contacts;
  std::vector<std::shared_ptr<MeasuredForceLevel>> m_measuredForces;
  double m_t;         
  unsigned int m_k;   
  unsigned int m_v;   
  unsigned int m_u;   
  unsigned int m_eq;  
  unsigned int m_in;  
  Matrix m_Jc;        
  std::shared_ptr<math::ConstraintEquality> m_baseDynamics;

  bool m_solutionDecoded;
  Vector m_dv;
  Vector m_f;
  Vector m_tau;

  Vector h_fext;  

  std::vector<std::shared_ptr<ContactTransitionInfo>> m_contactTransitions;
};
}  // namespace tsid
#endif  // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__