device-torque-ctrl.hh

Go to the documentation of this file.

/*
 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
 *
 */
 
#ifndef _DeviceTorqueCtrl_H_
#define _DeviceTorqueCtrl_H_
 
#include <pinocchio/fwd.hpp>
 
// include pinocchio first
 
#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal.h>
 
#include <Eigen/Cholesky>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/device.hh>
#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/tasks/task-se3-equality.hpp>
 
/* HELPER */
#include <dynamic-graph/signal-helper.h>
 
#include <sot/core/robot-utils.hh>
 
namespace dgsot = dynamicgraph::sot;
 
namespace dynamicgraph {
namespace sot {
namespace torque_control {
 
class DeviceTorqueCtrl : public dgsot::Device {
 public:
  static const std::string CLASS_NAME;
  static const double TIMESTEP_DEFAULT;
  static const double FORCE_SENSOR_NOISE_STD_DEV;
 
  virtual const std::string& getClassName() const { return CLASS_NAME; }
 
  DeviceTorqueCtrl(std::string RobotName);
  virtual ~DeviceTorqueCtrl();
 
  virtual void setStateSize(const unsigned int& size);
  virtual void setState(const dynamicgraph::Vector& st);
  virtual void setVelocity(const dynamicgraph::Vector& vel);
  virtual void setControlInputType(const std::string& cit);
 
  virtual void init(const double& dt, const std::string& urdfFile);
 
 protected:
  virtual void integrate(const double& dt);
  void computeForwardDynamics();
 
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
               const char* = "", int = 0) {
    logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
  }
 
  double timestep_;
  bool m_initSucceeded;
 
  dynamicgraph::SignalPtr<dynamicgraph::Vector, int>* forcesSIN_[4];
 
  dynamicgraph::Signal<dynamicgraph::Vector, int> accelerometerSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> gyrometerSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> robotStateSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> currentSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> p_gainsSOUT_;
  dynamicgraph::Signal<dynamicgraph::Vector, int> d_gainsSOUT_;
 
  DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
  DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
 
  dynamicgraph::Vector accelerometer_;
  dynamicgraph::Vector gyrometer_;
 
  dynamicgraph::Vector base6d_encoders_;      
  dynamicgraph::Vector jointsVelocities_;     
  dynamicgraph::Vector jointsAccelerations_;  
 
  dynamicgraph::Vector wrenches_[4];
  dynamicgraph::Vector temp6_;
 
  bool m_isTorqueControlled;
 
  tsid::robots::RobotWrapper* m_robot;
  pinocchio::Data* m_data;
  tsid::tasks::TaskSE3Equality* m_contactRF;
  tsid::tasks::TaskSE3Equality* m_contactLF;
  unsigned int m_nk;  // number of contact forces
 
  tsid::math::Vector m_q, m_v, m_dv, m_f;
  tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;
 
  typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky;
  Cholesky m_K_chol;  
  Eigen::MatrixXd m_K;
  Eigen::VectorXd m_k;
  Eigen::MatrixXd m_Jc;  
 
  double m_numericalDamping;  
  Eigen::JacobiSVD<Matrix> m_Jc_svd;  
  Vector m_dJcv;
  Matrix m_Z;                      
  Matrix m_ZMZ;                    
  Eigen::LDLT<Matrix> m_ZMZ_chol;  
  Vector m_dv_c;                   
  Vector m_dvBar;                  
  Vector m_tau_np6;
  int m_nj;  
 
  typedef boost::mt11213b ENG;  // uniform random number generator
  typedef boost::normal_distribution<double> DIST;  // Normal Distribution
  typedef boost::variate_generator<ENG, DIST> GEN;  // Variate generator
  ENG randomNumberGenerator_;
  DIST normalDistribution_;
  GEN normalRandomNumberGenerator_;
 
  RobotUtilShrPtr m_robot_util;
};
 
}  // end namespace torque_control
}  // end namespace sot
}  // end namespace dynamicgraph
#endif /* DevicePosCtrl*/