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 |
|
|
#include <dynamic-graph/factory.h> |
18 |
|
|
|
19 |
|
|
#include <boost/test/unit_test.hpp> |
20 |
|
|
#include <sot/core/debug.hh> |
21 |
|
|
#include <sot/torque_control/commands-helper.hh> |
22 |
|
|
#include <sot/torque_control/simple-inverse-dyn.hh> |
23 |
|
|
#include <tsid/math/utils.hpp> |
24 |
|
|
#include <tsid/solvers/solver-HQP-factory.hxx> |
25 |
|
|
#include <tsid/solvers/utils.hpp> |
26 |
|
|
#include <tsid/utils/statistics.hpp> |
27 |
|
|
#include <tsid/utils/stop-watch.hpp> |
28 |
|
|
|
29 |
|
|
#if DEBUG |
30 |
|
|
#define ODEBUG(x) std::cout << x << std::endl |
31 |
|
|
#else |
32 |
|
|
#define ODEBUG(x) |
33 |
|
|
#endif |
34 |
|
|
#define ODEBUG3(x) std::cout << x << std::endl |
35 |
|
|
|
36 |
|
|
#define DBGFILE "/tmp/debug-sot-torque-control.dat" |
37 |
|
|
|
38 |
|
|
#define RESETDEBUG5() \ |
39 |
|
|
{ \ |
40 |
|
|
std::ofstream DebugFile; \ |
41 |
|
|
DebugFile.open(DBGFILE, std::ofstream::out); \ |
42 |
|
|
DebugFile.close(); \ |
43 |
|
|
} |
44 |
|
|
#define ODEBUG5FULL(x) \ |
45 |
|
|
{ \ |
46 |
|
|
std::ofstream DebugFile; \ |
47 |
|
|
DebugFile.open(DBGFILE, std::ofstream::app); \ |
48 |
|
|
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \ |
49 |
|
|
<< "):" << x << std::endl; \ |
50 |
|
|
DebugFile.close(); \ |
51 |
|
|
} |
52 |
|
|
#define ODEBUG5(x) \ |
53 |
|
|
{ \ |
54 |
|
|
std::ofstream DebugFile; \ |
55 |
|
|
DebugFile.open(DBGFILE, std::ofstream::app); \ |
56 |
|
|
DebugFile << x << std::endl; \ |
57 |
|
|
DebugFile.close(); \ |
58 |
|
|
} |
59 |
|
|
|
60 |
|
|
#define RESETDEBUG4() |
61 |
|
|
#define ODEBUG4FULL(x) |
62 |
|
|
#define ODEBUG4(x) |
63 |
|
|
|
64 |
|
|
namespace dynamicgraph { |
65 |
|
|
namespace sot { |
66 |
|
|
namespace torque_control { |
67 |
|
|
namespace dg = ::dynamicgraph; |
68 |
|
|
using namespace dg; |
69 |
|
|
using namespace dg::command; |
70 |
|
|
using namespace std; |
71 |
|
|
using namespace tsid; |
72 |
|
|
using namespace tsid::trajectories; |
73 |
|
|
using namespace tsid::math; |
74 |
|
|
using namespace tsid::contacts; |
75 |
|
|
using namespace tsid::tasks; |
76 |
|
|
using namespace tsid::solvers; |
77 |
|
|
using namespace dg::sot; |
78 |
|
|
|
79 |
|
|
#define REQUIRE_FINITE(A) assert(is_finite(A)) |
80 |
|
|
|
81 |
|
|
// Size to be aligned "-------------------------------------------------------" |
82 |
|
|
#define PROFILE_TAU_DES_COMPUTATION "SimpleInverseDyn: desired tau" |
83 |
|
|
#define PROFILE_HQP_SOLUTION "SimpleInverseDyn: HQP" |
84 |
|
|
#define PROFILE_PREPARE_INV_DYN "SimpleInverseDyn: prepare inv-dyn" |
85 |
|
|
#define PROFILE_READ_INPUT_SIGNALS "SimpleInverseDyn: read input signals" |
86 |
|
|
|
87 |
|
|
#define ZERO_FORCE_THRESHOLD 1e-3 |
88 |
|
|
|
89 |
|
|
#define INPUT_SIGNALS \ |
90 |
|
|
m_posture_ref_posSIN << m_posture_ref_velSIN << m_posture_ref_accSIN \ |
91 |
|
|
<< m_kp_postureSIN << m_kd_postureSIN << m_w_postureSIN \ |
92 |
|
|
<< m_kp_posSIN << m_kd_posSIN << m_com_ref_posSIN \ |
93 |
|
|
<< m_com_ref_velSIN << m_com_ref_accSIN << m_kp_comSIN \ |
94 |
|
|
<< m_kd_comSIN << m_w_comSIN << m_kp_contactSIN \ |
95 |
|
|
<< m_kd_contactSIN << m_w_forcesSIN << m_muSIN \ |
96 |
|
|
<< m_contact_pointsSIN << m_contact_normalSIN \ |
97 |
|
|
<< m_f_minSIN << m_f_maxSIN << m_waist_ref_posSIN \ |
98 |
|
|
<< m_waist_ref_velSIN << m_waist_ref_accSIN \ |
99 |
|
|
<< m_kp_waistSIN << m_kd_waistSIN << m_w_waistSIN \ |
100 |
|
|
<< m_qSIN << m_vSIN << m_active_jointsSIN |
101 |
|
|
|
102 |
|
|
#define OUTPUT_SIGNALS \ |
103 |
|
|
m_tau_desSOUT << m_dv_desSOUT << m_v_desSOUT << m_q_desSOUT << m_uSOUT |
104 |
|
|
|
105 |
|
|
/// Define EntityClassName here rather than in the header file |
106 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
107 |
|
|
typedef SimpleInverseDyn EntityClassName; |
108 |
|
|
|
109 |
|
|
typedef Eigen::Matrix<double, 2, 1> Vector2; |
110 |
|
|
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN; |
111 |
|
|
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6; |
112 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
113 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SimpleInverseDyn, "SimpleInverseDyn"); |
114 |
|
|
|
115 |
|
|
/* ------------------------------------------------------------------- */ |
116 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
117 |
|
|
/* ------------------------------------------------------------------- */ |
118 |
|
|
SimpleInverseDyn::SimpleInverseDyn(const std::string& name) |
119 |
|
|
: Entity(name) |
120 |
|
|
|
121 |
|
|
, |
122 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector), |
123 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector), |
124 |
|
|
CONSTRUCT_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector), |
125 |
|
|
CONSTRUCT_SIGNAL_IN(kp_posture, dynamicgraph::Vector), |
126 |
|
|
CONSTRUCT_SIGNAL_IN(kd_posture, dynamicgraph::Vector), |
127 |
|
|
CONSTRUCT_SIGNAL_IN(w_posture, double), |
128 |
|
|
CONSTRUCT_SIGNAL_IN(kp_pos, dynamicgraph::Vector), |
129 |
|
|
CONSTRUCT_SIGNAL_IN(kd_pos, dynamicgraph::Vector), |
130 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector), |
131 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector), |
132 |
|
|
CONSTRUCT_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector), |
133 |
|
|
CONSTRUCT_SIGNAL_IN(kp_com, dynamicgraph::Vector), |
134 |
|
|
CONSTRUCT_SIGNAL_IN(kd_com, dynamicgraph::Vector), |
135 |
|
|
CONSTRUCT_SIGNAL_IN(w_com, double), |
136 |
|
|
CONSTRUCT_SIGNAL_IN(kp_contact, dynamicgraph::Vector), |
137 |
|
|
CONSTRUCT_SIGNAL_IN(kd_contact, dynamicgraph::Vector), |
138 |
|
|
CONSTRUCT_SIGNAL_IN(w_forces, double), |
139 |
|
|
CONSTRUCT_SIGNAL_IN(mu, double), |
140 |
|
|
CONSTRUCT_SIGNAL_IN(contact_points, dynamicgraph::Matrix), |
141 |
|
|
CONSTRUCT_SIGNAL_IN(contact_normal, dynamicgraph::Vector), |
142 |
|
|
CONSTRUCT_SIGNAL_IN(f_min, double), |
143 |
|
|
CONSTRUCT_SIGNAL_IN(f_max, double), |
144 |
|
|
CONSTRUCT_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector), |
145 |
|
|
CONSTRUCT_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector), |
146 |
|
|
CONSTRUCT_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector), |
147 |
|
|
CONSTRUCT_SIGNAL_IN(kp_waist, dynamicgraph::Vector), |
148 |
|
|
CONSTRUCT_SIGNAL_IN(kd_waist, dynamicgraph::Vector), |
149 |
|
|
CONSTRUCT_SIGNAL_IN(w_waist, double), |
150 |
|
|
CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector), |
151 |
|
|
CONSTRUCT_SIGNAL_IN(v, dynamicgraph::Vector), |
152 |
|
|
CONSTRUCT_SIGNAL_IN(active_joints, dynamicgraph::Vector), |
153 |
|
|
CONSTRUCT_SIGNAL_INNER(active_joints_checked, dg::Vector, |
154 |
|
|
m_active_jointsSIN), |
155 |
|
|
CONSTRUCT_SIGNAL_OUT(tau_des, dynamicgraph::Vector, INPUT_SIGNALS), |
156 |
|
|
CONSTRUCT_SIGNAL_OUT(dv_des, dg::Vector, m_tau_desSOUT), |
157 |
|
|
CONSTRUCT_SIGNAL_OUT(v_des, dg::Vector, m_dv_desSOUT), |
158 |
|
|
CONSTRUCT_SIGNAL_OUT(q_des, dg::Vector, m_v_desSOUT), |
159 |
|
|
CONSTRUCT_SIGNAL_OUT( |
160 |
|
|
u, dg::Vector, |
161 |
|
|
INPUT_SIGNALS << m_tau_desSOUT << m_v_desSOUT << m_q_desSOUT), |
162 |
|
|
m_t(0.0), |
163 |
|
|
m_initSucceeded(false), |
164 |
|
|
m_enabled(false), |
165 |
|
|
m_firstTime(true), |
166 |
|
|
m_timeLast(0), |
167 |
|
|
m_robot_util(RefVoidRobotUtil()), |
168 |
|
|
m_ctrlMode(CONTROL_OUTPUT_VELOCITY) { |
169 |
|
|
RESETDEBUG5(); |
170 |
|
|
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
171 |
|
|
|
172 |
|
|
m_com_offset.setZero(); |
173 |
|
|
|
174 |
|
|
/* Commands. */ |
175 |
|
|
addCommand("init", |
176 |
|
|
makeCommandVoid2(*this, &SimpleInverseDyn::init, |
177 |
|
|
docCommandVoid2("Initialize the entity.", |
178 |
|
|
"Time period in seconds (double)", |
179 |
|
|
"Robot reference (string)"))); |
180 |
|
|
/* SET of control output type. */ |
181 |
|
|
addCommand("setControlOutputType", |
182 |
|
|
makeCommandVoid1( |
183 |
|
|
*this, &SimpleInverseDyn::setControlOutputType, |
184 |
|
|
docCommandVoid1("Set the type of control output.", |
185 |
|
|
"Control type: velocity or torque (string)"))); |
186 |
|
|
|
187 |
|
|
addCommand( |
188 |
|
|
"updateComOffset", |
189 |
|
|
makeCommandVoid0( |
190 |
|
|
*this, &SimpleInverseDyn::updateComOffset, |
191 |
|
|
docCommandVoid0( |
192 |
|
|
"Update the offset on the CoM based on the CoP measurement."))); |
193 |
|
|
} |
194 |
|
|
|
195 |
|
|
void SimpleInverseDyn::updateComOffset() { |
196 |
|
|
const Vector3& com = m_robot->com(m_invDyn->data()); |
197 |
|
|
m_com_offset = com; |
198 |
|
|
SEND_MSG("CoM offset updated: " + toString(m_com_offset), MSG_TYPE_INFO); |
199 |
|
|
} |
200 |
|
|
|
201 |
|
|
void SimpleInverseDyn::setControlOutputType(const std::string& type) { |
202 |
|
|
for (int i = 0; i < CONTROL_OUTPUT_SIZE; i++) |
203 |
|
|
if (type == ControlOutput_s[i]) { |
204 |
|
|
m_ctrlMode = (ControlOutput)i; |
205 |
|
|
sotDEBUG(25) << "Control output type: " << ControlOutput_s[i] << endl; |
206 |
|
|
return; |
207 |
|
|
} |
208 |
|
|
sotDEBUG(25) << "Unrecognized control output type: " << type << endl; |
209 |
|
|
} |
210 |
|
|
|
211 |
|
|
void SimpleInverseDyn::init(const double& dt, const std::string& robotRef) { |
212 |
|
|
if (dt <= 0.0) |
213 |
|
|
return SEND_MSG("Init failed: Timestep must be positive", MSG_TYPE_ERROR); |
214 |
|
|
|
215 |
|
|
/* Retrieve m_robot_util informations */ |
216 |
|
|
std::string localName(robotRef); |
217 |
|
|
if (isNameInRobotUtil(localName)) |
218 |
|
|
m_robot_util = getRobotUtil(localName); |
219 |
|
|
else { |
220 |
|
|
SEND_MSG("You should have an entity controller manager initialized before", |
221 |
|
|
MSG_TYPE_ERROR); |
222 |
|
|
return; |
223 |
|
|
} |
224 |
|
|
|
225 |
|
|
const Eigen::Matrix<double, 3, 4>& contactPoints = m_contact_pointsSIN(0); |
226 |
|
|
const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0); |
227 |
|
|
const dg::sot::Vector6d& kp_contact = m_kp_contactSIN(0); |
228 |
|
|
const dg::sot::Vector6d& kd_contact = m_kd_contactSIN(0); |
229 |
|
|
const Eigen::Vector3d& kp_com = m_kp_comSIN(0); |
230 |
|
|
const Eigen::Vector3d& kd_com = m_kd_comSIN(0); |
231 |
|
|
const VectorN& kp_posture = m_kp_postureSIN(0); |
232 |
|
|
const VectorN& kd_posture = m_kd_postureSIN(0); |
233 |
|
|
const dg::sot::Vector6d& kp_waist = m_kp_waistSIN(0); |
234 |
|
|
const dg::sot::Vector6d& kd_waist = m_kd_waistSIN(0); |
235 |
|
|
|
236 |
|
|
assert(kp_posture.size() == |
237 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
238 |
|
|
assert(kd_posture.size() == |
239 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
240 |
|
|
|
241 |
|
|
m_w_com = m_w_comSIN(0); |
242 |
|
|
m_w_posture = m_w_postureSIN(0); |
243 |
|
|
m_w_waist = m_w_waistSIN(0); |
244 |
|
|
const double& w_forces = m_w_forcesSIN(0); |
245 |
|
|
const double& mu = m_muSIN(0); |
246 |
|
|
const double& fMin = m_f_minSIN(0); |
247 |
|
|
const double& fMax = m_f_maxSIN(0); |
248 |
|
|
|
249 |
|
|
try { |
250 |
|
|
vector<string> package_dirs; |
251 |
|
|
m_robot = |
252 |
|
|
new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs, |
253 |
|
|
pinocchio::JointModelFreeFlyer()); |
254 |
|
|
|
255 |
|
|
assert(m_robot->nv() >= 6); |
256 |
|
|
m_robot_util->m_nbJoints = m_robot->nv() - 6; |
257 |
|
|
|
258 |
|
|
m_q_sot.setZero(m_robot->nv()); |
259 |
|
|
m_v_sot.setZero(m_robot->nv()); |
260 |
|
|
m_dv_sot.setZero(m_robot->nv()); |
261 |
|
|
m_tau_sot.setZero(m_robot->nv() - 6); |
262 |
|
|
// m_f.setZero(24); |
263 |
|
|
m_q_urdf.setZero(m_robot->nq()); |
264 |
|
|
m_v_urdf.setZero(m_robot->nv()); |
265 |
|
|
m_dv_urdf.setZero(m_robot->nv()); |
266 |
|
|
|
267 |
|
|
m_invDyn = new InverseDynamicsFormulationAccForce("invdyn", *m_robot); |
268 |
|
|
|
269 |
|
|
// CONTACT 6D TASKS |
270 |
|
|
m_contactRF = |
271 |
|
|
new Contact6d("contact_rfoot", *m_robot, |
272 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name, |
273 |
|
|
contactPoints, contactNormal, mu, fMin, fMax); |
274 |
|
|
m_contactRF->Kp(kp_contact); |
275 |
|
|
m_contactRF->Kd(kd_contact); |
276 |
|
|
m_invDyn->addRigidContact(*m_contactRF, w_forces); |
277 |
|
|
|
278 |
|
|
m_contactLF = |
279 |
|
|
new Contact6d("contact_lfoot", *m_robot, |
280 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name, |
281 |
|
|
contactPoints, contactNormal, mu, fMin, fMax); |
282 |
|
|
m_contactLF->Kp(kp_contact); |
283 |
|
|
m_contactLF->Kd(kd_contact); |
284 |
|
|
m_invDyn->addRigidContact(*m_contactLF, w_forces); |
285 |
|
|
|
286 |
|
|
// TASK COM |
287 |
|
|
m_taskCom = new TaskComEquality("task-com", *m_robot); |
288 |
|
|
m_taskCom->Kp(kp_com); |
289 |
|
|
m_taskCom->Kd(kd_com); |
290 |
|
|
m_invDyn->addMotionTask(*m_taskCom, m_w_com, 0); |
291 |
|
|
|
292 |
|
|
// WAIST Task |
293 |
|
|
m_taskWaist = new TaskSE3Equality("task-waist", *m_robot, "root_joint"); |
294 |
|
|
m_taskWaist->Kp(kp_waist); |
295 |
|
|
m_taskWaist->Kd(kd_waist); |
296 |
|
|
// Add a Mask to the task which will select the vector dimensions on which |
297 |
|
|
// the task will act. In this case the waist configuration is a vector 6d |
298 |
|
|
// (position and orientation -> SE3) Here we set a mask = [0 0 0 1 1 1] so |
299 |
|
|
// the task on the waist will act on the orientation of the robot |
300 |
|
|
Eigen::VectorXd mask_orientation(6); |
301 |
|
|
mask_orientation << 0, 0, 0, 1, 1, 1; |
302 |
|
|
m_taskWaist->setMask(mask_orientation); |
303 |
|
|
// Add the task to the HQP with weight = 1.0, priority level = 1 (in the |
304 |
|
|
// cost function) and a transition duration = 0.0 |
305 |
|
|
m_invDyn->addMotionTask(*m_taskWaist, m_w_waist, 1); |
306 |
|
|
|
307 |
|
|
// POSTURE TASK |
308 |
|
|
m_taskPosture = new TaskJointPosture("task-posture", *m_robot); |
309 |
|
|
m_taskPosture->Kp(kp_posture); |
310 |
|
|
m_taskPosture->Kd(kd_posture); |
311 |
|
|
m_invDyn->addMotionTask(*m_taskPosture, m_w_posture, 1); |
312 |
|
|
|
313 |
|
|
// TRAJECTORY INIT |
314 |
|
|
m_sampleCom = TrajectorySample(3); |
315 |
|
|
m_sampleWaist = TrajectorySample(6); |
316 |
|
|
m_samplePosture = TrajectorySample(m_robot->nv() - 6); |
317 |
|
|
|
318 |
|
|
m_hqpSolver = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST, |
319 |
|
|
"eiquadprog-fast"); |
320 |
|
|
m_hqpSolver->resize(m_invDyn->nVar(), m_invDyn->nEq(), m_invDyn->nIn()); |
321 |
|
|
|
322 |
|
|
} catch (const std::exception& e) { |
323 |
|
|
std::cout << e.what(); |
324 |
|
|
return SEND_MSG( |
325 |
|
|
"Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, |
326 |
|
|
MSG_TYPE_ERROR); |
327 |
|
|
} |
328 |
|
|
m_dt = dt; |
329 |
|
|
m_initSucceeded = true; |
330 |
|
|
} |
331 |
|
|
|
332 |
|
|
/* ------------------------------------------------------------------- */ |
333 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
334 |
|
|
/* ------------------------------------------------------------------- */ |
335 |
|
|
/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To |
336 |
|
|
* all OFF)**/ |
337 |
|
|
DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector) { |
338 |
|
|
if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) |
339 |
|
|
s.resize(m_robot_util->m_nbJoints); |
340 |
|
|
|
341 |
|
|
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter); |
342 |
|
|
if (m_enabled == false) { |
343 |
|
|
if (active_joints_sot.any()) { |
344 |
|
|
/* from all OFF to some ON */ |
345 |
|
|
m_enabled = true; |
346 |
|
|
s = active_joints_sot; |
347 |
|
|
Eigen::VectorXd active_joints_urdf(m_robot_util->m_nbJoints); |
348 |
|
|
m_robot_util->joints_sot_to_urdf(active_joints_sot, active_joints_urdf); |
349 |
|
|
|
350 |
|
|
m_taskBlockedJoints = |
351 |
|
|
new TaskJointPosture("task-posture-blocked", *m_robot); |
352 |
|
|
Eigen::VectorXd blocked_joints(m_robot_util->m_nbJoints); |
353 |
|
|
for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) |
354 |
|
|
if (active_joints_urdf(i) == 0.0) |
355 |
|
|
blocked_joints(i) = 1.0; |
356 |
|
|
else |
357 |
|
|
blocked_joints(i) = 0.0; |
358 |
|
|
SEND_MSG("Blocked joints: " + toString(blocked_joints.transpose()), |
359 |
|
|
MSG_TYPE_INFO); |
360 |
|
|
m_taskBlockedJoints->mask(blocked_joints); |
361 |
|
|
TrajectorySample ref_zero( |
362 |
|
|
static_cast<unsigned int>(m_robot_util->m_nbJoints)); |
363 |
|
|
m_taskBlockedJoints->setReference(ref_zero); |
364 |
|
|
m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0); |
365 |
|
|
} |
366 |
|
|
} else if (!active_joints_sot.any()) { |
367 |
|
|
/* from some ON to all OFF */ |
368 |
|
|
m_enabled = false; |
369 |
|
|
} |
370 |
|
|
if (m_enabled == false) |
371 |
|
|
for (unsigned int i = 0; i < m_robot_util->m_nbJoints; i++) s(i) = false; |
372 |
|
|
return s; |
373 |
|
|
} |
374 |
|
|
|
375 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(tau_des, dynamicgraph::Vector) { |
376 |
|
|
if (!m_initSucceeded) { |
377 |
|
|
SEND_WARNING_STREAM_MSG( |
378 |
|
|
"Cannot compute signal tau_des before initialization!"); |
379 |
|
|
return s; |
380 |
|
|
} |
381 |
|
|
if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) |
382 |
|
|
s.resize(m_robot_util->m_nbJoints); |
383 |
|
|
|
384 |
|
|
getProfiler().start(PROFILE_TAU_DES_COMPUTATION); |
385 |
|
|
|
386 |
|
|
getProfiler().start(PROFILE_READ_INPUT_SIGNALS); |
387 |
|
|
|
388 |
|
|
m_active_joints_checkedSINNER(iter); |
389 |
|
|
|
390 |
|
|
const VectorN6& q_robot = m_qSIN(iter); |
391 |
|
|
assert(q_robot.size() == |
392 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
393 |
|
|
const VectorN6& v_robot = m_vSIN(iter); |
394 |
|
|
assert(v_robot.size() == |
395 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
396 |
|
|
|
397 |
|
|
const Vector3& x_com_ref = m_com_ref_posSIN(iter); |
398 |
|
|
const Vector3& dx_com_ref = m_com_ref_velSIN(iter); |
399 |
|
|
const Vector3& ddx_com_ref = m_com_ref_accSIN(iter); |
400 |
|
|
const VectorN& x_waist_ref = m_waist_ref_posSIN(iter); |
401 |
|
|
const Vector6& dx_waist_ref = m_waist_ref_velSIN(iter); |
402 |
|
|
const Vector6& ddx_waist_ref = m_waist_ref_accSIN(iter); |
403 |
|
|
const VectorN& q_ref = m_posture_ref_posSIN(iter); |
404 |
|
|
assert(q_ref.size() == |
405 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
406 |
|
|
const VectorN& dq_ref = m_posture_ref_velSIN(iter); |
407 |
|
|
assert(dq_ref.size() == |
408 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
409 |
|
|
const VectorN& ddq_ref = m_posture_ref_accSIN(iter); |
410 |
|
|
assert(ddq_ref.size() == |
411 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
412 |
|
|
|
413 |
|
|
const Vector6& kp_contact = m_kp_contactSIN(iter); |
414 |
|
|
const Vector6& kd_contact = m_kd_contactSIN(iter); |
415 |
|
|
const Vector3& kp_com = m_kp_comSIN(iter); |
416 |
|
|
const Vector3& kd_com = m_kd_comSIN(iter); |
417 |
|
|
const Vector6& kp_waist = m_kp_waistSIN(iter); |
418 |
|
|
const Vector6& kd_waist = m_kd_waistSIN(iter); |
419 |
|
|
|
420 |
|
|
const VectorN& kp_posture = m_kp_postureSIN(iter); |
421 |
|
|
assert(kp_posture.size() == |
422 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
423 |
|
|
const VectorN& kd_posture = m_kd_postureSIN(iter); |
424 |
|
|
assert(kd_posture.size() == |
425 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
426 |
|
|
|
427 |
|
|
/*const double & w_hands = m_w_handsSIN(iter);*/ |
428 |
|
|
const double& w_com = m_w_comSIN(iter); |
429 |
|
|
const double& w_posture = m_w_postureSIN(iter); |
430 |
|
|
const double& w_forces = m_w_forcesSIN(iter); |
431 |
|
|
const double& w_waist = m_w_waistSIN(iter); |
432 |
|
|
|
433 |
|
|
getProfiler().stop(PROFILE_READ_INPUT_SIGNALS); |
434 |
|
|
|
435 |
|
|
getProfiler().start(PROFILE_PREPARE_INV_DYN); |
436 |
|
|
|
437 |
|
|
m_sampleCom.pos = x_com_ref - m_com_offset; |
438 |
|
|
m_sampleCom.vel = dx_com_ref; |
439 |
|
|
m_sampleCom.acc = ddx_com_ref; |
440 |
|
|
m_taskCom->setReference(m_sampleCom); |
441 |
|
|
m_taskCom->Kp(kp_com); |
442 |
|
|
m_taskCom->Kd(kd_com); |
443 |
|
|
if (m_w_com != w_com) { |
444 |
|
|
m_w_com = w_com; |
445 |
|
|
m_invDyn->updateTaskWeight(m_taskCom->name(), w_com); |
446 |
|
|
} |
447 |
|
|
|
448 |
|
|
m_sampleWaist.pos = x_waist_ref; |
449 |
|
|
m_sampleWaist.vel = dx_waist_ref; |
450 |
|
|
m_sampleWaist.acc = ddx_waist_ref; |
451 |
|
|
m_taskWaist->setReference(m_sampleWaist); |
452 |
|
|
m_taskWaist->Kp(kp_waist); |
453 |
|
|
m_taskWaist->Kd(kd_waist); |
454 |
|
|
if (m_w_waist != w_waist) { |
455 |
|
|
m_w_waist = w_waist; |
456 |
|
|
m_invDyn->updateTaskWeight(m_taskWaist->name(), w_waist); |
457 |
|
|
} |
458 |
|
|
|
459 |
|
|
m_robot_util->joints_sot_to_urdf(q_ref, m_samplePosture.pos); |
460 |
|
|
m_robot_util->joints_sot_to_urdf(dq_ref, m_samplePosture.vel); |
461 |
|
|
m_robot_util->joints_sot_to_urdf(ddq_ref, m_samplePosture.acc); |
462 |
|
|
m_taskPosture->setReference(m_samplePosture); |
463 |
|
|
m_taskPosture->Kp(kp_posture); |
464 |
|
|
m_taskPosture->Kd(kd_posture); |
465 |
|
|
if (m_w_posture != w_posture) { |
466 |
|
|
m_w_posture = w_posture; |
467 |
|
|
m_invDyn->updateTaskWeight(m_taskPosture->name(), w_posture); |
468 |
|
|
} |
469 |
|
|
|
470 |
|
|
const double& fMin = m_f_minSIN(0); |
471 |
|
|
const double& fMax = m_f_maxSIN(iter); |
472 |
|
|
m_contactLF->setMinNormalForce(fMin); |
473 |
|
|
m_contactRF->setMinNormalForce(fMin); |
474 |
|
|
m_contactLF->setMaxNormalForce(fMax); |
475 |
|
|
m_contactRF->setMaxNormalForce(fMax); |
476 |
|
|
m_contactLF->Kp(kp_contact); |
477 |
|
|
m_contactLF->Kd(kd_contact); |
478 |
|
|
m_contactRF->Kp(kp_contact); |
479 |
|
|
m_contactRF->Kd(kd_contact); |
480 |
|
|
m_invDyn->updateRigidContactWeights(m_contactLF->name(), w_forces); |
481 |
|
|
m_invDyn->updateRigidContactWeights(m_contactRF->name(), w_forces); |
482 |
|
|
|
483 |
|
|
if (m_firstTime) { |
484 |
|
|
m_firstTime = false; |
485 |
|
|
m_robot_util->config_sot_to_urdf(q_robot, m_q_urdf); |
486 |
|
|
m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, m_v_urdf); |
487 |
|
|
|
488 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
489 |
|
|
// m_robot->computeAllTerms(m_invDyn->data(), q, v); |
490 |
|
|
pinocchio::SE3 H_lf = m_robot->position( |
491 |
|
|
m_invDyn->data(), |
492 |
|
|
m_robot->model().getJointId( |
493 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
494 |
|
|
m_contactLF->setReference(H_lf); |
495 |
|
|
SEND_MSG("Setting left foot reference to " + toString(H_lf), |
496 |
|
|
MSG_TYPE_DEBUG); |
497 |
|
|
|
498 |
|
|
pinocchio::SE3 H_rf = m_robot->position( |
499 |
|
|
m_invDyn->data(), |
500 |
|
|
m_robot->model().getJointId( |
501 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
502 |
|
|
m_contactRF->setReference(H_rf); |
503 |
|
|
SEND_MSG("Setting right foot reference to " + toString(H_rf), |
504 |
|
|
MSG_TYPE_DEBUG); |
505 |
|
|
} else if (m_timeLast != static_cast<unsigned int>(iter - 1)) { |
506 |
|
|
SEND_MSG("Last time " + toString(m_timeLast) + |
507 |
|
|
" is not current time-1: " + toString(iter), |
508 |
|
|
MSG_TYPE_ERROR); |
509 |
|
|
if (m_timeLast == static_cast<unsigned int>(iter)) { |
510 |
|
|
s = m_tau_sot; |
511 |
|
|
return s; |
512 |
|
|
} |
513 |
|
|
} |
514 |
|
|
// else if (m_ctrlMode == CONTROL_OUTPUT_TORQUE){ |
515 |
|
|
// // In velocity close the TSID loop on itself (v_des, q_des), in torque on |
516 |
|
|
// the (q,v) of the robot. m_robot_util->config_sot_to_urdf(q_robot, |
517 |
|
|
// m_q_urdf); m_robot_util->velocity_sot_to_urdf(m_q_urdf, v_robot, |
518 |
|
|
// m_v_urdf); |
519 |
|
|
// } |
520 |
|
|
m_timeLast = static_cast<unsigned int>(iter); |
521 |
|
|
|
522 |
|
|
const HQPData& hqpData = |
523 |
|
|
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf); |
524 |
|
|
|
525 |
|
|
getProfiler().stop(PROFILE_PREPARE_INV_DYN); |
526 |
|
|
|
527 |
|
|
getProfiler().start(PROFILE_HQP_SOLUTION); |
528 |
|
|
SolverHQPBase* solver = m_hqpSolver; |
529 |
|
|
getStatistics().store("solver dynamic size", 1.0); |
530 |
|
|
|
531 |
|
|
const HQPOutput& sol = solver->solve(hqpData); |
532 |
|
|
getProfiler().stop(PROFILE_HQP_SOLUTION); |
533 |
|
|
|
534 |
|
|
if (sol.status != HQP_STATUS_OPTIMAL) { |
535 |
|
|
SEND_ERROR_STREAM_MSG("HQP solver failed to find a solution: " + |
536 |
|
|
toString(sol.status)); |
537 |
|
|
SEND_DEBUG_STREAM_MSG(tsid::solvers::HQPDataToString(hqpData, false)); |
538 |
|
|
SEND_DEBUG_STREAM_MSG("q=" + toString(q_robot.transpose(), 1, 5)); |
539 |
|
|
SEND_DEBUG_STREAM_MSG("v=" + toString(v_robot.transpose(), 1, 5)); |
540 |
|
|
s.setZero(); |
541 |
|
|
return s; |
542 |
|
|
} |
543 |
|
|
|
544 |
|
|
getStatistics().store("active inequalities", |
545 |
|
|
static_cast<double>(sol.activeSet.size())); |
546 |
|
|
getStatistics().store("solver iterations", sol.iterations); |
547 |
|
|
|
548 |
|
|
m_dv_urdf = m_invDyn->getAccelerations(sol); |
549 |
|
|
m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_dv_urdf, m_dv_sot); |
550 |
|
|
m_robot_util->joints_urdf_to_sot(m_invDyn->getActuatorForces(sol), m_tau_sot); |
551 |
|
|
|
552 |
|
|
getProfiler().stop(PROFILE_TAU_DES_COMPUTATION); |
553 |
|
|
m_t += m_dt; |
554 |
|
|
|
555 |
|
|
s = m_tau_sot; |
556 |
|
|
|
557 |
|
|
return s; |
558 |
|
|
} |
559 |
|
|
|
560 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(dv_des, dynamicgraph::Vector) { |
561 |
|
|
if (!m_initSucceeded) { |
562 |
|
|
SEND_WARNING_STREAM_MSG( |
563 |
|
|
"Cannot compute signal dv_des before initialization!"); |
564 |
|
|
return s; |
565 |
|
|
} |
566 |
|
|
if (s.size() != m_robot->nv()) s.resize(m_robot->nv()); |
567 |
|
|
m_tau_desSOUT(iter); |
568 |
|
|
s = m_dv_sot; |
569 |
|
|
return s; |
570 |
|
|
} |
571 |
|
|
|
572 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_des, dynamicgraph::Vector) { |
573 |
|
|
if (!m_initSucceeded) { |
574 |
|
|
SEND_WARNING_STREAM_MSG( |
575 |
|
|
"Cannot compute signal v_des before initialization!"); |
576 |
|
|
return s; |
577 |
|
|
} |
578 |
|
|
if (s.size() != m_robot->nv()) s.resize(m_robot->nv()); |
579 |
|
|
m_dv_desSOUT(iter); |
580 |
|
|
tsid::math::Vector v_mean; |
581 |
|
|
v_mean = m_v_urdf + 0.5 * m_dt * m_dv_urdf; |
582 |
|
|
m_v_urdf = m_v_urdf + m_dt * m_dv_urdf; |
583 |
|
|
m_q_urdf = pinocchio::integrate(m_robot->model(), m_q_urdf, m_dt * v_mean); |
584 |
|
|
m_robot_util->velocity_urdf_to_sot(m_q_urdf, m_v_urdf, m_v_sot); |
585 |
|
|
s = m_v_sot; |
586 |
|
|
return s; |
587 |
|
|
} |
588 |
|
|
|
589 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(q_des, dynamicgraph::Vector) { |
590 |
|
|
if (!m_initSucceeded) { |
591 |
|
|
SEND_WARNING_STREAM_MSG( |
592 |
|
|
"Cannot compute signal q_des before initialization!"); |
593 |
|
|
return s; |
594 |
|
|
} |
595 |
|
|
if (s.size() != m_robot->nv()) s.resize(m_robot->nv()); |
596 |
|
|
m_v_desSOUT(iter); |
597 |
|
|
m_robot_util->config_urdf_to_sot(m_q_urdf, m_q_sot); |
598 |
|
|
s = m_q_sot; |
599 |
|
|
return s; |
600 |
|
|
} |
601 |
|
|
|
602 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) { |
603 |
|
|
if (!m_initSucceeded) { |
604 |
|
|
SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!"); |
605 |
|
|
return s; |
606 |
|
|
} |
607 |
|
|
if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)) |
608 |
|
|
s.resize(m_robot_util->m_nbJoints); |
609 |
|
|
|
610 |
|
|
const VectorN& kp_pos = m_kp_posSIN(iter); |
611 |
|
|
assert(kp_pos.size() == |
612 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
613 |
|
|
const VectorN& kd_pos = m_kd_posSIN(iter); |
614 |
|
|
assert(kd_pos.size() == |
615 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints)); |
616 |
|
|
|
617 |
|
|
const VectorN6& q_robot = m_qSIN(iter); |
618 |
|
|
assert(q_robot.size() == |
619 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
620 |
|
|
const VectorN6& v_robot = m_vSIN(iter); |
621 |
|
|
assert(v_robot.size() == |
622 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)); |
623 |
|
|
|
624 |
|
|
m_q_desSOUT(iter); |
625 |
|
|
|
626 |
|
|
s = m_tau_sot + |
627 |
|
|
kp_pos.cwiseProduct(m_q_sot.tail(m_robot_util->m_nbJoints) - |
628 |
|
|
q_robot.tail(m_robot_util->m_nbJoints)) + |
629 |
|
|
kd_pos.cwiseProduct(m_v_sot.tail(m_robot_util->m_nbJoints) - |
630 |
|
|
v_robot.tail(m_robot_util->m_nbJoints)); |
631 |
|
|
|
632 |
|
|
return s; |
633 |
|
|
} |
634 |
|
|
|
635 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
636 |
|
|
|
637 |
|
|
/* ------------------------------------------------------------------- */ |
638 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
639 |
|
|
/* ------------------------------------------------------------------- */ |
640 |
|
|
|
641 |
|
|
void SimpleInverseDyn::display(std::ostream& os) const { |
642 |
|
|
os << "SimpleInverseDyn " << getName(); |
643 |
|
|
try { |
644 |
|
|
getProfiler().report_all(3, os); |
645 |
|
|
getStatistics().report_all(1, os); |
646 |
|
|
os << "QP size: nVar " << m_invDyn->nVar() << " nEq " << m_invDyn->nEq() |
647 |
|
|
<< " nIn " << m_invDyn->nIn() << "\n"; |
648 |
|
|
} catch (ExceptionSignal e) { |
649 |
|
|
} |
650 |
|
|
} |
651 |
|
|
} // namespace torque_control |
652 |
|
|
} // namespace sot |
653 |
|
|
} // namespace dynamicgraph |