Merge branch 'jmirabel-master'

Author: olivier-stasse

include/sot/core/matrix-svd.hh

@@ -61,9 +61,7 @@ void dampedInverse( dg::Matrix& _inputMatrix,
   JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
   singularValues_inv.resizeLike(m_singularValues);
   for ( long i=0; i<m_singularValues.size(); ++i) {
-    if ( m_singularValues(i) > threshold )
-      singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
-    else singularValues_inv(i)=0;
+    singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
   }
   dg::Matrix matrix_U(svd.matrixU());
   dg::Matrix matrix_V(svd.matrixV());
@@ -83,9 +81,7 @@ void dampedInverse( dg::Matrix& _inputMatrix,
   JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
   singularValues_inv.resizeLike(m_singularValues);
   for ( long i=0; i<m_singularValues.size(); ++i) {
-    if ( m_singularValues(i) > threshold )
-      singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
-    else singularValues_inv(i)=0;
+    singularValues_inv(i)=m_singularValues(i)/(m_singularValues(i)*m_singularValues(i)+threshold*threshold);
   }
   dg::Matrix Uref(svd.matrixU());
   dg::Matrix Vref(svd.matrixV());

src/matrix/operator.cpp

@@ -55,8 +55,8 @@ namespace dynamicgraph {
     template<>const std::string TypeNameHelper<typeid>::typeName = #typeid
 
     ADD_KNOWN_TYPE(double);
-    ADD_KNOWN_TYPE(dg::Vector);
-    ADD_KNOWN_TYPE(dg::Matrix);
+    ADD_KNOWN_TYPE(Vector);
+    ADD_KNOWN_TYPE(Matrix);
     ADD_KNOWN_TYPE(MatrixRotation);
     ADD_KNOWN_TYPE(MatrixTwist);
     ADD_KNOWN_TYPE(MatrixHomogeneous);

src/sot/sot.cpp

@@ -346,14 +346,9 @@ computeJacobianConstrained( const dynamicgraph::Matrix& Jac,
     JK = Jac;
     return JK;
   }
-  for( int i=0;i<nJ;++i )
-    {
-      for( int j=0;j<nbConstraints;++j ) Jff(i,j)=Jac(i,j);
-      for( int j=nbConstraints;j<Jac.cols();++j )
-	Jact(i,j-nbConstraints)=Jac(i,j);
-    }
-  JK = Jff*K;
-  JK+=Jact;
+  JK.resize(nJ, mJ);
+  JK.noalias() = Jac.leftCols (nbConstraints) * K;
+  JK.noalias() += Jac.rightCols (Jac.cols() - nbConstraints);
 
   return JK;
 }
@@ -800,26 +795,19 @@ computeConstraintProjector( dynamicgraph::Matrix& ProjK, const int& time )
   dynamicgraph::Matrix Jff( J.rows(),ffJointIdLast-ffJointIdFirst );
   dynamicgraph::Matrix Jc( J.rows(),nJc-ffJointIdLast+ffJointIdFirst );
 
-  for( int i=0;i<J.rows();++i )
-    {
-      if( ffJointIdFirst )
-	for( unsigned int j=0;j<ffJointIdFirst;++j )
-	  Jc(i,j)=J(i,j);
-      if( ffJointIdLast<nJc )
-	for( unsigned int j=ffJointIdLast;j<nJc;++j )
-	    Jc(i,j+ffJointIdFirst-ffJointIdLast)=J(i,j);
-      for( unsigned int j=ffJointIdFirst;j<ffJointIdLast;++j )
-	Jff( i,j-ffJointIdFirst )=J(i,j);
-    }
+  Jc.leftCols (ffJointIdFirst)    = J.leftCols  (ffJointIdFirst);
+  Jc.rightCols(nJc-ffJointIdLast) = J.rightCols (nJc-ffJointIdLast);
+  Jff = J.middleCols(ffJointIdFirst, ffJointIdLast);
+
   sotDEBUG(25) << "Jc = "<< Jc;
   sotDEBUG(25) << "Jff = "<< Jff;
 
   dynamicgraph::Matrix Jffinv( Jff.cols(),Jff.rows() );
-  Eigen::pseudoInverse(Jff, Jffinv);   Jffinv *= -1;
+  Eigen::pseudoInverse(Jff, Jffinv);
 
   dynamicgraph::Matrix& Jffc = ProjK;
   Jffc.resize( Jffinv.rows(),Jc.cols() );
-  Jffc = Jffinv*Jc;
+  Jffc = -Jffinv*Jc;
   sotDEBUG(15) << "Jffc = "<< Jffc;
 
   sotDEBUGOUT(15);

src/task/constraint.cpp

@@ -105,7 +105,24 @@ computeJacobian( dynamicgraph::Matrix& J,int time )
   try {
     int dimJ = J .rows();
     int nbc = J.cols();
-    if( 0==dimJ ){ dimJ = 1; J.resize(dimJ,nbc); }
+    if( 0==dimJ ) {
+      // Compute the correct size
+      for( JacobianList::iterator iter = jacobianList.begin();
+           iter!=jacobianList.end(); ++iter )
+        {
+          Signal< dynamicgraph::Matrix,int >& jacobian = ** iter;
+          const dynamicgraph::Matrix& partialJacobian = jacobian(time);
+          dimJ += partialJacobian.rows();
+          if (nbc == 0)
+            nbc = partialJacobian.cols();
+          else if (nbc != partialJacobian.cols()) {
+            SOT_THROW ExceptionTask(ExceptionTask::NON_ADEQUATE_FEATURES,
+                                    "Features from the list don't "
+                                    "have compatible-size jacobians.");
+          }
+        }
+      J.resize (dimJ, nbc);
+    }
 
     int cursorJ = 0;
 
@@ -119,23 +136,12 @@ computeJacobian( dynamicgraph::Matrix& J,int time )
 	const dynamicgraph::Matrix& partialJacobian = jacobian(time);
 	const int nbr = partialJacobian.rows();
 
-	if( 0==nbc ) { nbc = partialJacobian.cols(); J.conservativeResize(nbc,dimJ); }
-	else if( partialJacobian.cols() != nbc )
-	  {SOT_THROW ExceptionTask(ExceptionTask::NON_ADEQUATE_FEATURES,
-				   "Features from the list don't "
-				   "have compatible-size jacobians.");}
 	sotDEBUG(25) << "Jp =" <<endl<< partialJacobian<<endl;
 
-	while( cursorJ+nbr>=dimJ ) 
-	  { dimJ *= 2; J.resize(dimJ,nbc); }
-	for( int kc=0;kc<nbc;++kc ) 
-	  for( int k=0;k<nbr;++k )
-	    { J(cursorJ+k,kc) = partialJacobian(k,kc); }
+        J.middleRows (cursorJ, nbr) = partialJacobian;
 	cursorJ += nbr;
       }
 
-    /* If too much memory has been allocated, resize. */
-    J.conservativeResize(cursorJ,nbc);
   } catch SOT_RETHROW;
 
   sotDEBUG(15) << "# Out }" << endl;

src/tools/device.cpp

@@ -419,9 +419,9 @@ void Device::integrate( const double & dt )
 
   if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION)
   {
+    if(controlIN.size() == velocity_.size()) offset = 0;
     for( int i=0;i<controlIN.size();++i )
     {
-      if(controlIN.size() == velocity_.size()) offset = 0;
       vel_control_(i) = velocity_(i+offset) + controlIN(i)*dt*0.5;
       velocity_(i+offset) = velocity_(i+offset) + controlIN(i)*dt;
     }