From 8eb4d5efc3ca94985e423ce4922899ac3f15ebed Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Thu, 17 Jan 2013 15:21:14 +0100
Subject: [PATCH] Revert "Added an operator into solver-kine."
This reverts commit d16b100c978423e422c9cb0d80cde2d4b5962586.
---
src/solver-kine.cpp | 137 +++-----------------------------------------
1 file changed, 7 insertions(+), 130 deletions(-)
diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index 1211d5e..0bfa094 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -20,7 +20,7 @@
#ifdef VP_DEBUG
class solver_op_space__INIT
{
- //public:solver_op_space__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
+public:solver_op_space__INIT( void ) { dynamicgraph::sot::DebugTrace::openFile(); }
};
solver_op_space__INIT solver_op_space_initiator;
#endif //#ifdef VP_DEBUG
@@ -39,45 +39,7 @@ solver_op_space__INIT solver_op_space_initiator;
#include <soth/Algebra.hpp>
#include <Eigen/QR>
#include <sot-dyninv/mal-to-eigen.h>
-#include <sys/time.h>
-namespace soth
-{
- Bound& operator -= (Bound& xb, const double & x )
- {
- return xb;
- }
-
- const Bound operator - (const Bound& a, const Bound & b )
- {
- assert( b.getType()==Bound::BOUND_TWIN || a.getType()==b.getType() );
-
- if( b.getType() ==Bound::BOUND_TWIN )
- {
- switch( a.getType() )
- {
- case Bound::BOUND_TWIN:
- case Bound::BOUND_INF:
- case Bound::BOUND_SUP:
- return Bound(a.getBound(a.getType())-b.getBound(Bound::BOUND_TWIN),
- a.getType());
- break;
- case Bound::BOUND_DOUBLE:
- return Bound(a.getBound(Bound::BOUND_INF)-b.getBound(Bound::BOUND_TWIN),
- a.getBound(Bound::BOUND_SUP)-b.getBound(Bound::BOUND_TWIN));
- break;
- }
- }
- else
- {
- // TODO
- throw "TODO";
- }
-
-
- return a;
- }
-}
namespace dynamicgraph
{
@@ -90,17 +52,7 @@ namespace dynamicgraph
using namespace dg;
using dg::SignalBase;
- static bool isLH(boost::shared_ptr<soth::Stage> s)
- {
- return s->name == "tasklh";
- }
- static bool isRH(boost::shared_ptr<soth::Stage> s)
- {
- return s->name == "taskrhorient";
- }
-
-
- /* --- DG FACTORY ------------------------------------------------------- */
+ /* --- DG FACTORY ------------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SolverKine,"SolverKine");
/* --- CONSTRUCTION ----------------------------------------------------- */
@@ -122,8 +74,6 @@ namespace dynamicgraph
,Ctasks(),btasks()
,solution()
- ,activeSet(),relevantActiveSet(false)
-
{
signalRegistration( controlSOUT << velocitySOUT
<< dampingSIN );
@@ -133,14 +83,6 @@ namespace dynamicgraph
makeCommandVoid0(*this,&SolverKine::debugOnce,
docCommandVoid0("open trace-file for next iteration of the solver.")));
- addCommand("resetAset",
- makeCommandVoid0(*this,&SolverKine::resetAset,
- docCommandVoid0("Reset the active set.")));
-
- addCommand("decompo",
- makeCommandVoid1(*this,&SolverKine::getDecomposition,
- docCommandVoid1("Return the decomposition of the given level.","Stage level")));
-
addCommand("setControlFreeFloating",
makeDirectSetter(*this,&controlFreeFloating,
docDirectSetter("If true, the ouput control includes the ff (ie, size nbDof). Oterwise, size is nbDof-6. FF is supposed to be at the head.","bool")));
@@ -178,25 +120,6 @@ namespace dynamicgraph
controlSOUT.setReady();
}
- /* --- NOTIFICATION HCOD ------------------------------------------------ */
-
- struct HCODUpdateCounter
- {
- void operator() (std::string stage,soth::ConstraintRef cst, std::string event)
- {
- if(!onoff) return;
- //sotDEBUG(0)
- //std::cout<< "At t="<<iter<<" [" << stage << "," << cst << "]: " << event << std::endl;
- count ++;
- }
- static int iter;
- static int count;
- static bool onoff;
- HCODUpdateCounter() {count = 0;}
- };
- int HCODUpdateCounter::count = 0;
- int HCODUpdateCounter::iter = 0;
- bool HCODUpdateCounter::onoff = true;
/* --- INIT SOLVER ------------------------------------------------------ */
/* --- INIT SOLVER ------------------------------------------------------ */
@@ -221,22 +144,8 @@ namespace dynamicgraph
resizeSolver( void )
{
hsolver = hcod_ptr_t(new soth::HCOD( nbDofs,stack.size() ));
- debugOnce();
-
Ctasks.resize(stack.size());
btasks.resize(stack.size());
- relevantActiveSet = false;
-
- // std::cout << controlSOUT.getTime() << std::endl;
- // if( controlSOUT.getTime() == 10)
- // {
- // std::cout <<"Exceptionaly keep the aset." << std::endl;
- // relevantActiveSet = true;
- // activeSet[5] = soth::cstref_vector_t();
- // activeSet[6] = soth::cstref_vector_t();
- // activeSet.push_back(soth::cstref_vector_t());
- // activeSet.push_back(soth::cstref_vector_t());
- // }
int i=0;
BOOST_FOREACH( TaskAbstract* task, stack )
@@ -251,7 +160,6 @@ namespace dynamicgraph
}
solution.resize( nbDofs );
- hsolver->notifiorRegistration(HCODUpdateCounter());
}
/* Return true iff the solver sizes fit to the task set. */
@@ -280,18 +188,15 @@ namespace dynamicgraph
return !toBeResized;
}
-
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
- std::ofstream fup("/tmp/up.dat");
ml::Vector& SolverKine::
controlSOUT_function( ml::Vector &mlcontrol, int t )
{
sotDEBUG(15) << " # In time = " << t << std::endl;
- HCODUpdateCounter::iter = t;
refreshTaskTime( t );
if(! checkSolverSize() ) resizeSolver();
@@ -324,6 +229,9 @@ namespace dynamicgraph
const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
const int nx = ddx.size();
+ sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
+ sotDEBUG(25) << "J"<<i<<" = " << J << std::endl;
+
assert( Ctask.rows() == nx && btask.size() == nx );
assert( J.rows()==nx && J.cols()==nbDofs && (int)ddx.size()==nx );
@@ -335,24 +243,13 @@ namespace dynamicgraph
/* --- */
sotDEBUG(1) << "Initial config." << std::endl;
- double time= 0;
-
hsolver->reset();
- if(relevantActiveSet)
- hsolver->setInitialActiveSet(activeSet);
- else hsolver->setInitialActiveSet();
-
- struct timeval t0,t1;
- gettimeofday(&t0,NULL);
+ hsolver->setInitialActiveSet();
sotDEBUG(1) << "Run for a solution." << std::endl;
hsolver->activeSearch(solution);
sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
- gettimeofday(&t1,NULL);
- time = ((t1.tv_sec-t0.tv_sec)+(t1.tv_usec-t0.tv_usec)/1.0e6);
-
- activeSet = hsolver->getOptimalActiveSet(); relevantActiveSet = true;
if( controlFreeFloating )
{
@@ -365,12 +262,6 @@ namespace dynamicgraph
control=solution.tail( nbDofs-6 );
}
- fup << t << "\t" << HCODUpdateCounter::count
- << " " << time << " " << hsolver->sizeA();
- fup << std::endl;
-
- HCODUpdateCounter::count = 0;
-
sotDEBUG(1) << "control = " << mlcontrol << std::endl;
return mlcontrol;
}
@@ -383,22 +274,8 @@ namespace dynamicgraph
void SolverKine::
debugOnce( void )
{
- std::cout << "Open the trace"<<std::endl;
dg::sot::DebugTrace::openFile("/tmp/sot.txt");
- hsolver->debugOnce("/tmp/soth.txt",true);
- }
- void SolverKine::
- resetAset( void )
- {
- relevantActiveSet = false;
- }
-
- void SolverKine::
- getDecomposition(const int & i)
- {
- using namespace soth;
- std::cout << "M"<<i<<" = " << (MATLAB) hsolver -> stage(i).getM() << std::endl;
- std::cout << "L"<<i<<" = " << (MATLAB)(MatrixXd) (hsolver -> stage(i).getLtri()) << std::endl;
+ hsolver->debugOnce();
}
/* --- ENTITY ----------------------------------------------------------- */
--
GitLab