From b1aae5f102102a7e1b3121d8bdfd8526f4b0fb42 Mon Sep 17 00:00:00 2001
From: Francesco Morsillo <fmorsill@laas.fr>
Date: Tue, 11 Jun 2013 10:40:03 +0200
Subject: [PATCH] Added gain utilisation to task-dyn-limits.cpp and added some
useful comments
---
src/task-dyn-limits.cpp | 12 +++++++-----
src/task-dyn-pd.cpp | 2 +-
2 files changed, 8 insertions(+), 6 deletions(-)
diff --git a/src/task-dyn-limits.cpp b/src/task-dyn-limits.cpp
index dcd5b06..b99ef20 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 f209c2e..bd21ca1 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());
--
GitLab