sot-torque-control  1.6.1
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
simple-inverse-dyn.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Andrea Del Prete, LAAS-CNRS
3  *
4  * This file is part of sot-torque-control.
5  * sot-torque-control is free software: you can redistribute it and/or
6  * modify it under the terms of the GNU Lesser General Public License
7  * as published by the Free Software Foundation, either version 3 of
8  * the License, or (at your option) any later version.
9  * sot-torque-control is distributed in the hope that it will be
10  * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details. You should
13  * have received a copy of the GNU Lesser General Public License along
14  * with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
15  */
16 
17 #ifndef __sot_torque_control_simple_inverse_dyn_H__
18 #define __sot_torque_control_simple_inverse_dyn_H__
19 
20 /* --------------------------------------------------------------------- */
21 /* --- API ------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 
24 #if defined(WIN32)
25 #if defined(simple_inverse_dyn_EXPORTS)
26 #define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport)
27 #else
28 #define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport)
29 #endif
30 #else
31 #define SOTSIMPLEINVERSEDYN_EXPORT
32 #endif
33 
34 /* --------------------------------------------------------------------- */
35 /* --- INCLUDE --------------------------------------------------------- */
36 /* --------------------------------------------------------------------- */
37 
38 #include <map>
39 #include "boost/assign.hpp"
40 
41 /* Pinocchio */
42 #include <pinocchio/multibody/model.hpp>
43 #include <pinocchio/parsers/urdf.hpp>
44 #include "pinocchio/algorithm/joint-configuration.hpp"
45 
46 #include <tsid/robots/robot-wrapper.hpp>
47 #include <tsid/solvers/solver-HQP-base.hpp>
48 #include <tsid/contacts/contact-6d.hpp>
49 #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
50 #include <tsid/tasks/task-com-equality.hpp>
51 #include <tsid/tasks/task-joint-posture.hpp>
52 #include <tsid/trajectories/trajectory-euclidian.hpp>
53 
54 /* HELPER */
55 #include <dynamic-graph/signal-helper.h>
56 #include <sot/core/matrix-geometry.hh>
57 #include <sot/core/robot-utils.hh>
58 
60 
61 namespace dynamicgraph {
62 namespace sot {
63 namespace torque_control {
64 
66 
67 const std::string ControlOutput_s[] = {"velocity", "torque"};
68 
69 /* --------------------------------------------------------------------- */
70 /* --- CLASS ----------------------------------------------------------- */
71 /* --------------------------------------------------------------------- */
72 
73 class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entity {
75  DYNAMIC_GRAPH_ENTITY_DECL();
76 
77  public:
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 
80  /* --- CONSTRUCTOR ---- */
81  SimpleInverseDyn(const std::string& name);
82 
83  /* --- SIGNALS --- */
84 
85  DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
86  DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
87  DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
88  DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
89  DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
90  DECLARE_SIGNAL_IN(w_posture, double);
91 
92  DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
93  DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
94 
95  DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
96  DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
97  DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
98  DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
99  DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
100  DECLARE_SIGNAL_IN(w_com, double);
101 
102  DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
103  DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
104  DECLARE_SIGNAL_IN(w_forces, double);
105  DECLARE_SIGNAL_IN(mu, double);
106  DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
107  DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
108  DECLARE_SIGNAL_IN(f_min, double);
109  DECLARE_SIGNAL_IN(f_max, double);
110 
111  DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
112  DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
113  DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
114  DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
115  DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
116  DECLARE_SIGNAL_IN(w_waist, double);
117 
118  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
119  DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
120  DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector);
121  DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
122 
123  DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
124  DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
125  DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
126  DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
127  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
128 
129  /* --- COMMANDS --- */
130 
131  void init(const double& dt, const std::string& robotRef);
132  virtual void setControlOutputType(const std::string& type);
133  void updateComOffset();
134 
135  /* --- ENTITY INHERITANCE --- */
136  virtual void display(std::ostream& os) const;
137 
138  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
139  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
140  }
141 
142  protected:
143  double m_dt;
144  double m_t;
146  bool m_enabled;
147  bool m_firstTime;
148 
151  tsid::robots::RobotWrapper* m_robot;
152 
154  tsid::solvers::SolverHQPBase* m_hqpSolver;
155  tsid::InverseDynamicsFormulationAccForce* m_invDyn;
156 
158  tsid::contacts::Contact6d* m_contactRF;
159  tsid::contacts::Contact6d* m_contactLF;
160  tsid::tasks::TaskComEquality* m_taskCom;
161  tsid::tasks::TaskSE3Equality* m_taskWaist;
162  tsid::tasks::TaskJointPosture* m_taskPosture;
163  tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
164 
166  tsid::trajectories::TrajectorySample m_sampleCom;
167  tsid::trajectories::TrajectorySample m_sampleWaist;
168  tsid::trajectories::TrajectorySample m_samplePosture;
169 
171  double m_w_com;
172  double m_w_posture;
173  double m_w_waist;
174 
176  tsid::math::Vector m_dv_sot;
177  tsid::math::Vector m_dv_urdf;
178  tsid::math::Vector m_v_sot;
179  tsid::math::Vector m_v_urdf;
180  tsid::math::Vector m_q_sot;
181  tsid::math::Vector m_q_urdf;
182  tsid::math::Vector m_tau_sot;
183 
185 
186  unsigned int m_timeLast;
187  RobotUtilShrPtr m_robot_util;
189 
190 }; // class SimpleInverseDyn
191 } // namespace torque_control
192 } // namespace sot
193 } // namespace dynamicgraph
194 
195 #endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__
dynamicgraph::sot::torque_control::Vector3
Eigen::Matrix< double, 3, 1 > Vector3
Definition: admittance-controller.cpp:46
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_taskPosture
tsid::tasks::TaskJointPosture * m_taskPosture
Definition: simple-inverse-dyn.hh:162
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_sampleCom
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
Definition: simple-inverse-dyn.hh:166
dynamicgraph::sot::torque_control::CONTROL_OUTPUT_SIZE
@ CONTROL_OUTPUT_SIZE
Definition: simple-inverse-dyn.hh:65
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_v_urdf
tsid::math::Vector m_v_urdf
desired velocities (sot order)
Definition: simple-inverse-dyn.hh:179
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_tau_sot
tsid::math::Vector m_tau_sot
desired and current positions (urdf order) (close the TSID loop on it)
Definition: simple-inverse-dyn.hh:182
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::SimpleInverseDyn
Definition: simple-inverse-dyn.hh:73
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_w_com
double m_w_com
Weights of the Tasks (which can be changed)
Definition: simple-inverse-dyn.hh:171
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_q_sot
tsid::math::Vector m_q_sot
desired and current velocities (urdf order) (close the TSID loop on it)
Definition: simple-inverse-dyn.hh:180
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_samplePosture
tsid::trajectories::TrajectorySample m_samplePosture
Definition: simple-inverse-dyn.hh:168
vector-conversions.hh
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_ctrlMode
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
Definition: simple-inverse-dyn.hh:188
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_dv_sot
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
Definition: simple-inverse-dyn.hh:176
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_contactLF
tsid::contacts::Contact6d * m_contactLF
Definition: simple-inverse-dyn.hh:159
dynamicgraph::sot::torque_control::ControlOutput
ControlOutput
Definition: simple-inverse-dyn.hh:65
dynamicgraph::sot::torque_control::SimpleInverseDyn::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: simple-inverse-dyn.hh:138
dynamicgraph::sot::torque_control::CONTROL_OUTPUT_TORQUE
@ CONTROL_OUTPUT_TORQUE
Definition: simple-inverse-dyn.hh:65
SOTSIMPLEINVERSEDYN_EXPORT
#define SOTSIMPLEINVERSEDYN_EXPORT
Definition: simple-inverse-dyn.hh:31
dynamicgraph::sot::torque_control::CONTROL_OUTPUT_VELOCITY
@ CONTROL_OUTPUT_VELOCITY
Definition: simple-inverse-dyn.hh:65
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_robot
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
Definition: simple-inverse-dyn.hh:151
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_hqpSolver
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
Definition: simple-inverse-dyn.hh:154
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_q_urdf
tsid::math::Vector m_q_urdf
desired positions (sot order)
Definition: simple-inverse-dyn.hh:181
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_taskWaist
tsid::tasks::TaskSE3Equality * m_taskWaist
Definition: simple-inverse-dyn.hh:161
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_t
double m_t
control loop time period
Definition: simple-inverse-dyn.hh:144
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_taskCom
tsid::tasks::TaskComEquality * m_taskCom
Definition: simple-inverse-dyn.hh:160
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_sampleWaist
tsid::trajectories::TrajectorySample m_sampleWaist
Definition: simple-inverse-dyn.hh:167
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_dv_urdf
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
Definition: simple-inverse-dyn.hh:177
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_taskBlockedJoints
tsid::tasks::TaskJointPosture * m_taskBlockedJoints
Definition: simple-inverse-dyn.hh:163
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_w_waist
double m_w_waist
Definition: simple-inverse-dyn.hh:173
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_timeLast
unsigned int m_timeLast
3d CoM offset
Definition: simple-inverse-dyn.hh:186
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_dt
double m_dt
Definition: simple-inverse-dyn.hh:143
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_firstTime
bool m_firstTime
True if controler is enabled.
Definition: simple-inverse-dyn.hh:147
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_invDyn
tsid::InverseDynamicsFormulationAccForce * m_invDyn
Definition: simple-inverse-dyn.hh:155
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_contactRF
tsid::contacts::Contact6d * m_contactRF
TASKS.
Definition: simple-inverse-dyn.hh:158
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_enabled
bool m_enabled
true if the entity has been successfully initialized
Definition: simple-inverse-dyn.hh:146
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_v_sot
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
Definition: simple-inverse-dyn.hh:178
dynamicgraph::sot::torque_control::ControlOutput_s
const std::string ControlOutput_s[]
Definition: simple-inverse-dyn.hh:67
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_initSucceeded
bool m_initSucceeded
current time
Definition: simple-inverse-dyn.hh:145
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_w_posture
double m_w_posture
Definition: simple-inverse-dyn.hh:172
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_com_offset
tsid::math::Vector3 m_com_offset
desired torques (sot order)
Definition: simple-inverse-dyn.hh:184
dynamicgraph::sot::torque_control::SimpleInverseDyn::m_robot_util
RobotUtilShrPtr m_robot_util
Final time of the control loop.
Definition: simple-inverse-dyn.hh:187