diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index c3e53f5b79a9f5bce6eb0897b1e0ad178d461fe4..fd9575efe83fe274fc912f7db84a9142cf1bf074 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -14,13 +14,13 @@
* 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
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/sot-kine-deb.txt"); }
};
solver_op_space__INIT solver_op_space_initiator;
#endif //#ifdef VP_DEBUG
@@ -112,11 +112,12 @@ namespace dynamicgraph
,stack_t()
,CONSTRUCT_SIGNAL_IN(damping,double)
+ ,CONSTRUCT_SIGNAL_IN(velocity,ml::Vector)
,CONSTRUCT_SIGNAL_OUT(control,ml::Vector,
dampingSIN )
- ,CONSTRUCT_SIGNAL(velocity,OUT,ml::Vector)
,controlFreeFloating(true)
+ ,secondOrderKinematics(false)
,hsolver()
@@ -125,8 +126,8 @@ namespace dynamicgraph
,activeSet(),relevantActiveSet(false)
{
- signalRegistration( controlSOUT << velocitySOUT
- << dampingSIN );
+ signalRegistration( controlSOUT
+ << dampingSIN << velocitySIN );
/* Command registration. */
addCommand("debugOnce",
@@ -143,7 +144,15 @@ namespace dynamicgraph
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")));
+ 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,&secondOrderKinematics,
+ docDirectSetter("second order kinematic inversion","bool")));
+
+ addCommand("getSecondOrderKine",
+ makeDirectGetter(*this,&secondOrderKinematics,
+ docDirectGetter("second order kinematic inversion","bool")));
ADD_COMMANDS_FOR_THE_STACK;
}
@@ -252,6 +261,11 @@ namespace dynamicgraph
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
/* --- SIGNALS ---------------------------------------------------------- */
+
+#define COLS_Q leftCols( nbDofs )
+#define COLS_TAU leftCols( nbDofs+ntau ).rightCols( ntau )
+#define COLS_F rightCols( nfs )
+
ml::Vector& SolverKine::
controlSOUT_function( ml::Vector &mlcontrol, int t )
{
@@ -279,24 +293,94 @@ namespace dynamicgraph
/* -Tasks 1:n- */
/* Ctaski = [ Ji 0 0 0 0 0 ] */
- for( int i=0;i<(int)stack.size();++i )
+
+ if( !secondOrderKinematics )
{
- TaskAbstract & task = * stack[i];
- MatrixXd & Ctask = Ctasks[i];
- VectorBound & btask = btasks[i];
+ 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 & ddx = task.taskSOUT(t);
- const int nx = ddx.size();
+ 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)ddx.size()==nx );
+ 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(ddx,btask);
+ Ctask = J; COPY_MB_VECTOR_TO_EIGEN(dx,btask);
- sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
- sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
- }
+ sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
+
+ } //for
+ } //if
+ else
+ {
+ /* -Tasks 1:n- */
+ /* Ctaski = [ Ji 0 0 0 0 0 ] */
+
+ for( int i=0;i<(int)stack.size();++i )
+ {
+ //TaskAbstract & taskAb = new TaskDynPD;
+ TaskAbstract & taskAb = * stack[i];
+ TaskDynPD & task = dynamic_cast<TaskDynPD &>(taskAb);
+
+ MatrixXd & Ctask = Ctasks[i];
+ VectorBound & btask1 = btasks[i];
+
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(Jdot,task.JdotSOUT(t));
+ EIGEN_CONST_MATRIX_FROM_SIGNAL(J,task.jacobianSOUT(t));
+ const dg::sot::VectorMultiBound & ddx = task.taskSOUT(t);
+ EIGEN_CONST_VECTOR_FROM_SIGNAL(dq,velocitySIN(t));
+
+ const int nx1 = ddx.size();
+
+ sotDEBUG(5) << "ddx"<<i<<" = " << ddx << std::endl;
+ sotDEBUG(25) << "J"<<i<<" = " << J << std::endl;
+ sotDEBUG(45) << "Jdot"<<i<<" = " << Jdot << std::endl;
+ sotDEBUG(1) << "dq = " << (MATLAB)dq << std::endl;
+
+ assert( Ctask.rows() == nx1 && btask1.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);
+ for( int c=0;c<nx1;++c )
+ {
+ if( ddx[c].getMode() == dg::sot::MultiBound::MODE_SINGLE )
+ btask1[c] = ddx[c].getSingleBound() + ddxdrift[c];
+ else
+ {
+ const bool binf = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_INF );
+ const bool bsup = ddx[c].getDoubleBoundSetup( dg::sot::MultiBound::BOUND_SUP );
+ if( binf&&bsup )
+ {
+ 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] );
+ }
+ else if( binf )
+ {
+ const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
+ btask1[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 );
+ } //else
+ } //else
+ } //for c
+
+ sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
+ } //for i
+ } //else
/* --- */
sotDEBUG(1) << "Initial config." << std::endl;
@@ -366,6 +450,7 @@ namespace dynamicgraph
os <<"control = "<<controlSOUT.accessCopy() <<std::endl << std::endl;
} catch (dynamicgraph::ExceptionSignal e) {}
stack_t::display(os);
+
}
void SolverKine::
diff --git a/src/solver-kine.h b/src/solver-kine.h
index 24bbd666dca698d54621abda090c68bc896fe7ce..91ac1bd48891bc1e593605a3714d5490222deba6 100644
--- a/src/solver-kine.h
+++ b/src/solver-kine.h
@@ -86,14 +86,15 @@ namespace dynamicgraph {
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(damping,double);
+ DECLARE_SIGNAL_IN(velocity,ml::Vector); //only used for second order kinematics
DECLARE_SIGNAL_OUT(control,ml::Vector);
- DECLARE_SIGNAL(velocity,OUT,ml::Vector);
public: /* --- COMMANDS --- */
void debugOnce( void );
void resetAset( void );
void getDecomposition( const int &stage );
bool controlFreeFloating;
+ bool secondOrderKinematics;
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
diff --git a/src/solver-op-space.cpp b/src/solver-op-space.cpp
index afdf26831e9e1065eee7df95bcfe4c79c168e4e4..6dd86b7b604a5edadbcb98c2014e1e7e4a20ddb1 100644
--- a/src/solver-op-space.cpp
+++ b/src/solver-op-space.cpp
@@ -475,7 +475,7 @@ namespace dynamicgraph
* -1- A ddq + b + J'f = S' tau
* -2- J ddq = 0
* -3- Xp f > eps
- * -i- Ji qddot = ddxi - Jidot qdot
+ * -i- Ji ddq = ddxi - Jidot dq
*
* -1- [ A -S' J' ] [ ddq; tau; f ] = -b
* -2- [ J 0 0 ] [ ddq; tau; f ] = 0
@@ -635,13 +635,13 @@ namespace dynamicgraph
assert( bsup );
const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
btask1[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
- }
- }
- }
+ } //else
+ } //else
+ } //for c
sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask1 << std::endl;
sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
- }
+ } //for i
/* -Last stage- */
/* Czero = [ [0(30x6) I(30x30)] 0 0 0 0 0 ] */