sot-torque-control  1.6.2
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__
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
Eigen::Matrix< double, 3, 1 > Vector3
tsid::trajectories::TrajectorySample m_sampleWaist
bool m_enabled
true if the entity has been successfully initialized
tsid::math::Vector3 m_com_offset
desired torques (sot order)
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
#define SOTSIMPLEINVERSEDYN_EXPORT
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
RobotUtilShrPtr m_robot_util
Final time of the control loop.
tsid::math::Vector m_tau_sot
desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::trajectories::TrajectorySample m_samplePosture
double m_w_com
Weights of the Tasks (which can be changed)
tsid::math::Vector m_q_sot
desired and current velocities (urdf order) (close the TSID loop on it)
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
tsid::math::Vector m_q_urdf
desired positions (sot order)
to read text file
Definition: treeview.dox:22
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.