sot-dynamic.cpp 43.9 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*
 * 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
22

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

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

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

//#include <abstract-robot-dynamics/humanoid-dynamic-robot.hh>
//#include <abstract-robot-dynamics/robot-dynamics-object-constructor.hh>
36
37
38
39
40
41
42
43
44

#include <dynamic-graph/all-commands.h>

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


using namespace dynamicgraph::sot;
using namespace dynamicgraph;

45
const std::string dg::sot::Dynamic::CLASS_NAME = "DynamicLib";
46
47

Dynamic::
48
Dynamic( const std::string & name)
49
  :Entity(name)
50
51
52
  ,m_model(NULL)
  ,m_data(NULL)
  ,m_urdfPath()
53
54
55
56
57
58
59
60
61
62

  ,init(false)

  ,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")

63
64
   //  ,firstSINTERN( boost::bind(&Dynamic::initNewtonEuler,this,_1,_2),
   //		 sotNOSIGNAL,"sotDynamic("+name+")::intern(dummy)::init" )
65
  ,newtonEulerSINTERN( boost::bind(&Dynamic::computeNewtonEuler,this,_1,_2),
66
		       sotNOSIGNAL<<jointPositionSIN<<freeFlyerPositionSIN
67
68
		       <<jointVelocitySIN<<freeFlyerVelocitySIN
		       <<jointAccelerationSIN<<freeFlyerAccelerationSIN,
69
		       "sotDynamic("+name+")::intern(dummy)::newtoneuler" )
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86

  ,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" )

87
  ,upperJlSOUT( boost::bind(&Dynamic::getUpperPositionLimits,this,_1,_2),
88
89
90
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::upperJl" )

91
  ,lowerJlSOUT( boost::bind(&Dynamic::getLowerPositionLimits,this,_1,_2),
92
93
94
95
96
97
98
		sotNOSIGNAL,
		"sotDynamic("+name+")::output(vector)::lowerJl" )

  ,upperVlSOUT( boost::bind(&Dynamic::getUpperVelocityLimits,this,_1,_2),
    sotNOSIGNAL,
    "sotDynamic("+name+")::output(vector)::upperVl" )

99
  ,upperTlSOUT( boost::bind(&Dynamic::getMaxEffortLimits,this,_1,_2),
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
    sotNOSIGNAL,
    "sotDynamic("+name+")::output(vector)::upperTl" )

  ,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),
		newtonEulerSINTERN,
		"sotDynamic("+name+")::output(vector)::momenta" )
  ,AngularMomentumSOUT( boost::bind(&Dynamic::computeAngularMomentum,this,_1,_2),
			newtonEulerSINTERN,
			"sotDynamic("+name+")::output(vector)::angularmomentum" )
  ,dynamicDriftSOUT( boost::bind(&Dynamic::computeTorqueDrift,this,_1,_2),
		     newtonEulerSINTERN,
		     "sotDynamic("+name+")::output(vector)::dynamicDrift" )
{

119
  sotDEBUGIN(5);
120

121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
  jointTypes.push_back("JointModelRX");
  jointTypes.push_back("JointModelRY");
  jointTypes.push_back("JointModelRZ");
  jointTypes.push_back("JointModelRevoluteUnaligned");
  jointTypes.push_back("JointModelSpherical");
  jointTypes.push_back("JointModelSphericalZYX");
  jointTypes.push_back("JointModelPX");
  jointTypes.push_back("JointModelPY");
  jointTypes.push_back("JointModelPZ");
  jointTypes.push_back("JointModelFreeFlyer");
  jointTypes.push_back("JointModelPlanar");
  jointTypes.push_back("JointModelTranslation");

  //TODO-------------------------------------------
 
  //if( build ) buildModel();
  //firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
138
  //DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
139
  //endTODO--------------------------------------------
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158

  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);
159
  signalRegistration(MomentaSOUT);
160
161
162
163
164
165
166
167
168
169
170
171
172
  signalRegistration(AngularMomentumSOUT);
  signalRegistration(dynamicDriftSOUT);

  //
  // Commands
  //
  std::string docstring;
  // setFiles
  docstring =
    "\n"
    "    Define files to parse in order to build the robot.\n"
    "\n"
    "      Input:\n"
173
    "        - a string: urdf file \n"
174
    "\n";
175
176
177
  addCommand("setFile",
	     new command::SetFile(*this, docstring));

178
179
  docstring =
    "\n"
180
    "    Display the current robot configuration.\n"
181
    "\n"
182
183
    "      Input:\n"
    "        - none \n"
184
    "\n";
185
186
187
188
189
190
  addCommand("displayModel",
	     new command::DisplayModel(*this, docstring));
  {
    using namespace ::dg::command;
    // CreateOpPoint
    //TODO add operational joints
191
    docstring = "    \n"
192
      "    Create an operational point attached to a robot joint local frame.\n"
193
      "    \n"
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
      "      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));
  }
  
  docstring = "    \n"
    "    Create an empty robot\n"
    "    \n";
  addCommand("createRobot", new command::CreateRobot(*this, docstring));
  
  docstring = "    \n"
    "    Create a joint\n"
    "    \n"
    "      Input:\n"
    "        - a string: name of the joint,\n"
    "        - a string: type of the joint in ['JointModelRX', 'JointModelRY', 'JointModelRZ', 'JointModelRevoluteUnaligned', 'JointModelSpherical', 'JointModelSphericalZYX', 'JointModelPX', 'JointModelPY', 'JointModelPZ', 'JointModelFreeFlyer', 'JointModelPlanar', 'JointModelTranslation'],\n"
    "        - a matrix: affine position of the joint.\n"
    "    \n";
  addCommand("createJoint", new command::CreateJoint(*this, docstring));
  
  docstring = "    \n"
    "    Add a child joint\n"
    "    \n"
    "      Input:\n"
    "        - a string: name of the parent body,\n"
    "        - a string: name of the joint. Joint must be newly created with CreateJoint,\n "
    "        - a string: name of the child body,\n"
    "        - a double: mass of the child body. Default = 0,\n"
    "        - a vector: com position of the child body. Default = Zero Vector,\n"
    "        - a matrix: 3x3 inertia matrix of the body. Default = Zero Matrix,\n"
    "    \n";
  addCommand("addBody", new command::AddBody(*this, docstring));
  
  docstring = "    \n"
    "    Set the mass of the body \n"
    "    \n"
    "      Input:\n"
    "        - a string:  name of the body whose properties are being set"
    "        - a double:  mass of the body."
    "    \n";
  addCommand("setMass", new command::SetMass(*this, docstring));

  docstring = "    \n"
    "    Set the position of the center of mass of a body\n"
    "    \n"
    "      Input:\n"
    "        - a string:  name of the body whose properties are being set"
    "        - a vector:  local com position of the body."
    "    \n";
  addCommand("setLocalCenterOfMass", new command::SetLocalCenterOfMass(*this, docstring));

  docstring = "    \n"
    "    Set inertia matrix of a body attached to a joint\n"
    "    \n"
    "      Input:\n"
    "        - a string: name of the joint,\n"
    "        - a matrix: inertia matrix.\n"
    "    \n";
  addCommand("setInertiaMatrix",
	     new command::SetInertiaMatrix(*this, docstring));  

  docstring = "    \n"
    "    Set Inertia properties of a body\n"
    "    \n"
    "      Input:\n"
    "        - a string:  name of the body whose properties are being set"
    "        - a double:  mass of the body. default 0."
    "        - a vector:  com position of the body. default zero vector,"
    "        - a matrix:  inertia matrix of the body. default zero matrix."
    "    \n";
  addCommand("setInertiaProperties", new command::SetInertiaProperties(*this, docstring));
287
288


289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
  docstring = "    \n"
    "    Set the lower bounds of the joint position\n"
    "    \n"
    "      Input:\n"
    "        - a string: the name of the joint,\n"
    "        - a vector: lower limit bounds for all dofs of joint,\n"
    "    \n";
  addCommand("setLowerPositionLimit", new command::SetLowerPositionLimit(*this, docstring));
  
  docstring = "    \n"
    "    Set the upper bounds of the joint position\n"
    "    \n"
    "      Input:\n"
    "        - a string: the name of the joint,\n"
    "        - a vector: upper limit bounds for all dofs of joint,\n"
    "    \n";
  addCommand("setUpperPositionLimit", new command::SetUpperPositionLimit(*this, docstring));
  
  docstring = "    \n"
    "    Set the upper bounds of the joint velocity\n"
    "    \n"
    "      Input:\n"
    "        - a string: the name of the joint,\n"
    "        - a vector: upper limit bounds for joint velocities,\n"
    "    \n";
  addCommand("setMaxVelocityLimit", new command::SetMaxVelocityLimit(*this, docstring));
  
  docstring = "    \n"
    "    Set the upper bounds of the joint effort\n"
    "    \n"
    "      Input:\n"
    "        - a string: the name of the joint,\n"
    "        - a vector: upper limit bounds for joint effort,\n"
    "    \n";
  addCommand("setMaxEffortLimit", new command::SetMaxEffortLimit(*this, docstring));
  
  sotDEBUGOUT(5);
}
327
328


329
330
331
332
333
334
335
336
337
338
339
Dynamic::~Dynamic( void ) {
  if (m_data) delete m_data;
  for( std::list< SignalBase<int>* >::iterator iter = genericSignalRefs.begin();
       iter != genericSignalRefs.end();
       ++iter) {
    SignalBase<int>* sigPtr = *iter;
    delete sigPtr;
  }
  if (m_model) delete m_model;
  return;
}
340

341
342
343
344
345
346
347
348
349
350
/* ---------------- CONFIG -------------------------------------------- */
//Import from urdf file
void Dynamic::setUrdfFile(const std::string& filename) {
  m_model = new se3::Model();
  *m_model = se3::urdf::buildModel(filename);
  this->m_urdfPath = filename;
  if (m_data) delete m_data;
  m_data = new se3::Data(*m_model);
  init=true;
}
351

352
353
354
355
356
357
358
359
360
361
//Create an empty robot
void Dynamic::createRobot()
{
  if (m_model) {
    m_model->~Model();
    delete m_data;
  }
  m_model= new se3::Model();
  m_data = new se3::Data(*m_model);
}
362
363


364
365
366
367
368
369
370
371
372
373
374
void Dynamic::createJoint(const std::string& inJointName,
			  const std::string& inJointType,
			  const dg::Matrix& inPosition) {
  if (jointMap_.count(inJointName) >= 1)
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "a joint with name " + inJointName +
			       " has already been created.");
  if (m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "a joint with name " + inJointName +
			       " already exists in the model.");
375

376
377
378
379
380
381
382
383
384
385
386
  if(std::find(jointTypes.begin(),jointTypes.end(),inJointType) != jointTypes.end()) {
    JointDetails jointDetails(inJointType,inPosition);
    jointMap_[inJointName] = jointDetails;
  }
  else {
    std::vector<std::string>::iterator it;    std::stringstream ss;
    for (it = jointTypes.begin(); it != jointTypes.end(); ++it)  ss << *it;
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       inJointType + " is not a valid type. Valid types are:" +ss.str());
  }
}
387

388
  //TODO: body name of joint and body are different in pinocchio
389
390


391
392
393
394
395
396
void Dynamic::addBody(const std::string& inParentName,
		      const std::string& inJointName,
		      const std::string& inChildName) {
  addBody(inParentName,inJointName,inChildName
	  ,0,Eigen::Vector3d::Zero(),Eigen::Matrix3d::Zero());
}
397
398


399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
void Dynamic::addBody(const std::string& inParentName,
		      const std::string& inJointName,
		      const std::string& inChildName,
		      const double mass,
		      const dg::Vector& lever,
		      const dg::Matrix& inertia3) {
  if (jointMap_.count(inJointName) != 1)
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No joint with name " + inJointName +
			       " has been created.");
  if (m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "A joint with name " + inJointName +
			       " already exists in the model.");
  if (!m_model->existBodyName(inParentName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No parent body with name " + inParentName +
			       " has been created.");
  se3::Model::Index parent = m_model->getBodyId(inParentName);
  const JointDetails jointDetails_ = jointMap_[inJointName];
  const std::string type = jointDetails_.first;
  const se3::Inertia inertia(mass,lever,inertia3);
  if(type == "JointModelRX")
    m_model->addBody(parent,se3::JointModelRX (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type == "JointModelRY" )
    m_model->addBody(parent,se3::JointModelRY (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelRZ" ) {
    m_model->addBody(parent,se3::JointModelRZ (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  }
  else if(type =="JointModelRevoluteUnaligned" )
    m_model->addBody(parent,se3::JointModelRevoluteUnaligned (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelSpherical" )
    m_model->addBody(parent,se3::JointModelSpherical (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelSphericalZYX" )
    m_model->addBody(parent,se3::JointModelSphericalZYX (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelPX" )
    m_model->addBody(parent,se3::JointModelPX (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelPY" )
    m_model->addBody(parent,se3::JointModelPY (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelPZ" )
    m_model->addBody(parent,se3::JointModelPZ (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelFreeFlyer" )
    m_model->addBody(parent,se3::JointModelFreeFlyer (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelPlanar" )
    m_model->addBody(parent,se3::JointModelPlanar (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else if(type =="JointModelTranslation" )
    m_model->addBody(parent,se3::JointModelTranslation (),
			  jointDetails_.second,inertia,inJointName,inChildName);
  else
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "Joint with type " + type +
			       "should not have been created");
}
463

464
  /*--------------------------------SETTERS-------------------------------------------*/
465

466
467
468
469
470
471
472
473
474
void Dynamic::setMass(const std::string& inBodyName,
		      const double mass) {
  if (!m_model->existBodyName(inBodyName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No body with name " + inBodyName +
			       " has been added.");
  se3::Model::Index index = m_model->getBodyId(inBodyName);
  m_model->inertias[index].mass() = mass;
}
475

476
477
478
479
480
481
482
483
484
void Dynamic::setLocalCenterOfMass(const std::string& inBodyName,
				   const dg::Vector& lever) {
  if (!m_model->existBodyName(inBodyName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No body with name " + inBodyName +
			       " has been added.");
  se3::Model::Index index = m_model->getBodyId(inBodyName);
  m_model->inertias[index].lever() = lever;
}
485

486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
void Dynamic::setInertiaMatrix(const std::string& inBodyName,
			       const dg::Matrix& inertia3) {
  if (!m_model->existBodyName(inBodyName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No body with name " + inBodyName +
			       " has been added.");
  se3::Model::Index index = m_model->getBodyId(inBodyName);
  se3::Symmetric3 symmetricMatrix(inertia3);
  m_model->inertias[index].inertia() = symmetricMatrix;
}

void Dynamic::setInertiaProperties(const std::string& inBodyName,
				   const double mass,
				   const dg::Vector& lever,
				   const dg::Matrix& inertia3) {
  if (!m_model->existBodyName(inBodyName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No body with name " + inBodyName +
			       " has been added.");
  se3::Inertia inertia_(mass,lever,inertia3);
  se3::Model::Index index = m_model->getBodyId(inBodyName);
  m_model->inertias[index] = inertia_;
}
509

510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
void Dynamic::setLowerPositionLimit(const std::string& inJointName,
				    const dg::Vector& lowPos) {
  if (!m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No joint with name " + inJointName +
			       " has been created.");
  se3::Model::Index joint_index = m_model->getJointId(inJointName);

  int prev_cumulative_nq = se3::idx_q(m_model->joints[joint_index]);  
  int current_nq = se3::nq(m_model->joints[joint_index]);

  assert (lowPos.size()==current_nq);
  m_data->lowerPositionLimit.segment(prev_cumulative_nq,current_nq) = lowPos;
  return;
  
525
526
527
}


528
529
530
531
532
533
534
void Dynamic::setUpperPositionLimit(const std::string& inJointName,
				    const dg::Vector& upPos) {
  if (!m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No joint with name " + inJointName +
			       " has been created.");
  se3::Model::Index joint_index = m_model->getJointId(inJointName);
535

536
537
  int prev_cumulative_nq = se3::idx_q(m_model->joints[joint_index]);  
  int current_nq = se3::nq(m_model->joints[joint_index]);
538

539
540
541
  assert (upPos.size()==current_nq);
  m_data->upperPositionLimit.segment(prev_cumulative_nq,current_nq) = upPos;
  return;
542
543
}

544
545
546
547
548
549
550
void Dynamic::setMaxVelocityLimit(const std::string& inJointName,
				  const dg::Vector& maxVel) {
  if (!m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No joint with name " + inJointName +
			       " has been created.");
  se3::Model::Index joint_index = m_model->getJointId(inJointName);
551

552
553
  int prev_cumulative_nv = se3::idx_v(m_model->joints[joint_index]);  
  int current_nv = se3::nv(m_model->joints[joint_index]);
554

555
556
557
558
  assert (maxVel.size()==current_nv);
  m_data->velocityLimit.segment(prev_cumulative_nv,current_nv) = maxVel;
  return;
}
559

560
561
562
563
564
565
566
567
568
569
570
571
572
573
574

void Dynamic::setMaxEffortLimit(const std::string& inJointName,
				const dg::Vector& maxEffort) {

  if (!m_model->existJointName(inJointName))
    SOT_THROW ExceptionDynamic(ExceptionDynamic::DYNAMIC_JRL,
			       "No joint with name " + inJointName +
			       " has been created.");
  se3::Model::Index joint_index = m_model->getJointId(inJointName);

  int prev_cumulative_nv = se3::idx_v(m_model->joints[joint_index]);  
  int current_nv = se3::nv(m_model->joints[joint_index]);

  assert (maxEffort.size()==current_nv);
  m_data->effortLimit.segment(prev_cumulative_nv,current_nv) = maxEffort;
575
576
577
  return;
}

578
579
580
581
582
/*--------------------------------GETTERS-------------------------------------------*/


dg::Vector& Dynamic::
getLowerPositionLimits(dg::Vector& res, const int&)
583
{
584
585
586
587
588
  sotDEBUGIN(15);
  res = m_data->lowerPositionLimit;
  sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
  sotDEBUGOUT(15);
  return res;
589
}
590
591
592

dg::Vector& Dynamic::
getUpperPositionLimits(dg::Vector& res, const int&)
593
{
594
595
596
597
598
  sotDEBUGIN(15);
  res = m_data->upperPositionLimit;
  sotDEBUG(15) << "upperLimit (" << res << ")=" <<std::endl;
  sotDEBUGOUT(15);
  return res;
599
}
600
601
602

dg::Vector& Dynamic::
getUpperVelocityLimits(dg::Vector& res, const int&)
603
{
604
605
606
607
608
  sotDEBUGIN(15);
  res = m_data->velocityLimit;
  sotDEBUG(15) << "upperVelocityLimit (" << res << ")=" <<std::endl;
  sotDEBUGOUT(15);
  return res;
609
}
610
611
612

dg::Vector& Dynamic::
getMaxEffortLimits(dg::Vector& res, const int&)
613
{
614
615
616
617
618
    sotDEBUGIN(15);
    //TODO: Confirm definition of effort
    res = m_data->effortLimit;
    sotDEBUGOUT(15);
    return res;
619
620
}

621
622
/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector Dynamic::getPinocchioPos(int time)
623
624
{

625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
  const Eigen::VectorXd qJoints=jointPositionSIN.access(time);
  if( freeFlyerPositionSIN ){
    const Eigen::VectorXd qFF=freeFlyerPositionSIN.access(time);
    Eigen::VectorXd q(qJoints.size() + 7);
    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;
    return q;
  }
  else {
    return qJoints;
  }
}
640

641
642
643
644
645
646
647
648
649
650
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;
  }
  else return vJoints;
651
652
}

653
654
655
656
657
658
659
660
661
662
663
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;
}
664
665

/* --- SIGNAL ACTIVATION ---------------------------------------------------- */
666
667
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createJacobianSignal( const std::string& signame, const std::string& jointName )
668
{
669
670
671
672
673
  int jointId = m_model->getJointId(jointName);

  dg::SignalTimeDependent< dg::Matrix,int > * sig
    = new dg::SignalTimeDependent< dg::Matrix,int >
    ( boost::bind(&Dynamic::computeGenericJacobian,this,jointId,_1,_2),
674
675
676
677
678
679
680
681
      newtonEulerSINTERN,
      "sotDynamic("+name+")::output(matrix)::"+signame );

  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );
  return *sig;
}

682
683
dg::SignalTimeDependent< dg::Matrix,int > & Dynamic::
createEndeffJacobianSignal( const std::string& signame, const std::string& jointName )
684
685
{
  sotDEBUGIN(15);
686
687
688
689
  int jointId = m_model->getJointId(jointName);
  dg::SignalTimeDependent< dg::Matrix,int > * sig
    = new dg::SignalTimeDependent< dg::Matrix,int >
    ( boost::bind(&Dynamic::computeGenericEndeffJacobian,this,jointId,_1,_2),
690
691
692
693
694
695
696
697
698
699
700
      newtonEulerSINTERN,
      "sotDynamic("+name+")::output(matrix)::"+signame );

  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );

  sotDEBUGOUT(15);
  return *sig;
}

dg::SignalTimeDependent< MatrixHomogeneous,int >& Dynamic::
701
createPositionSignal( const std::string& signame, const std::string& jointName)
702
703
{
  sotDEBUGIN(15);
704
  int jointId = m_model->getJointId(jointName);
705
706
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig
    = new dg::SignalTimeDependent< MatrixHomogeneous,int >
707
    ( boost::bind(&Dynamic::computeGenericPosition,this,jointId,_1,_2),
708
709
710
711
712
713
714
715
716
      newtonEulerSINTERN,
      "sotDynamic("+name+")::output(matrixHomo)::"+signame );
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );

  sotDEBUGOUT(15);
  return *sig;
}

717
718
SignalTimeDependent< dg::Vector,int >& Dynamic::
createVelocitySignal( const std::string& signame,const std::string& jointName )
719
{
720
721
  sotDEBUGIN(15);
  int jointId = m_model->getJointId(jointName);
722

723
724
725
726
727
728
729
  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 );
730

731
732
  sotDEBUGOUT(15);
  return *sig;
733
734
}

735
736
dg::SignalTimeDependent< dg::Vector,int >& Dynamic::
createAccelerationSignal( const std::string& signame, const std::string& jointName)
737
738
{
  sotDEBUGIN(15);
739
740
741
742
  int jointId = m_model->getJointId(jointName);
  dg::SignalTimeDependent< dg::Vector,int > * sig
    = new dg::SignalTimeDependent< dg::Vector,int >
    ( boost::bind(&Dynamic::computeGenericAcceleration,this,jointId,_1,_2),
743
      newtonEulerSINTERN,
744
745
      "sotDynamic("+name+")::output(matrixHomo)::"+signame );

746
747
748
749
750
751
752
753
  genericSignalRefs.push_back( sig );
  signalRegistration( *sig );

  sotDEBUGOUT(15);
  return *sig;
}


754
755


756
void Dynamic::
757
destroyJacobianSignal( const std::string& signame )
758
759
{
  bool deletable = false;
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
  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 )
{
  bool deletable = false;
  dg::SignalTimeDependent< MatrixHomogeneous,int > * sig = & positionsSOUT( signame );
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
  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;
}

806
807
void Dynamic::
destroyVelocitySignal( const std::string& signame )
808
{
809
810
811
812
813
814
815
816
  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; }
    }
817

818
819
820
821
822
823
824
  if(! deletable )
    {
      SOT_THROW ExceptionDynamic( ExceptionDynamic::CANT_DESTROY_SIGNAL,
				     "Cannot destroy signal",
				     " (while trying to remove generic pos. signal <%s>).",
				     signame.c_str() );
    }
825

826
  signalDeregistration( signame );
827

828
829
  delete sig;
}
830
831
832
833
834

void Dynamic::
destroyAccelerationSignal( const std::string& signame )
{
  bool deletable = false;
835
  dg::SignalTimeDependent< dg::Vector,int > * sig = & accelerationsSOUT( signame );
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
  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;
}

857
/* --------------------- COMPUTE ------------------------------------------------- */
858

859
dg::Vector& Dynamic::computeZmp( dg::Vector& res,int time )
860
{
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
    //TODO: To be verified
    sotDEBUGIN(25);
    if (res.size()!=3)
        res.resize(3);
    newtonEulerSINTERN(time);
    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;
876
877
}

878
879
880
//In world frame
dg::Matrix& Dynamic::
computeGenericJacobian( int jointId,dg::Matrix& res,int time )
881
882
883
{
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
884
885
886
887
  res.resize(6,m_model->nv);
  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
  //Computes Jacobian in world frame. 
  se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,res);
888
889
890
891
  sotDEBUGOUT(25);
  return res;
}

892
893
dg::Matrix& Dynamic::
computeGenericEndeffJacobian( int jointId,dg::Matrix& res,int time )
894
{
895
  //TODO: Check why named EndEff. Current output: in local frame
896
897
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
898
899
900
901
902
  res.resize(6,m_model->nv);
  //In local Frame.

  se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
  se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,res);
903
904
905
906
907
908
909

  sotDEBUGOUT(25);

  return res;
}

MatrixHomogeneous& Dynamic::
910
computeGenericPosition( int jointId, MatrixHomogeneous& res, int time)
911
912
913
{
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
914
  res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
915
916
917
  return res;
}

918
919
dg::Vector& Dynamic::
computeGenericVelocity( int jointId, dg::Vector& res,int time )
920
{
921

922
923
924
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
  res.resize(6);
925
926
  se3::Motion aRV = m_data->v[jointId];
  res<<aRV.linear(),aRV.angular();
927
928
929
930
  sotDEBUGOUT(25);
  return res;
}

931
932
dg::Vector& Dynamic::
computeGenericAcceleration( int jointId ,dg::Vector& res,int time )
933
934
935
936
{
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
  res.resize(6);
937
938
  se3::Motion aRA = m_data->a[jointId];
  res<<aRA.linear(),aRA.angular();
939
940
941
942
  sotDEBUGOUT(25);
  return res;
}

943
944
int& Dynamic::
computeNewtonEuler( int& dummy,int time )
945
{
946
  sotDEBUGIN(15);
947

948
949
950
951
952
953
954
955
  const Eigen::VectorXd q=getPinocchioPos(time);
  const Eigen::VectorXd v=getPinocchioVel(time);
  const Eigen::VectorXd a=getPinocchioAcc(time);
  se3::rnea(*m_model,*m_data,q,v,a);
  
  sotDEBUG(1)<< "pos = " <<q <<std::endl;
  sotDEBUG(1)<< "vel = " <<v <<std::endl;
  sotDEBUG(1)<< "acc = " <<a <<std::endl;
956

957
958
  sotDEBUGOUT(15);
  return dummy;
959
960
}

961
962
dg::Matrix& Dynamic::
computeJcom( dg::Matrix& Jcom,int time )
963
{
964
  
965
966
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
967
968
969
970
  const Eigen::VectorXd q=getPinocchioPos(time);
  //TODO: getjcom center-of-mass.hpp
  Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
				   q, false);
971
972
973
974
  sotDEBUGOUT(25);
  return Jcom;
}

975
976
dg::Vector& Dynamic::
computeCom( dg::Vector& com,int time )
977
{
978
  //enter
979
980
  sotDEBUGIN(25);
  newtonEulerSINTERN(time);
981
982
  const Eigen::VectorXd q=getPinocchioPos(time);
  com = se3::centerOfMass(*m_model,*m_data,q,false,true);
983
984
985
986
  sotDEBUGOUT(25);
  return com;
}

987
988
dg::Matrix& Dynamic::
computeInertia( dg::Matrix& res,int time )
989
{
990
991
992
993
994
995
996
997
    sotDEBUGIN(25);
    newtonEulerSINTERN(time);
    dg::Matrix upperInertiaMatrix = se3::crba(*m_model,
							*m_data,this->getPinocchioPos(time));
    res = upperInertiaMatrix;
    res.triangularView<Eigen::StrictlyLower>() = upperInertiaMatrix.transpose().triangularView<Eigen::StrictlyLower>();
    sotDEBUGOUT(25);
    return res;
998
999
}

1000
dg::Matrix& Dynamic::