Avoid some dynamic allocation.

Author: jmirabel

include/sot/core/memory-task-sot.hh

@@ -38,6 +38,7 @@ namespace dynamicgraph {
     {
     public://   protected:
       /* Internal memory to reduce the dynamic allocation at task resolution. */
+      dg::Vector err;
       dg::Matrix Jt;  //( nJ,mJ );
       dg::Matrix Jp;
       dg::Matrix PJp;

include/sot/core/sot.hh

@@ -136,8 +136,8 @@ namespace dynamicgraph {
 						      dg::Matrix& Jact );
       static dg::Matrix & computeJacobianConstrained( const TaskAbstract& task,
 						      const dg::Matrix& K );
-      static dg::Vector
-	taskVectorToMlVector(const VectorMultiBound& taskVector);
+      static void
+	taskVectorToMlVector(const VectorMultiBound& taskVector, Vector& err);
 
     public:
 

src/sot/sot-qr.cpp

@@ -674,7 +674,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
   {
     TaskAbstract & task = **(stack.begin());
     const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
-    const dynamicgraph::Vector err = Sot::taskVectorToMlVector(task.taskSOUT(iterTime));
+    const dynamicgraph::Vector err;
+    Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
     const unsigned int mJ = Jac.rows();
     /***/sotCOUNTER(0,1); // Direct Dynamic
 
@@ -765,7 +766,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
 
       TaskAbstract & task = **iter;
       const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
-      const dynamicgraph::Vector err = Sot::taskVectorToMlVector(task.taskSOUT(iterTime));
+      const dynamicgraph::Vector err;
+      Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
       const unsigned int mJ = Jac.rows();
       /***/sotCOUNTER(0,1); // Direct Dynamic
 
@@ -875,7 +877,8 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       TaskAbstract & task = **iter;
       MemoryTaskSOT * mem = dynamic_cast<MemoryTaskSOT *>( task.memoryInternal );
       const dynamicgraph::Matrix &Jac = mem->JK;
-      const dynamicgraph::Vector &err = Sot::taskVectorToMlVector(task.taskSOUT.accessCopy());
+      const dynamicgraph::Vector err;
+      Sot::taskVectorToMlVector(task.taskSOUT.accessCopy(), err);
 
       dynamicgraph::Vector diffErr(err.size());
       diffErr = Jac*control;

src/sot/sot.cpp

@@ -446,17 +446,16 @@ static void computeJacobianActivated( Task* taskSpec,
 #   define sotPRINTCOUNTER(nbc1)
 #endif // #ifdef  WITH_CHRONO
 
-dynamicgraph::Vector Sot::
-taskVectorToMlVector( const VectorMultiBound& taskVector )
+void Sot::
+taskVectorToMlVector( const VectorMultiBound& taskVector, Vector& res )
 {
-  dynamicgraph::Vector res(taskVector.size()); res.setZero();
+  res.resize(taskVector.size());
   unsigned int i=0;
 
   for( VectorMultiBound::const_iterator iter=taskVector.begin();
        iter!=taskVector.end();++iter,++i ) {
     res(i)=iter->getSingleBound();
   }
-  return res;
 }
 
 dynamicgraph::Vector& Sot::
@@ -495,7 +494,6 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       TaskAbstract & task = **iter;
       sotDEBUG(15) << "Task: e_" << task.getName() << std::endl;
       const dynamicgraph::Matrix &Jac = task.jacobianSOUT(iterTime);
-      const dynamicgraph::Vector err = taskVectorToMlVector(task.taskSOUT(iterTime));
       sotCOUNTER(0,1); // Direct Dynamic
 
       unsigned int rankJ;
@@ -516,6 +514,9 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       dynamicgraph::Matrix &Jt = mem->Jt;
       MemoryTaskSOT::SVD_t& svd = mem->svd;
 
+      taskVectorToMlVector(task.taskSOUT(iterTime), mem->err);
+      const dynamicgraph::Vector &err = mem->err;
+
       Jp.resize( mJ,nJ );
       V.resize( mJ,mJ );
       Jt.resize( nJ,mJ );
@@ -650,8 +651,6 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
 
   if( 0!=taskGradient )
     {
-      const dynamicgraph::Vector err
-        = taskVectorToMlVector(taskGradient->taskSOUT.access(iterTime));
       const dynamicgraph::Matrix & Jac = taskGradient->jacobianSOUT.access(iterTime);
 
       const unsigned int nJ = Jac.rows();
@@ -666,6 +665,9 @@ computeControlLaw( dynamicgraph::Vector& control,const int& iterTime )
           taskGradient->memoryInternal = mem;
         }
 
+      taskVectorToMlVector(taskGradient->taskSOUT.access(iterTime), mem->err);
+      const dynamicgraph::Vector& err = mem->err;
+
       sotDEBUG(45) << "K = " << K <<endl;
       sotDEBUG(45) << "Jff = " << Jac <<endl;
 

src/sot/weighted-sot.cpp

@@ -285,7 +285,8 @@ computeWeightedControlLaw( dynamicgraph::Vector& control,const int& iterTime )
       sotDEBUGF(5,"Rank %d.",iterTask);
       TaskAbstract & task = **iter;
       const dynamicgraph::Matrix &JacRO = task.jacobianSOUT(iterTime);
-      const dynamicgraph::Vector err = Sot::taskVectorToMlVector(task.taskSOUT(iterTime));
+      const dynamicgraph::Vector err;
+      Sot::taskVectorToMlVector(task.taskSOUT(iterTime), err);
       const unsigned int nJ = JacRO.rows();
       sotCOUNTER(0b,1); // Direct Dynamic