diff --git a/src/task-dyn-limits.cpp b/src/task-dyn-limits.cpp index dcd5b06c5996027299c6a72a9fdbebc5495bd924..b99ef208433844177aa10b707420cf62ee86be48 100644 --- a/src/task-dyn-limits.cpp +++ b/src/task-dyn-limits.cpp @@ -102,7 +102,9 @@ namespace dynamicgraph const ml::Vector & refPosSup = referencePosSupSIN(time); const ml::Vector & refVelInf = referenceVelInfSIN(time); const ml::Vector & refVelSup = referenceVelSupSIN(time); - const double & dt = dtSIN(time); + //const double & dt = dtSIN(time); + const double & gain = controlGainSIN(time); + const double & dt = dtSIN(time)/gain; sotDEBUG(35) << "position = " << position << std::endl; sotDEBUG(35) << "velocity = " << velocity << std::endl; @@ -113,10 +115,10 @@ namespace dynamicgraph res.resize(position.size()); for( unsigned int i=0;i<res.size();++i ) { - maxVel = 1/dt*(refVelSup(i)-velocity(i)); - minVel = 1/dt*(refVelInf(i)-velocity(i)); - maxPos = kt*(refPosSup(i)-position(i)-dt*velocity(i)); - minPos = kt*(refPosInf(i)-position(i)-dt*velocity(i)); + maxVel = 1/dt*(refVelSup(i)-velocity(i)); // MAXimum acceleration value due to VELocity limits + minVel = 1/dt*(refVelInf(i)-velocity(i)); // MINimum acceleration value due to VELocity limits + maxPos = kt*(refPosSup(i)-position(i)-dt*velocity(i)); // MAXimum acceleration value due to POSition limits + minPos = kt*(refPosInf(i)-position(i)-dt*velocity(i)); // MINimum acceleration value due to POSition limits maxLim = maxVel<maxPos ? maxVel : maxPos; minLim = minVel>minPos ? minVel : minPos; diff --git a/src/task-dyn-pd.cpp b/src/task-dyn-pd.cpp index f209c2e428243918292cf7345a51fe58761f8be9..bd21ca12c9e357908506be66a13dc912496a349a 100644 --- a/src/task-dyn-pd.cpp +++ b/src/task-dyn-pd.cpp @@ -158,7 +158,7 @@ namespace dynamicgraph ml::Vector& TaskDynPD:: - taskVectorSOUT_function( ml::Vector& taskV, int time ) + taskVectorSOUT_function( ml::Vector& taskV, int time ) { const dg::sot::VectorMultiBound & task = taskSOUT(time); taskV.resize(task.size());