sot-dynamic.cpp 31.4 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 * This file is part of sot-dynamic.
 * sot-dynamic is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-dynamic is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-dynamic.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <sot/core/debug.hh>
Rohan Budhiraja's avatar
Rohan Budhiraja committed
21

22
23
24
25
26
27
#include <sot-dynamic/dynamic.h>

#include <boost/version.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

28
29
30
31
32
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/algorithm/crba.hpp>

33
34
35
36
37
38
39
40
#include <dynamic-graph/all-commands.h>

#include "../src/dynamic-command.h"


using namespace dynamicgraph::sot;
using namespace dynamicgraph;

41
const std::string dg::sot::Dynamic::CLASS_NAME = "Dynamic";
42
43

Dynamic::
44
Dynamic( const std::string & name)
45
  :Entity(name)
46
  ,m_model(NULL)
47
  ,m_data(NULL)
48
49
50
51
52
53
54
55
56

  ,jointPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::position")
  ,freeFlyerPositionSIN(NULL,"sotDynamic("+name+")::input(vector)::ffposition")
  ,jointVelocitySIN(NULL,"sotDynamic("+name+")::input(vector)::velocity")
  ,freeFlyerVelocitySIN(NULL,"sotDynamic("+name+")::input(vector)::ffvelocity")
  ,jointAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::acceleration")
  ,freeFlyerAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::ffacceleration")

  ,newtonEulerSINTERN( boost::bind(&Dynamic::computeNewtonEuler,this,_1,_2),
57
		       jointPositionSIN<<freeFlyerPositionSIN
58
59
		       <<jointVelocitySIN<<freeFlyerVelocitySIN
		       <<jointAccelerationSIN<<freeFlyerAccelerationSIN,
60
		       "sotDynamic("+name+")::intern(dummy)::newtoneuler" )
61
   
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
  ,zmpSOUT( boost::bind(&Dynamic::computeZmp,this,_1,_2),
	    newtonEulerSINTERN,
	    "sotDynamic("+name+")::output(vector)::zmp" )
  ,JcomSOUT( boost::bind(&Dynamic::computeJcom,this,_1,_2),
	     newtonEulerSINTERN,
	     "sotDynamic("+name+")::output(matrix)::Jcom" )
  ,comSOUT( boost::bind(&Dynamic::computeCom,this,_1,_2),
	    newtonEulerSINTERN,
	    "sotDynamic("+name+")::output(vector)::com" )
  ,inertiaSOUT( boost::bind(&Dynamic::computeInertia,this,_1,_2),
		newtonEulerSINTERN,
		"sotDynamic("+name+")::output(matrix)::inertia" )
  ,footHeightSOUT( boost::bind(&Dynamic::computeFootHeight,this,_1,_2),
		   newtonEulerSINTERN,
		   "sotDynamic("+name+")::output(double)::footHeight" )
77
   
78
  ,upperJlSOUT( boost::bind(&Dynamic::getUpperPositionLimits,this,_1,_2),
79
80
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::upperJl" )
81
   
82
  ,lowerJlSOUT( boost::bind(&Dynamic::getLowerPositionLimits,this,_1,_2),
83
84
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::lowerJl" )
85
   
86
  ,upperVlSOUT( boost::bind(&Dynamic::getUpperVelocityLimits,this,_1,_2),
87
88
89
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::upperVl" )
   
90
  ,upperTlSOUT( boost::bind(&Dynamic::getMaxEffortLimits,this,_1,_2),
91
92
93
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::upperTl" )
   
94
95
96
97
98
99
  ,inertiaRotorSOUT( "sotDynamic("+name+")::output(matrix)::inertiaRotor" )
  ,gearRatioSOUT( "sotDynamic("+name+")::output(matrix)::gearRatio" )
  ,inertiaRealSOUT( boost::bind(&Dynamic::computeInertiaReal,this,_1,_2),
		    inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
		    "sotDynamic("+name+")::output(matrix)::inertiaReal" )
  ,MomentaSOUT( boost::bind(&Dynamic::computeMomenta,this,_1,_2),
100
		inertiaSOUT,
101
102
		"sotDynamic("+name+")::output(vector)::momenta" )
  ,AngularMomentumSOUT( boost::bind(&Dynamic::computeAngularMomentum,this,_1,_2),
103
			inertiaSOUT,
104
105
106
107
108
109
			"sotDynamic("+name+")::output(vector)::angularmomentum" )
  ,dynamicDriftSOUT( boost::bind(&Dynamic::computeTorqueDrift,this,_1,_2),
		     newtonEulerSINTERN,
		     "sotDynamic("+name+")::output(vector)::dynamicDrift" )
{

110
  sotDEBUGIN(5);
111

112
113
114
115
  //TODO-------------------------------------------
 
  //if( build ) buildModel();
  //firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
116
  //DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
117
  //endTODO--------------------------------------------
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136

  signalRegistration(jointPositionSIN);
  signalRegistration(freeFlyerPositionSIN);
  signalRegistration(jointVelocitySIN);
  signalRegistration(freeFlyerVelocitySIN);
  signalRegistration(jointAccelerationSIN);
  signalRegistration(freeFlyerAccelerationSIN);
  signalRegistration(zmpSOUT);
  signalRegistration(comSOUT);
  signalRegistration(JcomSOUT);
  signalRegistration(footHeightSOUT);
  signalRegistration(upperJlSOUT);
  signalRegistration(lowerJlSOUT);
  signalRegistration(upperVlSOUT);
  signalRegistration(upperTlSOUT);
  signalRegistration(inertiaSOUT);
  signalRegistration(inertiaRealSOUT);
  signalRegistration(inertiaRotorSOUT);
  signalRegistration(gearRatioSOUT);
137
  signalRegistration(MomentaSOUT);
138
139
140
141
142
143
144
145
  signalRegistration(AngularMomentumSOUT);
  signalRegistration(dynamicDriftSOUT);

  //
  // Commands
  //
  std::string docstring;
  // setFiles
146

147
148
  docstring =
    "\n"
149
    "    Display the current robot configuration.\n"
150
    "\n"
151
152
    "      Input:\n"
    "        - none \n"
153
    "\n";
154
155
  addCommand("displayModel",
	     new command::DisplayModel(*this, docstring));
156
157
158
159
160
161
162
163
164
  docstring = "    \n"
    "    Get the dimension of the robot configuration.\n"
    "    \n"
    "      Return:\n"
    "        an unsigned int: the dimension.\n"
    "    \n";
  addCommand("getDimension",
	     new command::GetDimension(*this, docstring));

165
166
167
168
  {
    using namespace ::dg::command;
    // CreateOpPoint
    //TODO add operational joints
169
    docstring = "    \n"
170
      "    Create an operational point attached to a robot joint local frame.\n"
171
      "    \n"
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
      "      Input: \n"
      "        - a string: name of the operational point,\n"
      "        - a string: name the joint, or among (gaze, left-ankle, right ankle\n"
      "          , left-wrist, right-wrist, waist, chest).\n"
      "\n";
    addCommand("createOpPoint",
	       makeCommandVoid2(*this,&Dynamic::cmd_createOpPointSignals,
				docstring));
    
    docstring = docCommandVoid2("Create a jacobian (world frame) signal only for one joint.",
				"string (signal name)","string (joint name)");
    addCommand("createJacobian",
	       makeCommandVoid2(*this,&Dynamic::cmd_createJacobianWorldSignal,
				docstring));
    
    docstring = docCommandVoid2("Create a jacobian (endeff frame) signal only for one joint.",
				"string (signal name)","string (joint name)");
    addCommand("createJacobianEndEff",
	       makeCommandVoid2(*this,&Dynamic::cmd_createJacobianEndEffectorSignal,
				docstring));
    
    docstring = docCommandVoid2("Create a position (matrix homo) signal only for one joint.",
				"string (signal name)","string (joint name)");
    addCommand("createPosition",
	       makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring));
  }
  
199
  sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl;
200
201
  sotDEBUGOUT(5);
}
202

203
Dynamic::~Dynamic( void ) {
204
205
  sotDEBUGIN(15);
    //  if (0!=m_model){ delete m_model; m_model=NULL;}
206
207
208
209
210
  if (0!=m_data)
    { delete m_data; m_data=NULL; }
  if (0!=m_model)
    { delete m_model; m_model=NULL; }

211
212
213
214
215
216
  for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
       iter != genericSignalRefs.end();
       ++iter) {
    SignalBase<int>* sigPtr = *iter;
    delete sigPtr;
  }
217
218
219
  sotDEBUGOUT(15);
}

220
221
222
223
/*--------------------------------GETTERS-------------------------------------------*/

dg::Vector& Dynamic::
getLowerPositionLimits(dg::Vector& res, const int&)
224
{
225
 
226
  sotDEBUGIN(15);
227
228
229
230
231
  assert(m_model);

  res.resize(m_model->nq);
  res = m_model->lowerPositionLimit;

232
233
234
  sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
235
}
236
237
238

dg::Vector& Dynamic::
getUpperPositionLimits(dg::Vector& res, const int&)
239
{
240
  sotDEBUGIN(15);
241
242
243
244
245
  assert(m_model);

  res.resize(m_model->nq);
  res = m_model->upperPositionLimit;

246
247
248
  sotDEBUG(15) << "upperLimit (" << res << ")=" <<std::endl;
  sotDEBUGOUT(15);
  return res;
249
}
250
251
252

dg::Vector& Dynamic::
getUpperVelocityLimits(dg::Vector& res, const int&)
253
{
254
  sotDEBUGIN(15);
255
256
257
258
259
  assert(m_model);

  res.resize(m_model->nv);
  res = m_model->velocityLimit;

260
261
262
  sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" <<std::endl;
  sotDEBUGOUT(15);
  return res;
263
}
264
265
266

dg::Vector& Dynamic::
getMaxEffortLimits(dg::Vector& res, const int&)
267
{
268
  sotDEBUGIN(15);
269
  assert(m_model);
270

271
272
  res.resize(m_model->nv);
  res = m_model->effortLimit;
273

274
  sotDEBUGOUT(15);
275
276
  return res;
}
277

278

279
280
/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector Dynamic::getPinocchioPos(int time)
281
{
282
283
284
  sotDEBUGIN(15);
  dg::Vector qJoints=jointPositionSIN.access(time);
  dg::Vector q;
285
286
  assert(m_model);

287
288
  if( freeFlyerPositionSIN) {
    dg::Vector qFF=freeFlyerPositionSIN.access(time);
289
290


291
    q.resize(qJoints.size() + 7);
292
293
294
295
296
297
298
299

    Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
      Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
      Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
    
    q << qFF(0),qFF(1),qFF(2),
      quat.x(),quat.y(),quat.z(),quat.w(),
      qJoints;
300
  }
301
  else if (se3::nv(m_model->joints[1]) == 6){
302
303
    dg::Vector qFF = qJoints.head<6>();
    q.resize(qJoints.size()+1);
304
305
306
307
308
309
310
311

    Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
      Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
      Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
    q << qFF(0),qFF(1),qFF(2),
      quat.x(),quat.y(),quat.z(),quat.w(),
      qJoints.segment(6,qJoints.size()-6);

312
    assert(q.size() == m_model->nq);
313
314
  }
  else {
315
316
    q.resize(qJoints.size());
    q=qJoints;
317
  }
318

319
320
  sotDEBUGOUT(15) <<"Position out"<<q<<std::endl;
  return q;
321
}
322

323
324
325
326
327
328
329
330
331
Eigen::VectorXd Dynamic::getPinocchioVel(int time)
{
  const Eigen::VectorXd vJoints=jointVelocitySIN.access(time);
  if(freeFlyerVelocitySIN){
    const Eigen::VectorXd vFF=freeFlyerVelocitySIN.access(time);
    Eigen::VectorXd v(vJoints.size() + vFF.size());
    v << vFF,vJoints;
    return v;
  }
332
333
  else 
    return vJoints;
334
335
}

336
337
338
339
340
341
342
343
344
345
346
Eigen::VectorXd Dynamic::getPinocchioAcc(int time)
{
  const Eigen::VectorXd aJoints=jointAccelerationSIN.access(time);
  if(freeFlyerAccelerationSIN){
    const Eigen::VectorXd aFF=freeFlyerAccelerationSIN.access(time);
    Eigen::VectorXd a(aJoints.size() + aFF.size());
    a << aFF,aJoints;
    return a;
  }
  else return aJoints;
}
347
348

/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
349
350
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createJacobianSignal( const std::string& signame, const std::string& jointName )
351
{
352
  sotDEBUGIN(15);
353
  assert(m_model);
354
  dg::SignalTimeDependent< dg::Matrix,int > * sig;
355
356
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
357
358
359
360
361
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericJacobian,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
362
363
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName);
364
365
366
367
368
369
370
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericJacobian,this,false,jointId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
  else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
				  "Robot has no joint corresponding to " + jointName);
371
372
373

  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
374
  sotDEBUGOUT(15);
375
376
377
  return *sig;
}

378
379
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createEndeffJacobianSignal( const std::string& signame, const std::string& jointName )
380
381
{
  sotDEBUGIN(15);
382
  assert(m_model);
383
  dg::SignalTimeDependent< dg::Matrix,int > * sig;
384
385
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
386
387
388
389
390
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
391
392
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName); 
393
394
395
396
397
398
399
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,false,jointId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
  else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
				  "Robot has no joint corresponding to " + jointName);
400
401
402
403
404
405
406
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
407
createPositionSignal( const std::string& signame, const std::string& jointName)
408
409
{
  sotDEBUGIN(15);
410
  assert(m_model);
411
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig;
412
413
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
414
415
416
417
418
    sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
      ( boost::bind(&Dynamic::computeGenericPosition,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrixHomo)::"+signame );
  }
419
420
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName); 
421
422
423
424
425
426
427
428
    sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
      ( boost::bind(&Dynamic::computeGenericPosition,this,false,jointId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrixHomo)::"+signame );
  }
  else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
				  "Robot has no joint corresponding to " + jointName);
  
429
430
431
432
433
434
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
  sotDEBUGOUT(15);
  return *sig;
}

435
436
SignalTimeDependent< dg::Vector,int >& Dynamic::
createVelocitySignal( const std::string& signame,const std::string& jointName )
437
{
438
  sotDEBUGIN(15);
439
440
  assert(m_model);
  int jointId = m_model->getJointId(jointName);
441

442
443
444
445
446
447
448
  SignalTimeDependent< dg::Vector,int > * sig
    = new SignalTimeDependent< dg::Vector,int >
    ( boost::bind(&Dynamic::computeGenericVelocity,this,jointId,_1,_2),
      newtonEulerSINTERN,
      "sotDynamic("+name+")::output(dg::Vector)::"+signame );
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
449

450
451
  sotDEBUGOUT(15);
  return *sig;
452
453
}

454
455
dg::SignalTimeDependent< dg::Vector,int >& Dynamic::
createAccelerationSignal( const std::string& signame, const std::string& jointName)
456
457
{
  sotDEBUGIN(15);
458
459
  assert(m_model);
  int jointId = m_model->getJointId(jointName);
460
461
462
  dg::SignalTimeDependent< dg::Vector,int > * sig
    = new dg::SignalTimeDependent< dg::Vector,int >
    ( boost::bind(&Dynamic::computeGenericAcceleration,this,jointId,_1,_2),
463
      newtonEulerSINTERN,
464
465
      "sotDynamic("+name+")::output(matrixHomo)::"+signame );

466
467
468
469
470
471
472
473
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );

  sotDEBUGOUT(15);
  return *sig;
}


474
475


476
void Dynamic::
477
destroyJacobianSignal( const std::string& signame )
478
{
479
  sotDEBUGIN(15);
480

481
  bool deletable = false;
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
  dg::SignalTimeDependent< dg::Matrix,int > * sig = & jacobiansSOUT( signame );
  for(std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
      iter != genericSignalRefs.end();
      ++iter) {
    if( (*iter) == sig ) {
      genericSignalRefs.erase(iter); deletable = true;
      break;
    }
  }

  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				     "Cannot destroy signal",
				     " (while trying to remove generic jac. signal <%s>).",
				     signame.c_str() );
    }
  signalDeregistration( signame );
  delete sig;
}

void Dynamic::
destroyPositionSignal( const std::string& signame )
{
506
  sotDEBUGIN(15);
507
508
  bool deletable = false;
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig = & positionsSOUT( signame );
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
  for(  std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
	iter != genericSignalRefs.end();
	++iter )
    {
      if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
    }

  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				     "Cannot destroy signal",
				     " (while trying to remove generic pos. signal <%s>).",
				     signame.c_str() );
    }

  signalDeregistration( signame );

  delete sig;
}

529
530
void Dynamic::
destroyVelocitySignal( const std::string& signame )
531
{
532
  sotDEBUGIN(15);
533
534
535
536
537
538
539
540
  bool deletable = false;
  SignalTimeDependent< dg::Vector,int > * sig = & velocitiesSOUT( signame );
  for(  std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
	iter != genericSignalRefs.end();
	++iter )
    {
      if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
    }
541

542
543
544
545
546
547
548
  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				     "Cannot destroy signal",
				     " (while trying to remove generic pos. signal <%s>).",
				     signame.c_str() );
    }
549

550
  signalDeregistration( signame );
551

552
553
  delete sig;
}
554
555
556
557

void Dynamic::
destroyAccelerationSignal( const std::string& signame )
{
558
  sotDEBUGIN(15);
559
  bool deletable = false;
560
  dg::SignalTimeDependent< dg::Vector,int > * sig = & accelerationsSOUT( signame );
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
  for(  std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
	iter != genericSignalRefs.end();
	++iter )
    {
      if( (*iter) == sig ) { genericSignalRefs.erase(iter); deletable = true; break; }
    }

  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				  getName() + ":cannot destroy signal",
				  " (while trying to remove generic acc "
				  "signal <%s>).",
				  signame.c_str() );
    }

  signalDeregistration( signame );

  delete sig;
}

582
/* --------------------- COMPUTE ------------------------------------------------- */
583

584
dg::Vector& Dynamic::computeZmp( dg::Vector& res,int time )
585
{
586
587
    //TODO: To be verified
    sotDEBUGIN(25);
588
    assert(m_data);
589
590
591
    if (res.size()!=3)
        res.resize(3);
    newtonEulerSINTERN(time);
592

593
594
595
596
597
598
599
600
601
602
    se3::Force ftau = m_data->oMi[1].act(m_data->f[1]);
    se3::Force::Vector3 tau = ftau.angular();
    se3::Force::Vector3 f = ftau.linear();
    res(0) = -tau[1]/f[2];
    res(1) = tau[0]/f[2];
    res(2) = 0;

    sotDEBUGOUT(25);

    return res;
603
604
}

605
//In world coordinates
606
dg::Matrix& Dynamic::
607
computeGenericJacobian(bool isFrame, int jointId, dg::Matrix& res,int time )
608
609
{
  sotDEBUGIN(25);
610
611
  assert(m_model);
  assert(m_data);
612
  newtonEulerSINTERN(time);
613
614
  res.resize(6,m_model->nv);
  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
615

616
  se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
617
618
  //Computes Jacobian in world coordinates. 
  if(isFrame){
619
620
    se3::framesForwardKinematics(*m_model,*m_data);
    se3::getFrameJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
621
  }
622
  else se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
623
  res = m_output;
624
625
626
627
  sotDEBUGOUT(25);
  return res;
}

628
dg::Matrix& Dynamic::
629
computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time )
630
631
{
  sotDEBUGIN(25);
632
633
  assert(m_model);
  assert(m_data);
634
  newtonEulerSINTERN(time);
635
  res.resize(6,m_model->nv);
636
  //In local coordinates.
637
638
  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
  se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
639
640

  if(isFrame){
641
642
    se3::framesForwardKinematics(*m_model,*m_data);
    se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
643
  }
644
  else se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
645

646
  res = m_output;
647
648
649
650
651
652
  sotDEBUGOUT(25);

  return res;
}

MatrixHomogeneous& Dynamic::
653
computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int time)
654
655
{
  sotDEBUGIN(25);
656
  assert(m_data);
657
  newtonEulerSINTERN(time);
658
659
660
  if(isFrame){
    //TODO: Confirm if we need this. Already being called when calculating jacobian
    //se3::framesForwardKinematics(m_model,*m_data);
661
    res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
662
663
664
665
  }
  else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();

  sotDEBUGOUT(25);
666
667
668
  return res;
}

669
670
dg::Vector& Dynamic::
computeGenericVelocity( int jointId, dg::Vector& res,int time )
671
672
{
  sotDEBUGIN(25);
673
  assert(m_data);
674
675
  newtonEulerSINTERN(time);
  res.resize(6);
676
677
  se3::Motion aRV = m_data->v[jointId];
  res<<aRV.linear(),aRV.angular();
678
679
680
681
  sotDEBUGOUT(25);
  return res;
}

682
683
dg::Vector& Dynamic::
computeGenericAcceleration( int jointId ,dg::Vector& res,int time )
684
685
{
  sotDEBUGIN(25);
686
  assert(m_data);
687
688
  newtonEulerSINTERN(time);
  res.resize(6);
689
690
  se3::Motion aRA = m_data->a[jointId];
  res<<aRA.linear(),aRA.angular();
691
692
693
694
  sotDEBUGOUT(25);
  return res;
}

695
696
int& Dynamic::
computeNewtonEuler( int& dummy,int time )
697
{
698
  sotDEBUGIN(15);
699
700
701
  assert(m_model);
  assert(m_data);
  
702
703
704
  const Eigen::VectorXd q=getPinocchioPos(time);
  const Eigen::VectorXd v=getPinocchioVel(time);
  const Eigen::VectorXd a=getPinocchioAcc(time);
705
  se3::rnea(*m_model,*m_data,q,v,a);
706
707
708
709
  
  sotDEBUG(1)<< "pos = " <<q <<std::endl;
  sotDEBUG(1)<< "vel = " <<v <<std::endl;
  sotDEBUG(1)<< "acc = " <<a <<std::endl;
710

711
712
  sotDEBUGOUT(15);
  return dummy;
713
714
}

715
716
dg::Matrix& Dynamic::
computeJcom( dg::Matrix& Jcom,int time )
717
{
718
  
719
720
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
721
  const Eigen::VectorXd q=getPinocchioPos(time);
722

723
  Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
724
				   q, false);
725
726
727
728
  sotDEBUGOUT(25);
  return Jcom;
}

729
730
dg::Vector& Dynamic::
computeCom( dg::Vector& com,int time )
731
{
732

733
734
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
735
  const Eigen::VectorXd q=getPinocchioPos(time);
736
  com = se3::centerOfMass(*m_model,*m_data,q,false,true);
737
738
739
740
  sotDEBUGOUT(25);
  return com;
}

741
742
dg::Matrix& Dynamic::
computeInertia( dg::Matrix& res,int time )
743
{
744
745
    sotDEBUGIN(25);
    newtonEulerSINTERN(time);
746
    //TODO: USE CCRBA
747
748
    dg::Matrix upperInertiaMatrix = se3::crba(*m_model,
					      *m_data,this->getPinocchioPos(time)); 
749
750
751
752
    res = upperInertiaMatrix;
    res.triangularView<Eigen::StrictlyLower>() = upperInertiaMatrix.transpose().triangularView<Eigen::StrictlyLower>();
    sotDEBUGOUT(25);
    return res;
753
754
}

755
756
dg::Matrix& Dynamic::
computeInertiaReal( dg::Matrix& res,int time )
757
758
759
{
  sotDEBUGIN(25);

760
761
762
  const dg::Matrix & A = inertiaSOUT(time);
  const dg::Vector & gearRatio = gearRatioSOUT(time);
  const dg::Vector & inertiaRotor = inertiaRotorSOUT(time);
763
764

  res = A;
765
  for( int i=0;i<gearRatio.size();++i )
766
767
768
769
770
771
772
    res(i,i) += (gearRatio(i)*gearRatio(i)*inertiaRotor(i));

  sotDEBUGOUT(25);
  return res;
}

double& Dynamic::
773
computeFootHeight (double &res , int time)
774
{
775
776
  //Ankle position in local foot frame
  //TODO: Confirm that it is in the foot frame
777
778
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
779
  if(!m_model->existJointName("r_sole_joint")) {
780
781
782
    SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
			       "Robot has no joint corresponding to rigthFoot");
  }
783
  int jointId = m_model->getJointId("r_sole_joint");
784
  Eigen::Vector3d anklePosInLocalRefFrame= m_data->liMi[jointId].translation();
785
  // TODO: positive or negative? Current output:negative
786
787
788
789
790
791
  res = anklePosInLocalRefFrame(2);
  sotDEBUGOUT(25);
  return res;
}

dg::Vector& Dynamic::
792
computeTorqueDrift( dg::Vector& tauDrift,const int  &time )
793
794
{
  sotDEBUGIN(25);
795
  newtonEulerSINTERN(time);
796
  tauDrift = m_data->tau;
797
  sotDEBUGOUT(25);
798
  return tauDrift;
799
800
}

801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
dg::Vector& Dynamic::
computeMomenta(dg::Vector & Momenta, int time)
{
  sotDEBUGIN(25);
  inertiaSOUT(time);
  if (Momenta.size()!=6)
    Momenta.resize(6);

  Momenta = m_data->hg.toVector_impl();

  sotDEBUGOUT(25) << "Momenta :" << Momenta ;
  return Momenta; 
}

dg::Vector& Dynamic::
computeAngularMomentum(dg::Vector & Momenta, int time)
{
  sotDEBUGIN(25);
  inertiaSOUT(time);

  if (Momenta.size()!=3)
    Momenta.resize(3);
  return Momenta;
  Momenta = m_data->hg.angular_impl();

  sotDEBUGOUT(25) << "AngularMomenta :" << Momenta ;
  return Momenta;
}
829

830
/* ------------------------ SIGNAL CASTING--------------------------------------- */
831

832
dg::SignalTimeDependent<dg::Matrix,int>& Dynamic::
833
834
835
836
jacobiansSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
837
838
    dg::SignalTimeDependent<dg::Matrix,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Matrix,int>& >( sigabs );
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
    return res;
  } catch( std::bad_cast e ) {
    SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
				  "Impossible cast.",
				  " (while getting signal <%s> of type matrix.",
				  name.c_str());
  }
}
dg::SignalTimeDependent<MatrixHomogeneous,int>& Dynamic::
positionsSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
    dg::SignalTimeDependent<MatrixHomogeneous,int>& res
      = dynamic_cast< dg::SignalTimeDependent<MatrixHomogeneous,int>& >( sigabs );
    return res;
  } catch( std::bad_cast e ) {
    SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
				  "Impossible cast.",
				  " (while getting signal <%s> of type matrixHomo.",
				  name.c_str());
  }
}

863
dg::SignalTimeDependent<dg::Vector,int>& Dynamic::
864
865
866
867
velocitiesSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
868
869
    dg::SignalTimeDependent<dg::Vector,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Vector,int>& >( sigabs );
870
871
872
873
874
875
876
877
878
    return res;
 } catch( std::bad_cast e ) {
    SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
				  "Impossible cast.",
				  " (while getting signal <%s> of type Vector.",
				  name.c_str());
  }
}

879
dg::SignalTimeDependent<dg::Vector,int>& Dynamic::
880
881
882
883
accelerationsSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
884
885
    dg::SignalTimeDependent<dg::Vector,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Vector,int>& >( sigabs );
886
887
888
889
890
891
892
893
894
    return res;
  } catch( std::bad_cast e ) {
    SOT_THROW ExceptionSignal( ExceptionSignal::BAD_CAST,
				  "Impossible cast.",
				  " (while getting signal <%s> of type Vector.",
				  name.c_str());
  }
}

895
/*-------------------------------------------------------------------------*/
896

897
/*-------------------------------------------------------------------------*/
898
899
900
901
902
903
904

/* --- PARAMS --------------------------------------------------------------- */
void Dynamic::
commandLine( const std::string& cmdLine,
	     std::istringstream& cmdArgs,
	     std::ostream& os )
{
905
  sotDEBUG(25) << "# In { Cmd " << cmdLine <<std::endl;
906
  std::string filename;
907
908
  if( cmdLine == "displayModel" ) {
    displayModel();
909
  }
910
911
912
913
  else if( cmdLine == "createJacobian" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createJacobianSignal(signame,jointName);
914
  }
915
916
917
  else if( cmdLine == "destroyJacobian" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyJacobianSignal(Jname);
918
  }
919
920
921
922
  else if( cmdLine == "createPosition" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createPositionSignal(signame,jointName);
923
  }
924
925
926
  else if( cmdLine == "destroyPosition" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyPositionSignal(Jname);
927
  }
928
929
930
931
  else if( cmdLine == "createVelocity" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createVelocitySignal(signame,jointName);
932
  }
933
934
935
  else if( cmdLine == "destroyVelocity" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyVelocitySignal(Jname);
936
  }
937
938
939
940
  else if( cmdLine == "createAcceleration" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createAccelerationSignal(signame,jointName);
941
  }
942
943
944
  else if( cmdLine == "destroyAcceleration" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyAccelerationSignal(Jname);
945
  }
946
947
948
949
  else if( cmdLine == "createEndeffJacobian" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createEndeffJacobianSignal(signame,jointName);
950
  }
951
952
953
954
955
956
  else if( cmdLine == "createOpPoint" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createEndeffJacobianSignal(std::string("J")+signame,jointName);
    createPositionSignal(signame,jointName);
    sotDEBUG(15)<<std::endl;
957
  }
958
959
960
961
  else if( cmdLine == "destroyOpPoint" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyJacobianSignal(std::string("J")+Jname);
    destroyPositionSignal(Jname);
962
  }
963
964
965
  else if( cmdLine == "ndof" ) {
    os << "Number of Degree of freedom:" <<m_data->J.cols() << std::endl;
    return;
966
  }
967
968
969
970
971
972
973
974
975
976
  else if( cmdLine == "help" ) {
    os << "Dynamics:"<<std::endl
       << "  - displayModel\t:display the current model configuration" <<std::endl
       << "  - createJacobian <name> <point>:create a signal named <name> " << std::endl
       << "  - destroyJacobian <name>\t:delete the jacobian signal <name>" << std::endl
       << "  - createEndeffJacobian <name> <point>:create a signal named <name> "
       << "forwarding the jacobian computed at <point>." <<std::endl
       << "  - {create|destroy}Position\t:handle position signals." <<std::endl
       << "  - {create|destroy}OpPoint\t:handle Operation Point (ie pos+jac) signals." <<std::endl
       << "  - {create|destroy}Acceleration\t:handle acceleration signals." <<std::endl
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
      /*TODO: Put these flags for computations (copied from humanoid_robot.py):
    def setProperties(self, model):
        model.setProperty('TimeStep', str(self.timeStep))
        model.setProperty('ComputeAcceleration', 'false')
        model.setProperty('ComputeAccelerationCoM', 'false')
        model.setProperty('ComputeBackwardDynamics', 'false')
        model.setProperty('ComputeCoM', 'false')
        model.setProperty('ComputeMomentum', 'false')
        model.setProperty('ComputeSkewCom', 'false')
        model.setProperty('ComputeVelocity', 'false')
        model.setProperty('ComputeZMP', 'false')

        model.setProperty('ComputeAccelerationCoM', 'true')
        model.setProperty('ComputeCoM', 'true')
        model.setProperty('ComputeVelocity', 'true')
        model.setProperty('ComputeZMP', 'true')

        if self.enableZmpComputation:
            model.setProperty('ComputeBackwardDynamics', 'true')
            model.setProperty('ComputeAcceleration', 'true')
            model.setProperty('ComputeMomentum', 'true')
      */
999
1000
      //       << "  - {get|set}Property <name> [<val>]: set/get the property." <<std::endl
      //       << "  - displayProperties: print the prop-val couples list." <<std::endl