simple-inverse-dyn.hh

Go to the documentation of this file.

/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-torque-control 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.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef __sot_torque_control_simple_inverse_dyn_H__
#define __sot_torque_control_simple_inverse_dyn_H__
 
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#if defined(WIN32)
#if defined(simple_inverse_dyn_EXPORTS)
#define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport)
#else
#define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSIMPLEINVERSEDYN_EXPORT
#endif
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
#include <map>
 
#include "boost/assign.hpp"
 
/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <tsid/contacts/contact-6d.hpp>
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/solvers/solver-HQP-base.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
 
#include "pinocchio/algorithm/joint-configuration.hpp"
 
/* HELPER */
#include <dynamic-graph/signal-helper.h>
 
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
 
namespace dynamicgraph {
namespace sot {
namespace torque_control {
 
enum ControlOutput {
  CONTROL_OUTPUT_VELOCITY = 0,
  CONTROL_OUTPUT_TORQUE = 1,
  CONTROL_OUTPUT_SIZE = 2
};
 
const std::string ControlOutput_s[] = {"velocity", "torque"};
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn
    : public ::dynamicgraph::Entity {
  typedef SimpleInverseDyn EntityClassName;
  DYNAMIC_GRAPH_ENTITY_DECL();
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  /* --- CONSTRUCTOR ---- */
  SimpleInverseDyn(const std::string& name);
 
  /* --- SIGNALS --- */
 
  DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(w_posture, double);
 
  DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
 
  DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(w_com, double);
 
  DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(w_forces, double);
  DECLARE_SIGNAL_IN(mu, double);
  DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
  DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(f_min, double);
  DECLARE_SIGNAL_IN(f_max, double);
 
  DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(w_waist, double);
 
  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(
      active_joints,
      dynamicgraph::Vector);  
  DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
 
  DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
 
  /* --- COMMANDS --- */
 
  void init(const double& dt, const std::string& robotRef);
  virtual void setControlOutputType(const std::string& type);
  void updateComOffset();
 
  /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;
 
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
               const char* = "", int = 0) {
    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
  }
 
 protected:
  double m_dt;  
  double m_t;   
  bool
      m_initSucceeded;  
  bool m_enabled;       
  bool m_firstTime;     
 
  tsid::robots::RobotWrapper* m_robot;
 
  tsid::solvers::SolverHQPBase* m_hqpSolver;
  tsid::InverseDynamicsFormulationAccForce* m_invDyn;
 
  tsid::contacts::Contact6d* m_contactRF;
  tsid::contacts::Contact6d* m_contactLF;
  tsid::tasks::TaskComEquality* m_taskCom;
  tsid::tasks::TaskSE3Equality* m_taskWaist;
  tsid::tasks::TaskJointPosture* m_taskPosture;
  tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
 
  tsid::trajectories::TrajectorySample m_sampleCom;
  tsid::trajectories::TrajectorySample m_sampleWaist;
  tsid::trajectories::TrajectorySample m_samplePosture;
 
  double m_w_com;
  double m_w_posture;
  double m_w_waist;
 
  tsid::math::Vector m_dv_sot;   
  tsid::math::Vector m_dv_urdf;  
  tsid::math::Vector m_v_sot;    
  tsid::math::Vector m_v_urdf;   
  tsid::math::Vector m_q_sot;    
  tsid::math::Vector m_q_urdf;   
  tsid::math::Vector m_tau_sot;  
 
  tsid::math::Vector3 m_com_offset;  
 
  unsigned int m_timeLast;       
  RobotUtilShrPtr m_robot_util;  
  ControlOutput
      m_ctrlMode;  
 
};  // class SimpleInverseDyn
}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph
 
#endif  // #ifndef __sot_torque_control_simple_inverse_dyn_H__