sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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
40#include "boost/assign.hpp"
41
42/* Pinocchio */
43#include <pinocchio/multibody/model.hpp>
44#include <pinocchio/parsers/urdf.hpp>
45#include <tsid/contacts/contact-6d.hpp>
46#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
47#include <tsid/robots/robot-wrapper.hpp>
48#include <tsid/solvers/solver-HQP-base.hpp>
49#include <tsid/tasks/task-com-equality.hpp>
50#include <tsid/tasks/task-joint-posture.hpp>
51#include <tsid/trajectories/trajectory-euclidian.hpp>
52
53#include "pinocchio/algorithm/joint-configuration.hpp"
54
55/* HELPER */
56#include <dynamic-graph/signal-helper.h>
57
58#include <sot/core/matrix-geometry.hh>
59#include <sot/core/robot-utils.hh>
61
62namespace dynamicgraph {
63namespace sot {
64namespace torque_control {
65
70};
71
72const std::string ControlOutput_s[] = {"velocity", "torque"};
73
74/* --------------------------------------------------------------------- */
75/* --- CLASS ----------------------------------------------------------- */
76/* --------------------------------------------------------------------- */
77
79 : public ::dynamicgraph::Entity {
81 DYNAMIC_GRAPH_ENTITY_DECL();
82
83 public:
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85
86 /* --- CONSTRUCTOR ---- */
87 SimpleInverseDyn(const std::string& name);
88
89 /* --- SIGNALS --- */
90
91 DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
92 DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
93 DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
94 DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
95 DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
96 DECLARE_SIGNAL_IN(w_posture, double);
97
98 DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
99 DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
100
101 DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
102 DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
103 DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
104 DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
105 DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
106 DECLARE_SIGNAL_IN(w_com, double);
107
108 DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
109 DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
110 DECLARE_SIGNAL_IN(w_forces, double);
111 DECLARE_SIGNAL_IN(mu, double);
112 DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
113 DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
114 DECLARE_SIGNAL_IN(f_min, double);
115 DECLARE_SIGNAL_IN(f_max, double);
116
117 DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
118 DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
119 DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
120 DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
121 DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
122 DECLARE_SIGNAL_IN(w_waist, double);
123
124 DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
125 DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
127 active_joints,
128 dynamicgraph::Vector);
129 DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
130
131 DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
132 DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
133 DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
134 DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
135 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
136
137 /* --- COMMANDS --- */
138
139 void init(const double& dt, const std::string& robotRef);
140 virtual void setControlOutputType(const std::string& type);
141 void updateComOffset();
142
143 /* --- ENTITY INHERITANCE --- */
144 virtual void display(std::ostream& os) const;
145
146 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
147 const char* = "", int = 0) {
148 logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
149 }
150
151 protected:
152 double m_dt;
153 double m_t;
154 bool
158
161 tsid::robots::RobotWrapper* m_robot;
162
164 tsid::solvers::SolverHQPBase* m_hqpSolver;
165 tsid::InverseDynamicsFormulationAccForce* m_invDyn;
166
168 tsid::contacts::Contact6d* m_contactRF;
169 tsid::contacts::Contact6d* m_contactLF;
170 tsid::tasks::TaskComEquality* m_taskCom;
171 tsid::tasks::TaskSE3Equality* m_taskWaist;
172 tsid::tasks::TaskJointPosture* m_taskPosture;
173 tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
174
176 tsid::trajectories::TrajectorySample m_sampleCom;
177 tsid::trajectories::TrajectorySample m_sampleWaist;
178 tsid::trajectories::TrajectorySample m_samplePosture;
179
181 double m_w_com;
183 double m_w_waist;
184
186 tsid::math::Vector m_dv_sot;
187 tsid::math::Vector m_dv_urdf;
188 tsid::math::Vector m_v_sot;
189 tsid::math::Vector m_v_urdf;
191 tsid::math::Vector m_q_sot;
192 tsid::math::Vector m_q_urdf;
194 tsid::math::Vector m_tau_sot;
195
196 tsid::math::Vector3 m_com_offset;
197
198 unsigned int m_timeLast;
199 RobotUtilShrPtr m_robot_util;
202
203}; // class SimpleInverseDyn
204} // namespace torque_control
205} // namespace sot
206} // namespace dynamicgraph
207
208#endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__
double m_w_com
Weights of the Tasks (which can be changed)
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector)
tsid::solvers::SolverHQPBase * m_hqpSolver
Solver and problem formulation.
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector)
RobotUtilShrPtr m_robot_util
Final time of the control loop.
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector)
bool m_enabled
true if the entity has been successfully initialized
tsid::math::Vector m_dv_sot
Computed solutions (accelerations and torques) and their derivatives.
DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector)
tsid::InverseDynamicsFormulationAccForce * m_invDyn
tsid::math::Vector m_v_urdf
desired velocities (sot order)
tsid::robots::RobotWrapper * m_robot
True at the first iteration of the controller.
DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector)
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector)
mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector)
tsid::math::Vector m_q_urdf
desired positions (sot order)
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector)
tsid::math::Vector m_v_sot
desired accelerations (urdf order)
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector)
tsid::trajectories::TrajectorySample m_samplePosture
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector)
tsid::trajectories::TrajectorySample m_sampleCom
Trajectories of the tasks.
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix)
tsid::math::Vector m_dv_urdf
desired accelerations (sot order)
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector)
tsid::math::Vector3 m_com_offset
desired torques (sot order)
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector)
ControlOutput m_ctrlMode
Share pointer to the robot utils methods.
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector)
tsid::trajectories::TrajectorySample m_sampleWaist
to read text file
Definition: treeview.dox:22
#define SOTSIMPLEINVERSEDYN_EXPORT