sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
device-torque-ctrl.hh
Go to the documentation of this file.
1/*
2 * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#ifndef _DeviceTorqueCtrl_H_
7#define _DeviceTorqueCtrl_H_
8
9#include <pinocchio/fwd.hpp>
10
11// include pinocchio first
12
13#include <dynamic-graph/entity.h>
14#include <dynamic-graph/linear-algebra.h>
15#include <dynamic-graph/signal-ptr.h>
16#include <dynamic-graph/signal.h>
17
18#include <Eigen/Cholesky>
19#include <boost/random/mersenne_twister.hpp>
20#include <boost/random/normal_distribution.hpp>
21#include <boost/random/variate_generator.hpp>
22#include <sot/core/abstract-sot-external-interface.hh>
23#include <sot/core/device.hh>
24#include <tsid/robots/robot-wrapper.hpp>
25#include <tsid/tasks/task-se3-equality.hpp>
26
27/* HELPER */
28#include <dynamic-graph/signal-helper.h>
29
30#include <sot/core/robot-utils.hh>
31
32namespace dgsot = dynamicgraph::sot;
33
34namespace dynamicgraph {
35namespace sot {
36namespace torque_control {
37
59class DeviceTorqueCtrl : public dgsot::Device {
60 public:
61 static const std::string CLASS_NAME;
62 static const double TIMESTEP_DEFAULT;
63 static const double FORCE_SENSOR_NOISE_STD_DEV;
64
65 virtual const std::string& getClassName() const { return CLASS_NAME; }
66
67 DeviceTorqueCtrl(std::string RobotName);
68 virtual ~DeviceTorqueCtrl();
69
70 virtual void setStateSize(const unsigned int& size);
71 virtual void setState(const dynamicgraph::Vector& st);
72 virtual void setVelocity(const dynamicgraph::Vector& vel);
73 virtual void setControlInputType(const std::string& cit);
74
75 virtual void init(const double& dt, const std::string& urdfFile);
76
77 protected:
78 virtual void integrate(const double& dt);
80
81 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
82 const char* = "", int = 0) {
83 logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
84 }
85
87 double timestep_;
89
91 dynamicgraph::SignalPtr<dynamicgraph::Vector, int>* forcesSIN_[4];
92
94 dynamicgraph::Signal<dynamicgraph::Vector, int> accelerometerSOUT_;
96 dynamicgraph::Signal<dynamicgraph::Vector, int> gyrometerSOUT_;
98 dynamicgraph::Signal<dynamicgraph::Vector, int> robotStateSOUT_;
100 dynamicgraph::Signal<dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
102 dynamicgraph::Signal<dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
104 dynamicgraph::Signal<dynamicgraph::Vector, int> currentSOUT_;
106 dynamicgraph::Signal<dynamicgraph::Vector, int> p_gainsSOUT_;
107 dynamicgraph::Signal<dynamicgraph::Vector, int> d_gainsSOUT_;
108
109 DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
110 DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
111 DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
112 DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
113
115 dynamicgraph::Vector accelerometer_;
116 dynamicgraph::Vector gyrometer_;
117
118 dynamicgraph::Vector base6d_encoders_;
119 dynamicgraph::Vector jointsVelocities_;
120 dynamicgraph::Vector jointsAccelerations_;
121
122 dynamicgraph::Vector wrenches_[4];
123 dynamicgraph::Vector temp6_;
124
126
128 tsid::robots::RobotWrapper* m_robot;
129 pinocchio::Data* m_data;
130 tsid::tasks::TaskSE3Equality* m_contactRF;
131 tsid::tasks::TaskSE3Equality* m_contactLF;
132 unsigned int m_nk; // number of contact forces
133
134 tsid::math::Vector m_q, m_v, m_dv, m_f;
135 tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;
136
137 typedef Eigen::LDLT<Eigen::MatrixXd> Cholesky;
139 Eigen::MatrixXd m_K;
140 Eigen::VectorXd m_k;
141 Eigen::MatrixXd m_Jc;
142
145 Eigen::JacobiSVD<Matrix> m_Jc_svd;
146 Vector m_dJcv;
147 Matrix m_Z;
148 Matrix m_ZMZ;
149 Eigen::LDLT<Matrix> m_ZMZ_chol;
150 Vector m_dv_c;
151 Vector m_dvBar;
152 Vector m_tau_np6;
153 int m_nj;
154
155 typedef boost::mt11213b ENG; // uniform random number generator
156 typedef boost::normal_distribution<double> DIST; // Normal Distribution
157 typedef boost::variate_generator<ENG, DIST> GEN; // Variate generator
161
162 RobotUtilShrPtr m_robot_util;
163};
164
165} // end namespace torque_control
166} // end namespace sot
167} // end namespace dynamicgraph
168#endif /* DevicePosCtrl*/
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
dynamicgraph::Vector jointsVelocities_
base 6d pose + joints' angles
virtual void setControlInputType(const std::string &cit)
dynamicgraph::Signal< dynamicgraph::Vector, int > robotStateSOUT_
base 6d pose + joints' angles measured by encoders
dynamicgraph::Vector wrenches_[4]
joints' accelerations
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector)
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsAccelerationsSOUT_
joints' accelerations computed by the integrator
virtual void setVelocity(const dynamicgraph::Vector &vel)
Eigen::LDLT< Matrix > m_ZMZ_chol
projected mass matrix: Z_c^T*M*Z_c
virtual void setStateSize(const unsigned int &size)
dynamicgraph::Signal< dynamicgraph::Vector, int > currentSOUT_
motor currents
tsid::robots::RobotWrapper * m_robot
robot geometric/inertial data
dynamicgraph::Signal< dynamicgraph::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
dynamicgraph::Signal< dynamicgraph::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
virtual void setState(const dynamicgraph::Vector &st)
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > * forcesSIN_[4]
input force sensor values
dynamicgraph::Vector accelerometer_
Intermediate variables to avoid allocation during control.
virtual void init(const double &dt, const std::string &urdfFile)
dynamicgraph::Signal< dynamicgraph::Vector, int > d_gainsSOUT_
DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector)
dynamicgraph::Signal< dynamicgraph::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector)
dynamicgraph::Signal< dynamicgraph::Vector, int > jointsVelocitiesSOUT_
joints' velocities computed by the integrator
Eigen::MatrixXd m_K
cholesky decomposition of the K matrix
DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector)
dynamicgraph::Vector jointsAccelerations_
joints' velocities
to read text file
Definition treeview.dox:22