From e8625362d3dcdfd170f5dc1f774d9d5ad2cb5561 Mon Sep 17 00:00:00 2001
From: Armar6Demo <Armar6Demo@h2t.com>
Date: Mon, 12 Jun 2017 19:23:42 +0200
Subject: [PATCH] worked on basic controllers
---
.../units/RobotUnit/BasicControllers.cpp | 29 ++++++++++++-------
.../units/RobotUnit/BasicControllers.h | 8 ++---
.../RobotUnit/test/BasicControllerTest.cpp | 2 +-
3 files changed, 23 insertions(+), 16 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index f4ddf940d..16b3511bf 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -45,7 +45,7 @@ namespace armarx
//we can have 3 cases:
// 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
// 2. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
- // 3. we need to decellerate (other cases)
+ // 3. we need to decelerate (other cases)
//handle case 1
const float curverror = targetV - currentV;
@@ -105,7 +105,7 @@ namespace armarx
// 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them, e.g. if we violated them initially)
// 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
// 3. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
- // 4. we need to decellerate (other cases)
+ // 4. we need to decelerate (other cases)
float nextv;
//handle case 1
const float vsquared = currentV * currentV;
@@ -119,7 +119,7 @@ namespace armarx
const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
- //decellerate!
+ //decelerate!
// s = v²/(2a) <=> a = v²/(2s)
const float dec = std::abs(vsquared / 2.f / wayToGo);
const float vel = currentV - sign(currentV) * dec * upperDt;
@@ -165,7 +165,7 @@ namespace armarx
// 2. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
// the brakingDistance have the same sign and brakingDistance < e
// and currentVel <= maxV)
- // 3. we need to decellerate (other cases)
+ // 3. we need to decelerate (other cases)
//handle case 1
const float positionError = targetPosition - currentPosition;
@@ -219,7 +219,7 @@ namespace armarx
// 4. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
// and the brakingDistance have the same sign and brakingDistance < e
// and currentVel <= maxV)
- // 5. we need to decellerate (other cases)
+ // 5. we need to decelerate (other cases)
//handle case 1
const float vsquared = currentV * currentV;
@@ -324,7 +324,7 @@ namespace armarx
//we can have 3 cases:
// 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
// 2. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
- // 3. we need to decellerate (other cases)
+ // 3. we need to decelerate (other cases)
//handle case 1
const float curverror = targetV - currentV;
@@ -373,6 +373,7 @@ namespace armarx
{
if (currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
{
+ ARMARX_INFO << deactivateSpam(1) << "Hard limit violation. " << VAROUT(currentPosition) << VAROUT(positionLimitLoHard) << VAROUT(positionLimitHiHard);
return std::nanf("1");
}
@@ -392,7 +393,7 @@ namespace armarx
// 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them, e.g. if we violated them initially)
// 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
// 3. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
- // 4. we need to decellerate (other cases)
+ // 4. we need to decelerate (other cases)
float nextv;
//handle case 1
const float vsquared = currentV * currentV;
@@ -405,13 +406,14 @@ namespace armarx
const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
const float wayToGo = limit - currentPosition;
- //decellerate!
+ //decelerate!
// s = v²/(2a) <=> a = v²/(2s)
const float dec = std::abs(vsquared / 2.f / wayToGo);
const float vel = currentV - sign(currentV) * dec * upperDt;
nextv = boost::algorithm::clamp(vel, -maxV, maxV);
if (sign(currentV) != sign(nextv))
{
+ ARMARX_INFO << deactivateSpam(1) << "wrong sign: stopping";
//stop now
nextv = 0;
}
@@ -425,6 +427,7 @@ namespace armarx
{
//the area between soft and hard limits is sticky
//the controller can only move out of it (not further in)
+ ARMARX_INFO << deactivateSpam(1) << "Soft limit violation. " << softLimitViolation;
return 0;
}
@@ -451,7 +454,7 @@ namespace armarx
// 2. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
// the brakingDistance have the same sign and brakingDistance < e
// and currentVel <= maxV)
- // 3. we need to decellerate (other cases)
+ // 3. we need to decelerate (other cases)
//handle case 1
const float positionError = targetPosition - currentPosition;
@@ -564,13 +567,16 @@ namespace armarx
// 4. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
// and the brakingDistance have the same sign and brakingDistance < e
// and currentVel <= maxV)
- // 5. we need to decellerate (other cases)
+ // 5. we need to decelerate (other cases)
//handle case 1
const float vsquared = currentV * currentV;
const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
const float posIfBrakingNow = currentPosition + brakingDistance;
- if (posIfBrakingNow <= positionLimitLo || posIfBrakingNow >= positionLimitHi)
+ float direction = targetPosition < currentPosition ? -1 : 1;
+ // ARMARX_INFO << deactivateSpam(0.5) << VAROUT(direction) << VAROUT(brakingDistance) << VAROUT(currentPosition) << VAROUT(targetPosition);
+ if ((posIfBrakingNow <= positionLimitLo && direction < 1) ||
+ (posIfBrakingNow >= positionLimitHi && direction > 1))
{
//case 1. -> brake now! (we try to have v=0 at the limit)
const auto limit = brakingDistance > 0 ? positionLimitHi : positionLimitLo;
@@ -578,6 +584,7 @@ namespace armarx
// s = v²/(2a) <=> a = v²/(2s)
const float dec = std::abs(vsquared / 2.f / wayToGo);
const float vel = currentV - signV * dec * useddt;
+
return vel;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 12d6f5ef9..9a9225f89 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -82,7 +82,7 @@ namespace armarx
inline float angleDistance(float angle1, float angle2)
{
- return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI*2) - M_PI);
+ return M_PI - std::fabs(std::fmod(std::fabs(angle1 - angle2), M_PI * 2) - M_PI);
}
struct VelocityControllerWithAccelerationBounds
@@ -114,7 +114,7 @@ namespace armarx
* 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them. e.f. if we already violate them or the robot can't brake fast enough)
* 2. we directly set v and ignore acc/dec (if |currentV - targetV| <= directSetVLimit)
* 3. we need to accelerate (if currentV and targetV have same sign and |currentV| < |currentV|)
- * 4. we need to decellerate (other cases)
+ * 4. we need to decelerate (other cases)
* @param dt The time in seconds until the next call is made. (use the time since the last call as an approximate)
* @param maxDt Limits dt in case the given approximation has a sudden high value.
* @param currentV
@@ -161,8 +161,8 @@ namespace armarx
float deceleration;
float currentPosition;
float targetPosition;
-// float pControlPosErrorLimit;
-// float pControlVelLimit;
+ // float pControlPosErrorLimit;
+ // float pControlVelLimit;
float p;
bool validParameters() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index 4739e6fc1..440b92b34 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -289,7 +289,7 @@ struct Simulation
if (std::abs(oldvel) > std::abs(curvel))
{
// deceleration check with PID controller for motion-finish does not work -> check disabled
- // //we decellerated
+ // //we decelerated
// if (!(curacc < dec * 1.01))
// {
// std::cout << "Time[" << time << "] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel - curvel) << " / dt " << dt << " / dec " << dec
--
GitLab