From d2912bfa0c20a2d247657cd047e0c643210611a3 Mon Sep 17 00:00:00 2001
From: Armar6 <armar6@h2t.com>
Date: Thu, 13 Sep 2018 14:30:50 +0200
Subject: [PATCH] TCPControllerSubUnit: Use
NJointCartesianVelocityControllerWithRamp controler TCPControllerSubUnit:
Fixed isRequested()
---
...intCartesianVelocityControllerWithRamp.cpp | 5 +++
...JointCartesianVelocityControllerWithRamp.h | 1 +
.../RobotUnit/Units/TCPControllerSubUnit.cpp | 44 ++++++++++---------
.../RobotUnit/Units/TCPControllerSubUnit.h | 4 +-
4 files changed, 32 insertions(+), 22 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index bcd182e7e..1e0973a0c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -176,6 +176,11 @@ namespace armarx
}
+ const std::string& NJointCartesianVelocityControllerWithRamp::getNodeSetName() const
+ {
+ return nodeSetName;
+ }
+
WidgetDescription::StringWidgetDictionary NJointCartesianVelocityControllerWithRamp::getFunctionDescriptions(const Ice::Current&) const
{
return
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index ebc173247..90aa79247 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -143,6 +143,7 @@ namespace armarx
void setJointLimitAvoidanceScale(float jointLimitAvoidanceScale, const Ice::Current&) override;
void setKpJointLimitAvoidance(float KpJointLimitAvoidance, const Ice::Current&) override;
void immediateHardStop(const Ice::Current&) override;
+ const std::string& getNodeSetName() const;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
index 4c394cb29..cfe04c382 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -23,8 +23,10 @@
#include "TCPControllerSubUnit.h"
#include <RobotAPI/libraries/core/FramedPose.h>
-
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
+
using namespace armarx;
@@ -51,11 +53,12 @@ void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
{
- ARMARX_WARNING << "Setting cycle time in RT-Controller does not have an effect";
+ ARMARX_WARNING << deactivateSpam() << "Setting cycle time in RT-Controller does not have an effect";
}
void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c)
{
+ ScopedLock lock(dataMutex);
ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
std::string tcp;
if (tcpName.empty())
@@ -69,11 +72,11 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
- NJointCartesianVelocityControllerPtr tcpController;
+ NJointCartesianVelocityControllerWithRampPtr tcpController;
bool nodeSetAlreadyControlled = false;
for (NJointControllerPtr controller : activeNJointControllers)
{
- tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+ tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
if (!tcpController)
{
continue;
@@ -113,19 +116,19 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
}
- NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
+ CartesianSelectionMode::CartesianSelection mode = CartesianSelectionMode::eAll;
bool noMode = false;
if (globalTransVel && globalOriVel)
{
- mode = NJointCartesianVelocityControllerMode::eAll;
+ mode = CartesianSelectionMode::eAll;
}
else if (globalTransVel && !globalOriVel)
{
- mode = NJointCartesianVelocityControllerMode::ePosition;
+ mode = CartesianSelectionMode::ePosition;
}
else if (!globalTransVel && globalOriVel)
{
- mode = NJointCartesianVelocityControllerMode::eOrientation;
+ mode = CartesianSelectionMode::eOrientation;
}
else
{
@@ -136,16 +139,18 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
if (!nodeSetAlreadyControlled)
{
- NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
- NJointCartesianVelocityControllerPtr ctrl = NJointCartesianVelocityControllerPtr::dynamicCast(
+ NJointCartesianVelocityControllerWithRampConfigPtr config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, tcp, mode, 500, 1, 2, 1, 2);
+ // NJointCartesianVelocityControllerConfigPtr config = new NJointCartesianVelocityControllerConfig(nodeSetName, tcp, mode);
+ NJointCartesianVelocityControllerWithRampPtr ctrl = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(
robotUnit->createNJointController(
- "NJointCartesianVelocityController", controllerName,
+ "NJointCartesianVelocityControllerWithRamp", controllerName,
config, true, true
)
);
tcpController = ctrl;
- tcpController->setAvoidJointLimitsKp(getProperty<float>("AvoidJointLimitsKp").getValue());
+ tcpControllerNameMap[nodeSetName] = controllerName;
+ tcpController->setKpJointLimitAvoidance(getProperty<float>("AvoidJointLimitsKp").getValue(), c);
}
@@ -155,7 +160,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
{
ARMARX_CHECK_EXPRESSION(tcpController);
// ARMARX_CHECK_EXPRESSION(tcpController->getObjectScheduler());
- tcpController->setVelocities(xVel, yVel, zVel, rollVel, pitchVel, yawVel, NJointCartesianVelocityController::ModeFromIce(mode));
+ tcpController->setTargetVelocity(xVel, yVel, zVel, rollVel, pitchVel, yawVel, c);
// if (!tcpController->getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted, 5000))
// {
// ARMARX_ERROR << "NJointController was not initialized after 5000ms - bailing out!";
@@ -170,16 +175,15 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
bool TCPControllerSubUnit::isRequested(const Ice::Current&)
{
- auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
- for (NJointControllerPtr controller : activeNJointControllers)
+ ScopedLock lock(dataMutex);
+ for (auto& pair : tcpControllerNameMap)
{
- auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
- if (!tcpController)
+ auto ctrl = robotUnit->getNJointController(pair.second);
+ if (ctrl && ctrl->isControllerActive())
{
return true;
}
}
-
return false;
}
@@ -191,10 +195,10 @@ void armarx::TCPControllerSubUnit::componentPropertiesUpdated(const std::set<std
auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
for (NJointControllerPtr controller : activeNJointControllers)
{
- auto tcpController = NJointCartesianVelocityControllerPtr::dynamicCast(controller);
+ auto tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
if (tcpController)
{
- tcpController->setAvoidJointLimitsKp(avoidJointLimitsKp);
+ tcpController->setKpJointLimitAvoidance(avoidJointLimitsKp, GlobalIceCurrent);
}
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
index 2f45f0126..812578179 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -27,7 +27,6 @@
#include "RobotUnitSubUnit.h"
#include "../NJointControllers/NJointTCPController.h"
#include "../util.h"
-
namespace armarx
{
TYPEDEF_PTRS_HANDLE(RobotUnit);
@@ -58,9 +57,10 @@ namespace armarx
// RobotUnitSubUnit interface
void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
private:
- // mutable std::mutex dataMutex;
+ mutable Mutex dataMutex;
RobotUnit* robotUnit = nullptr;
VirtualRobot::RobotPtr coordinateTransformationRobot;
+ std::map<std::string, std::string> tcpControllerNameMap;
// ManagedIceObject interface
protected:
--
GitLab