diff --git a/src/solver-kine.cpp b/src/solver-kine.cpp
index fd9575efe83fe274fc912f7db84a9142cf1bf074..428b4aac0584af1c76047a51b58b8ba6f6db73cd 100644
--- a/src/solver-kine.cpp
+++ b/src/solver-kine.cpp
@@ -17,6 +17,7 @@
#define VP_DEBUG
#define VP_DEBUG_MODE 50
#include <sot/core/debug.hh>
+#include <exception>
#ifdef VP_DEBUG
class solver_op_space__INIT
{
@@ -43,7 +44,7 @@ solver_op_space__INIT solver_op_space_initiator;
namespace soth
{
- Bound& operator -= (Bound& xb, const double & x )
+ Bound& operator -= (Bound& xb, const double & )
{
return xb;
}
@@ -147,8 +148,17 @@ namespace dynamicgraph
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")));
+ makeDirectSetter(*this,&secondOrderKinemat
+ std::string docstring =
+ "Set second order kinematic inversion\n"
+ "\n"
+ " Input: bool\n"
+ "\n"
+ " If true, check that the solver is empty, since second order\n"
+ " kinematics requires tasks to be of type TaskDynPD.";
+ addCommand("setSecondOrderKine",
+ makeCommandVoid1(*this,&SolverKine::setSecondOrderKine,
+ docstring));
addCommand("getSecondOrderKine",
makeDirectGetter(*this,&secondOrderKinematics,
@@ -160,6 +170,38 @@ namespace dynamicgraph
/* --- STACK ----------------------------------------------------------- */
/* --- STACK ----------------------------------------------------------- */
/* --- STACK ----------------------------------------------------------- */
+
+ void SolverKine::push (TaskAbstract& task)
+ {
+ if (secondOrderKinematics) {
+ checkDynamicTask (task);
+ }
+ sot::Stack< TaskAbstract >::push (task);
+ }
+
+ void SolverKine::checkDynamicTask (const TaskAbstract& task) const
+ {
+ try {
+ dynamic_cast <const TaskDynPD &> (task);
+ } catch (const std::bad_cast& esc) {
+ std::string taskName = task.getName ();
+ std::string className = task.getClassName ();
+ std::string msg ("Type " + className + " of task \"" + taskName +
+ "\" does not derive from TaskDynPD");
+ throw std::runtime_error (msg);
+ }
+ }
+
+ void SolverKine::setSecondOrderKine (const bool& secondOrder)
+ {
+ if (secondOrder) {
+ if (stack.size() != 0) {
+ throw std::runtime_error
+ ("The solver should contain no task before switching to second order mode.");
+ }
+ }
+ secondOrderKinematics = secondOrder;
+ }
SolverKine::TaskDependancyList_t SolverKine::
getTaskDependancyList( const TaskAbstract& task )
@@ -321,65 +363,88 @@ namespace dynamicgraph
/* -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);
+ /*for( int i=0;i<(int)stack.size();++i )
+ {
+ TaskAbstract & task = * stack[i];
+ MatrixXd & Ctask = Ctasks[i];
+ VectorBound & btask = btasks[i];
- MatrixXd & Ctask = Ctasks[i];
- VectorBound & btask1 = 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);
- 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));
+ sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask << std::endl;
- const int nx1 = ddx.size();
+ } //for*/
- 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;
+ for( int i=0;i<(int)stack.size();++i )
- VectorXd ddxdrift = - (Jdot*dq);
- for( int c=0;c<nx1;++c )
+ {
+ //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 )
{
- if( ddx[c].getMode() == dg::sot::MultiBound::MODE_SINGLE )
- btask1[c] = ddx[c].getSingleBound() + ddxdrift[c];
+ const double xi = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_INF);
+ btask1[c] = Bound( xi+ddxdrift[c], Bound::BOUND_INF );
+ }
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
+ assert( bsup );
+ const double xs = ddx[c].getDoubleBound(dg::sot::MultiBound::BOUND_SUP);
+ btask1[c] = Bound( xs+ddxdrift[c], Bound::BOUND_SUP );
} //else
- } //for c
-
- sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
- sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
- } //for i
+ } //else
+ } //for c
+
+ sotDEBUG(15) << "Ctask"<<i<<" = " << (MATLAB)Ctask << std::endl;
+ sotDEBUG(1) << "btask"<<i<<" = " << btask1 << std::endl;
+ } //for i
} //else
/* --- */
diff --git a/src/solver-kine.h b/src/solver-kine.h
index 91ac1bd48891bc1e593605a3714d5490222deba6..2ac4ba4a144da78ed543e13efa8a3efbced01cf4 100644
--- a/src/solver-kine.h
+++ b/src/solver-kine.h
@@ -95,11 +95,17 @@ namespace dynamicgraph {
void getDecomposition( const int &stage );
bool controlFreeFloating;
bool secondOrderKinematics;
+ /// Push the task in the stack.
+ /// Call parent implementation anc check that task is
+ /// of type dynamic if necessary
+ virtual void push( TaskAbstract& task );
+ void setSecondOrderKine (const bool& secondOrder);
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
bool checkSolverSize( void );
void resizeSolver( void );
+ void checkDynamicTask (const TaskAbstract& task) const;
private:
typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;