inverse-dynamics-formulation-base.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

#ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
#define __invdyn_inverse_dynamics_formulation_base_hpp__

#include "tsid/deprecated.hh"
#include "tsid/math/fwd.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/tasks/task-actuation.hpp"
#include "tsid/tasks/task-motion.hpp"
#include "tsid/tasks/task-contact-force.hpp"
#include "tsid/contacts/contact-base.hpp"
#include "tsid/contacts/measured-force-base.hpp"
#include "tsid/solvers/solver-HQP-base.hpp"

#include <string>

namespace tsid {

struct TaskLevel {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 tasks::TaskBase& task;
 std::shared_ptr<math::ConstraintBase> constraint;
 unsigned int priority;

 TaskLevel(tasks::TaskBase& task, unsigned int priority);
};

struct TaskLevelForce {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 tasks::TaskContactForce& task;
 std::shared_ptr<math::ConstraintBase> constraint;
 unsigned int priority;

 TaskLevelForce(tasks::TaskContactForce& task, unsigned int priority);
};

struct MeasuredForceLevel {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 contacts::MeasuredForceBase& measuredForce;

 MeasuredForceLevel(contacts::MeasuredForceBase& measuredForce);
};

class InverseDynamicsFormulationBase {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef pinocchio::Data Data;
 typedef math::Vector Vector;
 typedef math::RefVector RefVector;
 typedef math::ConstRefVector ConstRefVector;
 typedef tasks::TaskMotion TaskMotion;
 typedef tasks::TaskContactForce TaskContactForce;
 typedef tasks::TaskActuation TaskActuation;
 typedef tasks::TaskBase TaskBase;
 typedef contacts::MeasuredForceBase MeasuredForceBase;
 typedef contacts::ContactBase ContactBase;
 typedef solvers::HQPData HQPData;
 typedef solvers::HQPOutput HQPOutput;
 typedef robots::RobotWrapper RobotWrapper;

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

 virtual ~InverseDynamicsFormulationBase() = default;

 virtual Data& data() = 0;

 virtual unsigned int nVar() const = 0;
 virtual unsigned int nEq() const = 0;
 virtual unsigned int nIn() const = 0;

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

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

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

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

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

 TSID_DEPRECATED virtual bool addRigidContact(ContactBase& contact);

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

 virtual bool addMeasuredForce(MeasuredForceBase& measuredForce) = 0;

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

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

 virtual bool removeMeasuredForce(const std::string& measuredForceName) = 0;

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

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

 virtual unsigned int getTaskPriority(const std::string& name) = 0;

 protected:
 std::string m_name;
 RobotWrapper m_robot;
 bool m_verbose;
};

} // namespace tsid

#endif // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__