Commit 2f1bb1c3 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix problems in DEBUG and default mode (not DEBUG and not RELEASE).

parent 81de7df1
......@@ -121,10 +121,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
double weight,
unsigned int priorityLevel,
double )
double transition_duration)
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
// This part is not used frequently so we can do some tests.
if (weight>=0.0)
std::cerr << __FILE__ << " " << __LINE__ << " "
<< "weight should be positive" << std::endl;
// This part is not used frequently so we can do some tests.
if (transition_duration>=0.0)
std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskMotions.push_back(tl);
addTask(tl, weight, priorityLevel);
......@@ -136,10 +146,18 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
double weight,
unsigned int priorityLevel,
double )
double transition_duration)
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
// This part is not used frequently so we can do some tests.
if (weight>=0.0)
std::cerr << __FILE__ << " " << __LINE__ << " "
<< "weight should be positive" << std::endl;
// This part is not used frequently so we can do some tests.
if (transition_duration>=0.0)
std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskContactForces.push_back(tl);
addTask(tl, weight, priorityLevel);
......@@ -150,10 +168,18 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task,
double weight,
unsigned int priorityLevel,
double )
double transition_duration)
{
assert(weight>=0.0);
assert(transition_duration>=0.0);
if (weight>=0.0)
std::cerr << __FILE__ << " " << __LINE__ << " "
<< "weight should be positive" << std::endl;
// This part is not used frequently so we can do some tests.
if (transition_duration>=0.0)
std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskActuations.push_back(tl);
......
......@@ -278,12 +278,12 @@ namespace tsid
if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon)
reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows()==m_nVars && Hess.cols()==m_nVars);
assert(g0.size()==m_nVars);
assert(CE.rows()==m_nEqCon && CE.cols()==m_nVars);
assert(ce0.size()==m_nEqCon);
assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars);
assert(ci0.size()==m_nIneqCon);
assert(static_cast<math::Index>(Hess.rows())==m_nVars && static_cast<math::Index>(Hess.cols())==m_nVars);
assert(static_cast<math::Index>(g0.size())==m_nVars);
assert(static_cast<math::Index>(CE.rows())==m_nEqCon && static_cast<math::Index>(CE.cols())==m_nVars);
assert(static_cast<math::Index>(ce0.size())==m_nEqCon);
assert(static_cast<math::Index>(CI.rows())==m_nIneqCon && static_cast<math::Index>(CI.cols())==m_nVars);
assert(static_cast<math::Index>(ci0.size())==m_nIneqCon);
math::Index i, k, l; // indices
math::Index ip; // index of the chosen violated constraint
......
Supports Markdown
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