From 34602223bad94ed2f21ca6cab1d3af45e93704a1 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 26 Jul 2011 15:04:56 +0200
Subject: [PATCH] Accounting for new gpool().
---
src/pseudo-robot-dynamic.cpp | 6 +++---
src/solver-kine.cpp | 2 +-
src/solver-op-space.cpp | 35 +++++++++++++++++++++++------------
src/stack-template.t.cpp | 2 +-
4 files changed, 28 insertions(+), 17 deletions(-)
diff --git a/src/pseudo-robot-dynamic.cpp b/src/pseudo-robot-dynamic.cpp
index 863a965..48728af 100644
--- a/src/pseudo-robot-dynamic.cpp
+++ b/src/pseudo-robot-dynamic.cpp
@@ -201,10 +201,10 @@ namespace dynamicgraph
replaceSimulatorEntity( const std::string& formerName, const bool & plug )
{
assert( formerOpenHRP == NULL );
- formerOpenHRP = &dg::g_pool.getEntity( formerName );
- dg::g_pool.deregisterEntity( formerName );
+ formerOpenHRP = &dg::g_pool().getEntity( formerName );
+ dg::g_pool().deregisterEntity( formerName );
// former.name = formerName+".old";
- dg::g_pool.registerEntity( formerName+"-old",formerOpenHRP );
+ dg::g_pool().registerEntity( formerName+"-old",formerOpenHRP );
entityDeregistration(); name = formerName; entityRegistration();
diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index c1d98d3..0bfa094 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -14,7 +14,7 @@
* with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
-#define VP_DEBUG
+//#define VP_DEBUG
#define VP_DEBUG_MODE 50
#include <sot/core/debug.hh>
#ifdef VP_DEBUG
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index d8c8b33..7eb439a 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.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("/tmp/dynh.txt"); }
};
solver_op_space__INIT solver_op_space_initiator;
#endif //#ifdef VP_DEBUG
@@ -173,7 +173,7 @@ namespace dynamicgraph
{
using namespace dynamicgraph::sot;
- TaskDynPD & task = dynamic_cast<TaskDynPD&> ( g_pool.getEntity( taskName ) );
+ TaskDynPD & task = dynamic_cast<TaskDynPD&> ( g_pool().getEntity( taskName ) );
assert( task.getFeatureList().size() == 1 );
BOOST_FOREACH( FeatureAbstract* fptr, task.getFeatureList() )
{
@@ -501,8 +501,9 @@ namespace dynamicgraph
{
Contact & contact = pContact.second;
EIGEN_CONST_MATRIX_FROM_SIGNAL(Jc,(*contact.jacobianSIN)(t));
- const int ri = contact.range.first;
+ const int ri = contact.range.first,rs = contact.range.second;
Cdyn.COLS_F.COLS(ri,ri+6) = Jc.transpose();
+ Cdyn.COLS_F.COLS(ri+6,rs).setZero();
}
for( int i=0;i<nbDofs+6;++i ) bdyn[i] = -b[i];
@@ -562,18 +563,23 @@ namespace dynamicgraph
::RowsBlockXpr::RowsBlockXpr Czi
= Czmp.COLS_F.COLS(ri,rs). ROWS(rows,rows+3);
computeForceNormalConversion(Czi,support);
- for( int i=0;i<3;++i ) bzmp. ROWS(rows,rows+3)[i] = 0;
-
- for( int p=0;p<nbP;++p )
- {
- Czmp.COLS_F.COLS(ri+6,rs).ROWS(rows+3,rows+3+nbP)
- (p,p) = 1;
- bzmp.ROWS(rows+3,rows+3+nbP)
- [p] = soth::Bound(0,soth::Bound::BOUND_SUP);
- }
+ //for( int i=0;i<3;++i ) bzmp. ROWS(rows,rows+3)[i] = 0;
+ bzmp. ROWS(rows,rows+3).fill( 0 );
+
+ Czmp.COLS_F.COLS(ri,ri+6).ROWS(rows+3,rows+3+nbP).setZero();
+ Czmp.COLS_F.COLS(ri+6,rs).ROWS(rows+3,rows+3+nbP).setIdentity();
+ bzmp.ROWS(rows+3,rows+3+nbP).fill( soth::Bound(0,soth::Bound::BOUND_SUP) );
+ // for( int p=0;p<nbP;++p )
+ // {
+ // Czmp.COLS_F.COLS(ri+6,rs).ROWS(rows+3,rows+3+nbP)
+ // (p,p) = 1;
+ // bzmp.ROWS(rows+3,rows+3+nbP)
+ // [p] = soth::Bound(0,soth::Bound::BOUND_SUP);
+ // }
rows += 3+nbP;
}
sotDEBUG(15) << "Czmp = " << (MATLAB)Czmp << std::endl;
+ sotDEBUG(15) << "Czmp = " << Czmp << std::endl;
sotDEBUG(1) << "bzmp = " << bzmp << std::endl;
}
@@ -666,9 +672,14 @@ namespace dynamicgraph
sotDEBUG(1) << "Run for a solution." << std::endl;
hsolver->activeSearch(solution);
sotDEBUG(1) << "solution = " << (MATLAB)solution << std::endl;
+ sotDEBUG(1) << "solution = " << solution << std::endl;
EIGEN_VECTOR_FROM_VECTOR( tau,control,nbDofs );
+ sotDEBUG(1) << "controlb = " << control << std::endl;
tau = solution.transpose().COLS_TAU;
+ sotDEBUG(1) << "tau = " << tau << std::endl;
+ sotDEBUG(1) << "stct = " << (MATLAB)solution.transpose().COLS_TAU << std::endl;
+ sotDEBUG(1) << "controla = " << control << std::endl;
/* --- forces signal --- */
BOOST_FOREACH(contacts_t::value_type& pContact, contactMap)
diff --git a/src/stack-template.t.cpp b/src/stack-template.t.cpp
index 28135e8..fc3c1be 100644
--- a/src/stack-template.t.cpp
+++ b/src/stack-template.t.cpp
@@ -195,7 +195,7 @@ namespace dynamicgraph
void Stack<TaskGeneric>:: \
ACT##ByTaskName( const std::string& tname ) \
{ \
- dg::Entity & taska = dg::g_pool.getEntity( tname ); \
+ dg::Entity & taska = dg::g_pool().getEntity( tname ); \
TaskGeneric & task = dynamic_cast<TaskGeneric&>( taska ); \
ACT(task); \
}
--
GitLab