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, ...@@ -121,10 +121,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task, bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
double weight, double weight,
unsigned int priorityLevel, unsigned int priorityLevel,
double ) double transition_duration)
{ {
assert(weight>=0.0); assert(weight>=0.0);
assert(transition_duration>=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); TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskMotions.push_back(tl); m_taskMotions.push_back(tl);
addTask(tl, weight, priorityLevel); addTask(tl, weight, priorityLevel);
...@@ -136,10 +146,18 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task, ...@@ -136,10 +146,18 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task, bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
double weight, double weight,
unsigned int priorityLevel, unsigned int priorityLevel,
double ) double transition_duration)
{ {
assert(weight>=0.0); assert(weight>=0.0);
assert(transition_duration>=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); TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskContactForces.push_back(tl); m_taskContactForces.push_back(tl);
addTask(tl, weight, priorityLevel); addTask(tl, weight, priorityLevel);
...@@ -150,10 +168,18 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task, ...@@ -150,10 +168,18 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task, bool InverseDynamicsFormulationAccForce::addTorqueTask(TaskActuation & task,
double weight, double weight,
unsigned int priorityLevel, unsigned int priorityLevel,
double ) double transition_duration)
{ {
assert(weight>=0.0); assert(weight>=0.0);
assert(transition_duration>=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); TaskLevel *tl = new TaskLevel(task, priorityLevel);
m_taskActuations.push_back(tl); m_taskActuations.push_back(tl);
......
...@@ -278,12 +278,12 @@ namespace tsid ...@@ -278,12 +278,12 @@ namespace tsid
if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon) if(nVars!=m_nVars || nEqCon!=m_nEqCon || nIneqCon!=m_nIneqCon)
reset(nVars, nEqCon, nIneqCon); reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows()==m_nVars && Hess.cols()==m_nVars); assert(static_cast<math::Index>(Hess.rows())==m_nVars && static_cast<math::Index>(Hess.cols())==m_nVars);
assert(g0.size()==m_nVars); assert(static_cast<math::Index>(g0.size())==m_nVars);
assert(CE.rows()==m_nEqCon && CE.cols()==m_nVars); assert(static_cast<math::Index>(CE.rows())==m_nEqCon && static_cast<math::Index>(CE.cols())==m_nVars);
assert(ce0.size()==m_nEqCon); assert(static_cast<math::Index>(ce0.size())==m_nEqCon);
assert(CI.rows()==m_nIneqCon && CI.cols()==m_nVars); assert(static_cast<math::Index>(CI.rows())==m_nIneqCon && static_cast<math::Index>(CI.cols())==m_nVars);
assert(ci0.size()==m_nIneqCon); assert(static_cast<math::Index>(ci0.size())==m_nIneqCon);
math::Index i, k, l; // indices math::Index i, k, l; // indices
math::Index ip; // index of the chosen violated constraint 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