From 657b2f21b69bea2419e5b11a80303f0b4249db4d Mon Sep 17 00:00:00 2001
From: Francesco Morsillo <fmorsill@laas.fr>
Date: Mon, 24 Jun 2013 15:00:30 +0200
Subject: [PATCH] Resolved timing problem on solver-kine. Now working on second
order
---
src/solver-kine.cpp | 60 ++++++++++++++++-----------------------------
src/solver-kine.h | 1 +
2 files changed, 22 insertions(+), 39 deletions(-)
diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index 428b4aa..96fd94a 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -14,8 +14,8 @@
* with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
-#define VP_DEBUG
-#define VP_DEBUG_MODE 50
+//#define VP_DEBUG
+//#define VP_DEBUG_MODE 50
#include <sot/core/debug.hh>
#include <exception>
#ifdef VP_DEBUG
@@ -80,6 +80,9 @@ namespace soth
}
}
+bool ddxdriftInit=false;
+Eigen::VectorXd ddxdrift ;
+
namespace dynamicgraph
{
namespace sot
@@ -125,6 +128,7 @@ namespace dynamicgraph
,Ctasks(),btasks()
,solution()
,activeSet(),relevantActiveSet(false)
+
{
signalRegistration( controlSOUT
@@ -147,8 +151,6 @@ namespace dynamicgraph
makeDirectSetter(*this,&controlFreeFloating,
docDirectSetter("ouput control includes the ff (ie, size nbDof). Oterwise, size is nbDof-6. FF is supposed to be at the head.","bool")));
- addCommand("setSecondOrderKine",
- makeDirectSetter(*this,&secondOrderKinemat
std::string docstring =
"Set second order kinematic inversion\n"
"\n"
@@ -360,40 +362,14 @@ namespace dynamicgraph
} //if
else
{
- /* -Tasks 1:n- */
- /* Ctaski = [ Ji 0 0 0 0 0 ] */
-
- /*for( int i=0;i<(int)stack.size();++i )
- {
- TaskAbstract & task = * stack[i];
- MatrixXd & Ctask = Ctasks[i];
- VectorBound & btask = btasks[i];
-
- EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
- const dg::sot::VectorMultiBound & dx = task.taskSOUT(t);
- const int nx = dx.size();
-
- assert( Ctask.rows() == nx && btask.size() == nx );
- assert( J.rows()==nx && J.cols()==nbDofs && (int)dx.size()==nx );
-
- Ctask = J; COPY_MB_VECTOR_TO_EIGEN(dx,btask);
-
- sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
- sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
-
- } //for*/
-
-
for( int i=0;i<(int)stack.size();++i )
-
{
- //TaskAbstract & taskAb = new TaskDynPD;
TaskAbstract & taskAb = * stack[i];
- TaskDynPD & task = dynamic_cast<TaskDynPD &>(taskAb);
+ TaskDynPD & task = dynamic_cast<TaskDynPD &>(taskAb); //it can be static_cast cause of control
MatrixXd & Ctask = Ctasks[i];
- VectorBound & btask1 = btasks[i];
+ VectorBound & btask = btasks[i];
EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
@@ -407,17 +383,23 @@ namespace dynamicgraph
sotDEBUG(45) << "Jdot"<<i<<" = " << Jdot << std::endl;
sotDEBUG(1) << "dq = " << (MATLAB)dq << std::endl;
- assert( Ctask.rows() == nx1 && btask1.size() == nx1 );
+ assert( Ctask.rows() == nx1 && btask.size() == nx1 );
assert( J.rows()==nx1 && J.cols()==nbDofs && (int)ddx.size()==nx1 );
assert( Jdot.rows()==nx1 && Jdot.cols()==nbDofs );
Ctask = J;
- VectorXd ddxdrift = - (Jdot*dq);
+ if (!ddxdriftInit)
+ {
+ ddxdrift= VectorXd(nx1);
+ ddxdriftInit=true;
+ }
+ ddxdrift = - (Jdot*dq);
+
for( int c=0;c<nx1;++c )
{
if( ddx[c].getMode() == dg::sot::MultiBound::MODE_SINGLE )
- btask1[c] = ddx[c].getSingleBound() + ddxdrift[c];
+ btask[c] = ddx[c].getSingleBound() + ddxdrift[c];
else
{
const bool binf = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_INF );
@@ -426,24 +408,24 @@ namespace dynamicgraph
{
const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
- btask1[c] = std::make_pair( xi+ddxdrift[c], xs+ddxdrift[c] );
+ btask[c] = std::make_pair( xi+ddxdrift[c], xs+ddxdrift[c] );
}
else if( binf )
{
const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
- btask1[c] = Bound( xi+ddxdrift[c], Bound::BOUND_INF );
+ btask[c] = Bound( xi+ddxdrift[c], Bound::BOUND_INF );
}
else
{
assert( bsup );
const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
- btask1[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
+ btask[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
} //else
} //else
} //for c
sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
- sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
} //for i
} //else
diff --git a/src/solver-kine.h b/src/solver-kine.h
index 2ac4ba4..d2f319b 100644
--- a/src/solver-kine.h
+++ b/src/solver-kine.h
@@ -118,6 +118,7 @@ namespace dynamicgraph {
std::vector<soth::cstref_vector_t> activeSet;
bool relevantActiveSet;
+
}; // class SolverKine
} // namespace dyninv
--
GitLab