@@ -334,13 +334,11 @@ defineNbDof( const unsigned int& nbDof )
334
334
dynamicgraph::Matrix & Sot::
335
335
computeJacobianConstrained ( const dynamicgraph::Matrix& Jac,
336
336
const dynamicgraph::Matrix& K,
337
- dynamicgraph::Matrix& JK,
338
- dynamicgraph::Matrix& Jff,
339
- dynamicgraph::Matrix& Jact )
337
+ dynamicgraph::Matrix& JK)
340
338
{
341
- const int nJ = Jac.rows ();
342
- const int mJ = K.cols ();
343
- const int nbConstraints = Jac.cols () - mJ ;
339
+ const Matrix::Index nJ = Jac.rows ();
340
+ const Matrix::Index mJ = K.cols ();
341
+ const Matrix::Index nbConstraints = Jac.cols () - mJ ;
344
342
345
343
if (nbConstraints == 0 ) {
346
344
JK = Jac;
@@ -361,10 +359,8 @@ computeJacobianConstrained( const TaskAbstract& task,
361
359
const dynamicgraph::Matrix &Jac = task.jacobianSOUT .accessCopy ();
362
360
MemoryTaskSOT * mem = dynamic_cast <MemoryTaskSOT *>( task.memoryInternal );
363
361
if ( NULL ==mem ) throw ; // TODO
364
- dynamicgraph::Matrix &Jff = mem->Jff ;
365
- dynamicgraph::Matrix &Jact = mem->Jact ;
366
362
dynamicgraph::Matrix &JK = mem->JK ;
367
- return computeJacobianConstrained (Jac,K,JK,Jff,Jact );
363
+ return computeJacobianConstrained (Jac,K,JK);
368
364
}
369
365
370
366
static void computeJacobianActivated ( Task* taskSpec,
@@ -423,15 +419,15 @@ static void computeJacobianActivated( Task* taskSpec,
423
419
# define sotSTART_CHRONO1 gettimeofday (&t0,NULL )
424
420
# define sotCHRONO1 \
425
421
gettimeofday (&t1,NULL );\
426
- dt = ( (t1.tv_sec-t0.tv_sec) * 1000 .* 1000 .\
427
- + (t1.tv_usec-t0.tv_usec+ 0 . ) );\
422
+ dt = ( (double )( t1.tv_sec-t0.tv_sec) * 1000 .* 1000 .\
423
+ + (double )( t1.tv_usec-t0.tv_usec) );\
428
424
sotDEBUG (1 ) << "dt: "<< dt / 1000. << std::endl
429
425
# define sotINITPARTCOUNTERS struct timeval tpart0
430
426
# define sotSTARTPARTCOUNTERS gettimeofday (&tpart0,NULL )
431
427
# define sotCOUNTER (nbc1,nbc2 ) \
432
428
gettimeofday (&tpart##nbc2,NULL ); \
433
- dt##nbc2 += ( (tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000 .* 1000 . \
434
- + ( tpart##nbc2.tv_usec-tpart##nbc1.tv_usec+ 0 . ) )
429
+ dt##nbc2 += ( (double )( tpart##nbc2.tv_sec-tpart##nbc1.tv_sec) * 1000 .* 1000 . \
430
+ + ( double )( tpart##nbc2.tv_usec-tpart##nbc1.tv_usec) )
435
431
# define sotINITCOUNTER (nbc1 ) \
436
432
struct timeval tpart##nbc1; double dt##nbc1=0 ;
437
433
# define sotPRINTCOUNTER (nbc1 ) sotDEBUG(1 ) << " dt" << nbc1 << " = " << dt##nbc1 << std::endl
@@ -471,8 +467,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
471
467
sotSTARTPARTCOUNTERS;
472
468
473
469
const double &th = inversionThresholdSIN (iterTime);
474
- const dynamicgraph:: Matrix &K = constraintSOUT (iterTime);
475
- const int mJ = K.cols (); // number dofs - number constraints
470
+ const Matrix &K = constraintSOUT (iterTime);
471
+ const Matrix::Index mJ = K.cols (); // number dofs - number constraints
476
472
477
473
try {
478
474
control = q0SIN ( iterTime );
@@ -497,7 +493,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
497
493
sotCOUNTER (0 ,1 ); // Direct Dynamic
498
494
499
495
unsigned int rankJ;
500
- const int nJ = Jac.rows (); // number dofs
496
+ const Matrix::Index nJ = Jac.rows (); // number dofs
501
497
502
498
/* Init memory. */
503
499
MemoryTaskSOT * mem = dynamic_cast <MemoryTaskSOT *>( task.memoryInternal );
@@ -653,7 +649,7 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
653
649
{
654
650
const dynamicgraph::Matrix & Jac = taskGradient->jacobianSOUT .access (iterTime);
655
651
656
- const unsigned int nJ = Jac.rows ();
652
+ const Matrix::Index nJ = Jac.rows ();
657
653
658
654
MemoryTaskSOT * mem
659
655
= dynamic_cast <MemoryTaskSOT *>( taskGradient->memoryInternal );
@@ -739,7 +735,7 @@ computeConstraintProjector( dynamicgraph::Matrix& ProjK, const int& time )
739
735
const dynamicgraph::Matrix &J = *Jptr;
740
736
sotDEBUG (12 ) << " J = " << J;
741
737
742
- const unsigned int nJc = J.cols ();
738
+ const Matrix::Index nJc = J.cols ();
743
739
dynamicgraph::Matrix Jff ( J.rows (),ffJointIdLast-ffJointIdFirst );
744
740
dynamicgraph::Matrix Jc ( J.rows (),nJc-ffJointIdLast+ffJointIdFirst );
745
741
0 commit comments