Skip to content

GitLab

Commit 26ba11b6 authored by Francesco Morsillo's avatar Francesco Morsillo
Browse files

Added control on task type when second order is set

parents cf6b4ef5 c1a9ada1
Hide whitespace changes
Inline Side-by-side
src/solver-kine.cpp
View file @ 26ba11b6
......@@ -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
/* --- */
......
src/solver-kine.h
View file @ 26ba11b6
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment