sot-dynamic.cpp 31.2 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
289
  if( freeFlyerPositionSIN) {
    dg::Vector qFF=freeFlyerPositionSIN.access(time);
    q.resize(qJoints.size() + 7);
290
291
292
293
294
    urdf::Rotation rot;
    rot.setFromRPY(qFF(3),qFF(4),qFF(5));
    double x,y,z,w;
    rot.getQuaternion(x,y,z,w);
    q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints;
295
  }
296
  else if (se3::nv(m_model->joints[1]) == 6){
297
298
299
300
301
302
303
    dg::Vector qFF = qJoints.head<6>();
    urdf::Rotation rot;
    rot.setFromRPY(qFF(3),qFF(4),qFF(5));
    double x,y,z,w;
    rot.getQuaternion(x,y,z,w);
    q.resize(qJoints.size()+1);
    q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints.segment(6,qJoints.size()-6);
304
    assert(q.size() == m_model->nq);
305
306
  }
  else {
307
308
    q.resize(qJoints.size());
    q=qJoints;
309
  }
310

311
312
  sotDEBUGOUT(15) <<"Position out"<<q<<std::endl;
  return q;
313
}
314

315
316
317
318
319
320
321
322
323
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;
  }
324
325
  else 
    return vJoints;
326
327
}

328
329
330
331
332
333
334
335
336
337
338
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;
}
339
340

/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
341
342
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createJacobianSignal( const std::string& signame, const std::string& jointName )
343
{
344
  sotDEBUGIN(15);
345
  assert(m_model);
346
  dg::SignalTimeDependent< dg::Matrix,int > * sig;
347
348
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
349
350
351
352
353
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericJacobian,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
354
355
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName);
356
357
358
359
360
361
362
    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);
363
364
365

  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
366
  sotDEBUGOUT(15);
367
368
369
  return *sig;
}

370
371
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createEndeffJacobianSignal( const std::string& signame, const std::string& jointName )
372
373
{
  sotDEBUGIN(15);
374
  assert(m_model);
375
  dg::SignalTimeDependent< dg::Matrix,int > * sig;
376
377
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
378
379
380
381
382
    sig = new dg::SignalTimeDependent< dg::Matrix,int >
      ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrix)::"+signame );
  }
383
384
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName); 
385
386
387
388
389
390
391
    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);
392
393
394
395
396
397
398
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
399
createPositionSignal( const std::string& signame, const std::string& jointName)
400
401
{
  sotDEBUGIN(15);
402
  assert(m_model);
403
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig;
404
405
  if(m_model->existFrame(jointName)) {
    int frameId = m_model->getFrameId(jointName);
406
407
408
409
410
    sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
      ( boost::bind(&Dynamic::computeGenericPosition,this,true,frameId,_1,_2),
	newtonEulerSINTERN,
	"sotDynamic("+name+")::output(matrixHomo)::"+signame );
  }
411
412
  else if(m_model->existJointName(jointName)) {
    int jointId = m_model->getJointId(jointName); 
413
414
415
416
417
418
419
420
    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);
  
421
422
423
424
425
426
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
  sotDEBUGOUT(15);
  return *sig;
}

427
428
SignalTimeDependent< dg::Vector,int >& Dynamic::
createVelocitySignal( const std::string& signame,const std::string& jointName )
429
{
430
  sotDEBUGIN(15);
431
432
  assert(m_model);
  int jointId = m_model->getJointId(jointName);
433

434
435
436
437
438
439
440
  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 );
441

442
443
  sotDEBUGOUT(15);
  return *sig;
444
445
}

446
447
dg::SignalTimeDependent< dg::Vector,int >& Dynamic::
createAccelerationSignal( const std::string& signame, const std::string& jointName)
448
449
{
  sotDEBUGIN(15);
450
451
  assert(m_model);
  int jointId = m_model->getJointId(jointName);
452
453
454
  dg::SignalTimeDependent< dg::Vector,int > * sig
    = new dg::SignalTimeDependent< dg::Vector,int >
    ( boost::bind(&Dynamic::computeGenericAcceleration,this,jointId,_1,_2),
455
      newtonEulerSINTERN,
456
457
      "sotDynamic("+name+")::output(matrixHomo)::"+signame );

458
459
460
461
462
463
464
465
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );

  sotDEBUGOUT(15);
  return *sig;
}


466
467


468
void Dynamic::
469
destroyJacobianSignal( const std::string& signame )
470
{
471
  sotDEBUGIN(15);
472

473
  bool deletable = false;
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
  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 )
{
498
  sotDEBUGIN(15);
499
500
  bool deletable = false;
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig = & positionsSOUT( signame );
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
  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;
}

521
522
void Dynamic::
destroyVelocitySignal( const std::string& signame )
523
{
524
  sotDEBUGIN(15);
525
526
527
528
529
530
531
532
  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; }
    }
533

534
535
536
537
538
539
540
  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				     "Cannot destroy signal",
				     " (while trying to remove generic pos. signal <%s>).",
				     signame.c_str() );
    }
541

542
  signalDeregistration( signame );
543

544
545
  delete sig;
}
546
547
548
549

void Dynamic::
destroyAccelerationSignal( const std::string& signame )
{
550
  sotDEBUGIN(15);
551
  bool deletable = false;
552
  dg::SignalTimeDependent< dg::Vector,int > * sig = & accelerationsSOUT( signame );
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
  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;
}

574
/* --------------------- COMPUTE ------------------------------------------------- */
575

576
dg::Vector& Dynamic::computeZmp( dg::Vector& res,int time )
577
{
578
579
    //TODO: To be verified
    sotDEBUGIN(25);
580
    assert(m_data);
581
582
583
    if (res.size()!=3)
        res.resize(3);
    newtonEulerSINTERN(time);
584

585
586
587
588
589
590
591
592
593
594
    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;
595
596
}

597
//In world coordinates
598
dg::Matrix& Dynamic::
599
computeGenericJacobian(bool isFrame, int jointId, dg::Matrix& res,int time )
600
601
{
  sotDEBUGIN(25);
602
603
  assert(m_model);
  assert(m_data);
604
  newtonEulerSINTERN(time);
605
606
  res.resize(6,m_model->nv);
  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
607

608
  se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
609
610
  //Computes Jacobian in world coordinates. 
  if(isFrame){
611
612
    se3::framesForwardKinematics(*m_model,*m_data);
    se3::getFrameJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
613
  }
614
  else se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
615
  res = m_output;
616
617
618
619
  sotDEBUGOUT(25);
  return res;
}

620
dg::Matrix& Dynamic::
621
computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time )
622
623
{
  sotDEBUGIN(25);
624
625
  assert(m_model);
  assert(m_data);
626
  newtonEulerSINTERN(time);
627
  res.resize(6,m_model->nv);
628
  //In local coordinates.
629
630
  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
  se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
631
632

  if(isFrame){
633
634
    se3::framesForwardKinematics(*m_model,*m_data);
    se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
635
  }
636
  else se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
637

638
  res = m_output;
639
640
641
642
643
644
  sotDEBUGOUT(25);

  return res;
}

MatrixHomogeneous& Dynamic::
645
computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int time)
646
647
{
  sotDEBUGIN(25);
648
  assert(m_data);
649
  newtonEulerSINTERN(time);
650
651
652
  if(isFrame){
    //TODO: Confirm if we need this. Already being called when calculating jacobian
    //se3::framesForwardKinematics(m_model,*m_data);
653
    res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
654
655
656
657
  }
  else res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();

  sotDEBUGOUT(25);
658
659
660
  return res;
}

661
662
dg::Vector& Dynamic::
computeGenericVelocity( int jointId, dg::Vector& res,int time )
663
664
{
  sotDEBUGIN(25);
665
  assert(m_data);
666
667
  newtonEulerSINTERN(time);
  res.resize(6);
668
669
  se3::Motion aRV = m_data->v[jointId];
  res<<aRV.linear(),aRV.angular();
670
671
672
673
  sotDEBUGOUT(25);
  return res;
}

674
675
dg::Vector& Dynamic::
computeGenericAcceleration( int jointId ,dg::Vector& res,int time )
676
677
{
  sotDEBUGIN(25);
678
  assert(m_data);
679
680
  newtonEulerSINTERN(time);
  res.resize(6);
681
682
  se3::Motion aRA = m_data->a[jointId];
  res<<aRA.linear(),aRA.angular();
683
684
685
686
  sotDEBUGOUT(25);
  return res;
}

687
688
int& Dynamic::
computeNewtonEuler( int& dummy,int time )
689
{
690
  sotDEBUGIN(15);
691
692
693
  assert(m_model);
  assert(m_data);
  
694
695
696
  const Eigen::VectorXd q=getPinocchioPos(time);
  const Eigen::VectorXd v=getPinocchioVel(time);
  const Eigen::VectorXd a=getPinocchioAcc(time);
697
  se3::rnea(*m_model,*m_data,q,v,a);
698
699
700
701
  
  sotDEBUG(1)<< "pos = " <<q <<std::endl;
  sotDEBUG(1)<< "vel = " <<v <<std::endl;
  sotDEBUG(1)<< "acc = " <<a <<std::endl;
702

703
704
  sotDEBUGOUT(15);
  return dummy;
705
706
}

707
708
dg::Matrix& Dynamic::
computeJcom( dg::Matrix& Jcom,int time )
709
{
710
  
711
712
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
713
  const Eigen::VectorXd q=getPinocchioPos(time);
714

715
  Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
716
				   q, false);
717
718
719
720
  sotDEBUGOUT(25);
  return Jcom;
}

721
722
dg::Vector& Dynamic::
computeCom( dg::Vector& com,int time )
723
{
724

725
726
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
727
  const Eigen::VectorXd q=getPinocchioPos(time);
728
  com = se3::centerOfMass(*m_model,*m_data,q,false,true);
729
730
731
732
  sotDEBUGOUT(25);
  return com;
}

733
734
dg::Matrix& Dynamic::
computeInertia( dg::Matrix& res,int time )
735
{
736
737
    sotDEBUGIN(25);
    newtonEulerSINTERN(time);
738
    //TODO: USE CCRBA
739
740
    dg::Matrix upperInertiaMatrix = se3::crba(*m_model,
					      *m_data,this->getPinocchioPos(time)); 
741
742
743
744
    res = upperInertiaMatrix;
    res.triangularView<Eigen::StrictlyLower>() = upperInertiaMatrix.transpose().triangularView<Eigen::StrictlyLower>();
    sotDEBUGOUT(25);
    return res;
745
746
}

747
748
dg::Matrix& Dynamic::
computeInertiaReal( dg::Matrix& res,int time )
749
750
751
{
  sotDEBUGIN(25);

752
753
754
  const dg::Matrix & A = inertiaSOUT(time);
  const dg::Vector & gearRatio = gearRatioSOUT(time);
  const dg::Vector & inertiaRotor = inertiaRotorSOUT(time);
755
756

  res = A;
757
  for( int i=0;i<gearRatio.size();++i )
758
759
760
761
762
763
764
    res(i,i) += (gearRatio(i)*gearRatio(i)*inertiaRotor(i));

  sotDEBUGOUT(25);
  return res;
}

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

dg::Vector& Dynamic::
784
computeTorqueDrift( dg::Vector& tauDrift,const int  &time )
785
786
{
  sotDEBUGIN(25);
787
  newtonEulerSINTERN(time);
788
  tauDrift = m_data->tau;
789
  sotDEBUGOUT(25);
790
  return tauDrift;
791
792
}

793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
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;
}
821

822
/* ------------------------ SIGNAL CASTING--------------------------------------- */
823

824
dg::SignalTimeDependent<dg::Matrix,int>& Dynamic::
825
826
827
828
jacobiansSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
829
830
    dg::SignalTimeDependent<dg::Matrix,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Matrix,int>& >( sigabs );
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
    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());
  }
}

855
dg::SignalTimeDependent<dg::Vector,int>& Dynamic::
856
857
858
859
velocitiesSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
860
861
    dg::SignalTimeDependent<dg::Vector,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Vector,int>& >( sigabs );
862
863
864
865
866
867
868
869
870
    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());
  }
}

871
dg::SignalTimeDependent<dg::Vector,int>& Dynamic::
872
873
874
875
accelerationsSOUT( const std::string& name )
{
  SignalBase<int> & sigabs = Entity::getSignal(name);
  try {
876
877
    dg::SignalTimeDependent<dg::Vector,int>& res
      = dynamic_cast< dg::SignalTimeDependent<dg::Vector,int>& >( sigabs );
878
879
880
881
882
883
884
885
886
    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());
  }
}

887
/*-------------------------------------------------------------------------*/
888

889
/*-------------------------------------------------------------------------*/
890
891
892
893
894
895
896

/* --- PARAMS --------------------------------------------------------------- */
void Dynamic::
commandLine( const std::string& cmdLine,
	     std::istringstream& cmdArgs,
	     std::ostream& os )
{
897
  sotDEBUG(25) << "# In { Cmd " << cmdLine <<std::endl;
898
  std::string filename;
899
900
  if( cmdLine == "displayModel" ) {
    displayModel();
901
  }
902
903
904
905
  else if( cmdLine == "createJacobian" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createJacobianSignal(signame,jointName);
906
  }
907
908
909
  else if( cmdLine == "destroyJacobian" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyJacobianSignal(Jname);
910
  }
911
912
913
914
  else if( cmdLine == "createPosition" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createPositionSignal(signame,jointName);
915
  }
916
917
918
  else if( cmdLine == "destroyPosition" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyPositionSignal(Jname);
919
  }
920
921
922
923
  else if( cmdLine == "createVelocity" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createVelocitySignal(signame,jointName);
924
  }
925
926
927
  else if( cmdLine == "destroyVelocity" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyVelocitySignal(Jname);
928
  }
929
930
931
932
  else if( cmdLine == "createAcceleration" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createAccelerationSignal(signame,jointName);
933
  }
934
935
936
  else if( cmdLine == "destroyAcceleration" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyAccelerationSignal(Jname);
937
  }
938
939
940
941
  else if( cmdLine == "createEndeffJacobian" ) {
    std::string signame; cmdArgs >> signame;
    std::string jointName; cmdArgs >> jointName;
    createEndeffJacobianSignal(signame,jointName);
942
  }
943
944
945
946
947
948
  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;
949
  }
950
951
952
953
  else if( cmdLine == "destroyOpPoint" ) {
    std::string Jname; cmdArgs >> Jname;
    destroyJacobianSignal(std::string("J")+Jname);
    destroyPositionSignal(Jname);
954
  }
955
956
957
  else if( cmdLine == "ndof" ) {
    os << "Number of Degree of freedom:" <<m_data->J.cols() << std::endl;
    return;
958
  }
959
960
961
962
963
964
965
966
967
968
  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
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
      /*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')
      */
991
992
993
994
995
      //       << "  - {get|set}Property <name> [<val>]: set/get the property." <<std::endl
      //       << "  - displayProperties: print the prop-val couples list." <<std::endl
       << "  - ndof\t\t\t: display the number of DOF of the robot."<< std::endl;
    
    Entity::commandLine(cmdLine,cmdArgs,os);
996
  }
997
998
999
1000
  else {
    Entity::commandLine( cmdLine,cmdArgs,os); }
  
  sotDEBUGOUT(15);