From 91531ba7181ba2ac68858b1504e96fc6b5a8296f Mon Sep 17 00:00:00 2001
From: armarx-user <armarx-user@kit.edu>
Date: Mon, 4 Feb 2019 14:07:06 +0100
Subject: [PATCH] EtherCATRTUnit: Fixes for KIT Gripper
---
.../units/RobotUnit/BasicControllers.cpp | 32 +++++++++---
.../units/RobotUnit/BasicControllers.h | 6 +++
.../ArmarXEtherCAT/EtherCATRTUnit.cpp | 52 ++++++++++++++-----
.../libraries/ArmarXEtherCAT/EtherCATRTUnit.h | 2 +-
4 files changed, 71 insertions(+), 21 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
index 6666d1604..b9d64eb61 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -474,25 +474,36 @@ namespace armarx
const float posIfBrakingNow = currentPosition + brakingDistance;
const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
const bool hardBrakingNeeded = std::abs(brakingDistance) > std::abs(positionError);
+ const float safePositionError = (std::abs(positionError) < 0.0001) ? (sign(positionError) * 0.0001) : positionError;
const float usedDeceleration = hardBrakingNeeded ?
- std::abs(currentV * currentV / 2.f / math::MathUtils::LimitTo(positionError, 0.0001)) :
+ std::abs(currentV * currentV / 2.f / safePositionError) :
deceleration;
+
const bool decelerate =
std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
hardBrakingNeeded ||
sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+
const float usedacc = decelerate ? -usedDeceleration : acceleration;
const float deltaVel = signV * usedacc * useddt;
float newTargetVelRampCtrl = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
- bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
- || std::abs(newTargetVelPController) < pControlVelLimit
- || std::abs(positionError) < pControlPosErrorLimit;
- float finalTargetVel = usePID ? newTargetVelPController : newTargetVelRampCtrl;
+ currentlyPIDActive = std::abs(newTargetVelPController) < std::abs(newTargetVelRampCtrl)
+ || std::abs(newTargetVelPController) < pControlVelLimit
+ || std::abs(positionError) < pControlPosErrorLimit;
+ float finalTargetVel = (currentlyPIDActive && p > 0.0f) ? newTargetVelPController : newTargetVelRampCtrl;
if (std::abs(positionError) < accuracy)
{
- finalTargetVel = 0.0f; // if close to target set zero velocity to avoid oscillating around target
+ return 0.0;// if close to target set zero velocity to avoid oscillating around target
}
-
+ // if (hardBrakingNeeded)
+ // {
+ // ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "Hard braking! " << VAROUT(positionError) << VAROUT(brakingDistance) << VAROUT(finalTargetVel) << VAROUT(currentV);
+ // }
+ // if (decelerate)
+ // {
+ // ARMARX_INFO /*<< deactivateSpam(0.1)*/ << "Decelerating! " << VAROUT(targetPosition) << VAROUT(currentPosition) << VAROUT(positionError) << VAROUT(brakingDistance) << VAROUT(finalTargetVel) << VAROUT(currentV) <<
+ // VAROUT(usedacc) << VAROUT(deltaVel) << VAROUT(useddt);
+ // }
#ifdef DEBUG_POS_CTRL
buffer.push_back({currentPosition, newTargetVelPController, newTargetVelRampCtrl, currentV, positionError, IceUtil::Time::now().toMicroSeconds()});
@@ -605,7 +616,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;
-
+ // ARMARX_INFO << /*deactivateSpam(0.1) <<*/ "Braking now! " << VAROUT(vel) << VAROUT(wayToGo) << VAROUT(limit);
return vel;
}
@@ -762,6 +773,11 @@ namespace armarx
return sub.run();
}
+ bool PositionThroughVelocityControllerWithAccelerationBounds::getCurrentlyPIDActive() const
+ {
+ return currentlyPIDActive;
+ }
+
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index ff0592b4b..8e8e3204c 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -249,6 +249,12 @@ namespace armarx
mutable boost::circular_buffer<HelpStruct> buffer;
mutable int eventHappeningCounter = -1;
#endif
+
+ public:
+ bool getCurrentlyPIDActive() const;
+
+ private:
+ mutable bool currentlyPIDActive = false;
};
float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
float currentV, float maxV,
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index ce734f3e3..e0256e48c 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -55,6 +55,7 @@
#include <syscall.h>
#include <sys/stat.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
+#include <RobotAPI/libraries/core/Pose.h>
using namespace armarx;
@@ -79,14 +80,6 @@ EtherCATRTUnit::EtherCATRTUnit() :
}
-void stack_prefault(void)
-{
-
- unsigned char dummy[MAX_SAFE_STACK];
-
- memset(dummy, 0, MAX_SAFE_STACK);
- return;
-}
void EtherCATRTUnit::onInitRobotUnit()
@@ -95,7 +88,7 @@ void EtherCATRTUnit::onInitRobotUnit()
rtLoopTime = getProperty<int>("RTLoopFrequency").getValue();
- ARMARX_DEBUG << "Lock memory";
+ ARMARX_INFO << "Locking memory";
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
@@ -104,11 +97,11 @@ void EtherCATRTUnit::onInitRobotUnit()
}
ARMARX_DEBUG << "Pre-fault our stack";
- stack_prefault();
-
+ unsigned char dummy[MAX_SAFE_STACK];
+ memset(dummy, 0, MAX_SAFE_STACK);
ARMARX_INFO << "EtherCATRTUnit initializing now";
-
+ robot = rtGetRobot()->clone();
rtRobotJointSet = rtGetRobot()->getRobotNodeSet("RobotJoints");
rtRobotBodySet = rtGetRobot()->getRobotNodeSet("RobotCol");
for (auto& node : *rtRobotJointSet)
@@ -226,6 +219,41 @@ void EtherCATRTUnit::joinControlThread()
void EtherCATRTUnit::publish(StringVariantBaseMap debugObserverMap, StringVariantBaseMap timingMap)
{
RobotUnit::publish(std::move(debugObserverMap), std::move(timingMap));
+
+ for (auto& name : getSensorDeviceNames())
+ {
+ auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorPosition>();
+ if (!data)
+ {
+ continue;
+ }
+ robot->getRobotNode(name)->setJointValueNoUpdate(data->position);
+ }
+ robot->applyJointValues();
+ for (auto& name : getSensorDeviceNames())
+ {
+ auto data = getSensorDevice(name)->getSensorValue()->asA<SensorValue1DoFActuatorTorque>();
+ if (!data)
+ {
+ continue;
+ }
+ auto node = robot->getRobotNode(name);
+ ARMARX_CHECK_EXPRESSION(node);
+ auto joint = boost::dynamic_pointer_cast<VirtualRobot::RobotNodeRevolute>(node);
+ ARMARX_CHECK_EXPRESSION(joint);
+ Vector3Ptr pos = new Vector3(joint->getGlobalPosition());
+ Vector3Ptr axis = new Vector3(joint->getJointRotationAxis());
+ float percent = data->torque / 4.0f;
+ if (std::abs(percent) > 0.1)
+ {
+ getDebugDrawerProxy()->setCircleArrowVisu("TorqueEstimation", name, pos, axis, 100, percent, 5, DrawColor{0, 1, 0, 1});
+ }
+ else
+ {
+ getDebugDrawerProxy()->removeCircleVisu("TorqueEstimation", name);
+ }
+
+ }
}
armarx::PropertyDefinitionsPtr EtherCATRTUnit::createPropertyDefinitions()
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
index 58f06a145..faf3d505d 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
@@ -148,7 +148,7 @@ namespace armarx
IceUtil::Time currentPDOUpdateTimestamp;
-
+ VirtualRobot::RobotPtr robot;
DeviceContainerPtr deviceContainerPtr;
--
GitLab