From 9947325ef2f04bbc6eb76eed7e129f9432815230 Mon Sep 17 00:00:00 2001
From: Stefan Reither <stef.reither@web.de>
Date: Tue, 5 Dec 2017 14:56:29 +0100
Subject: [PATCH] adds possibility to show trajectory only as a preview to
TrajectoryControllerSubUnit
---
.../NJointTrajectoryController.cpp | 52 +++++++++-----
.../NJointTrajectoryController.h | 7 +-
.../Units/TrajectoryControllerSubUnit.cpp | 70 ++++++++++++++++++-
.../Units/TrajectoryControllerSubUnit.h | 5 ++
.../RobotUnit/NJointTrajectoryController.ice | 1 +
5 files changed, 113 insertions(+), 22 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index a98c0beb3..020b91bd6 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -9,22 +9,18 @@ namespace armarx
NJointTrajectoryController::NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
- // robot->getRobotNode("")->isLimitless();
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointTrajectoryControllerConfigPtr");
- // trajectory = TrajectoryPtr::dynamicCast(cfg->trajectory);
for (size_t i = 0; i < cfg->jointNames.size(); i++)
{
auto jointName = cfg->jointNames.at(i);
ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
const SensorValueBase* sv = prov->getSensorValue(jointName);
- targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
- sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
- PIDs.push_back(PIDControllerPtr(new PIDController(cfg->PID_p,
- cfg->PID_i, cfg->PID_d,
- cfg->maxVelocity)));
+ targets.insert(std::make_pair(jointName, ct->asA<ControlTarget1DoFActuatorVelocity>()));
+ sensors.insert(std::make_pair(jointName, sv->asA<SensorValue1DoFActuatorPosition>()));
+ // targets[jointName] = ct->asA<ControlTarget1DoFActuatorVelocity>();
+ // sensors[jointName] = sv->asA<SensorValue1DoFActuatorPosition>();
}
- currentPos.resize(cfg->jointNames.size());
}
std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
@@ -38,18 +34,27 @@ namespace armarx
{
TIMING_START(TrajectoryControl);
TrajectoryController& contr = *rtGetControlStruct().trajectory;
- int i = 0;
- for (auto& sv : sensors)
+
+ auto dimNames = contr.getTraj()->getDimensionNames();
+ for (size_t i = 0; i < dimNames.size(); i++)
{
- currentPos(i) = sv->position;
- i++;
+ const auto& jointName = dimNames.at(i);
+ currentPos(i) = (sensors.count(jointName) == 1) ? sensors[jointName]->position : 0.0f;
}
+
auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
bool finished = (contr.getCurrentTimestamp() >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
|| (contr.getCurrentTimestamp() <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
- for (size_t i = 0; i < targets.size(); ++i)
+ if (!cfg->isPreview)
{
- targets.at(i)->velocity = finished ? 0.0f : newVelocities(i);
+ for (size_t i = 0; i < dimNames.size(); ++i)
+ {
+ const auto& jointName = dimNames.at(i);
+ if (targets.count(jointName) == 1)
+ {
+ targets[jointName]->velocity = finished ? 0.0f : newVelocities(i);
+ }
+ }
}
if (finished && looping)
@@ -130,6 +135,7 @@ namespace armarx
}
trajEndTime = *trajectory->getTimestamps().rbegin() - *trajectory->getTimestamps().begin();
+ currentPos.resize(trajectory->getDimensionNames().size());
LockGuardType guard {controlDataMutex};
getWriterControlStruct().trajectory.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
@@ -162,6 +168,20 @@ namespace armarx
return trajEndTime;
}
+ TrajectoryPtr NJointTrajectoryController::getTrajectoryCopy() const
+ {
+ LockGuardType guard {controlDataMutex};
+ TrajectoryControllerPtr contr = rtGetControlStruct().trajectory;
+ if (contr)
+ {
+ return TrajectoryPtr::dynamicCast(contr->getTraj()->clone());
+ }
+ else
+ {
+ return new Trajectory();
+ }
+ }
+
void NJointTrajectoryController::rtPreActivateController()
{
startTime = IceUtil::Time();
@@ -169,9 +189,9 @@ namespace armarx
void NJointTrajectoryController::rtPostDeactivateController()
{
- for (size_t i = 0; i < targets.size(); ++i)
+ for (auto& target : targets)
{
- targets.at(i)->velocity = 0.0f;
+ target.second->velocity = 0.0f;
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index aec0b8035..20090d066 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -4,7 +4,6 @@
#include "NJointController.h"
#include <VirtualRobot/Robot.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/TrajectoryController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
@@ -41,12 +40,12 @@ namespace armarx
double getCurrentTrajTime() const;
void setLooping(bool looping);
double getTrajEndTime() const;
+ TrajectoryPtr getTrajectoryCopy() const;
private:
// TrajectoryPtr trajectory;
IceUtil::Time startTime;
- std::vector<PIDControllerPtr> PIDs;
- std::vector<ControlTarget1DoFActuatorVelocity*> targets;
- std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+ std::map<std::string, ControlTarget1DoFActuatorVelocity*> targets;
+ std::map<std::string, const SensorValue1DoFActuatorPosition*> sensors;
Eigen::VectorXf currentPos;
bool looping = false;
float direction = 1.0;
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
index 110aa4865..d62a7df31 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -35,11 +35,14 @@ void TrajectoryControllerSubUnit::onInitComponent()
ki = getProperty<float>("Ki").getValue();
kd = getProperty<float>("Kd").getValue();
maxVelocity = getProperty<float>("absMaximumVelocity").getValue() / 180 * M_PI; // config expects value in rad/sec
+
+ offeringTopic("DebugDrawerUpdates");
}
void TrajectoryControllerSubUnit::onConnectComponent()
{
kinematicUnit = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
+ debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
}
bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
@@ -48,7 +51,19 @@ bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
assureTrajectoryController();
jointTrajController->setTrajectory(this->jointTraj, c);
+ jointTraj = jointTrajController->getTrajectoryCopy();
jointTrajController->activateController();
+
+ if (isPreview)
+ {
+ ARMARX_INFO << "robot file name : " << kinematicUnit->getRobotFilename();
+ debugDrawer->setRobotVisu("Preview", "previewRobot", kinematicUnit->getRobotFilename(), boost::join(kinematicUnit->getArmarXPackages(), ","), armarx::CollisionModel);
+ debugDrawer->updateRobotColor("Preview", "previewRobot", DrawColor {0, 1, 0, 0.5});
+
+ previewTask = new PeriodicTask<TrajectoryControllerSubUnit>(this, &TrajectoryControllerSubUnit::previewRun, 20, false, "TrajectoryControllerSubUnitPreviewTask", false);
+ previewTask->start();
+ }
+
return true;
}
@@ -72,6 +87,12 @@ bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
{
if (controllerExists())
{
+ if (isPreview)
+ {
+ previewTask->stop();
+ debugDrawer->clearLayer("Preview");
+ }
+
jointTrajController->deactivateController();
while (jointTrajController->isControllerActive()) {}
jointTrajController->deleteController();
@@ -225,7 +246,18 @@ void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current
void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
{
- ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::setIsPreview()";
+ if (controllerExists() && jointTrajController->isControllerActive())
+ {
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting wheter the controller is only for preview.";
+ return;
+ }
+ this->isPreview = isPreview;
+
+ if (jointTraj)
+ {
+ jointTrajController = createTrajectoryController(usedJoints, false);
+ jointTrajController->setTrajectory(jointTraj, c);
+ }
}
bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, bool isInUse, const Ice::Current& c)
@@ -278,7 +310,7 @@ void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::
{
if (controllerExists() && jointTrajController->isControllerActive())
{
- ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting joints in use.";
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether constraints should be considered.";
return;
}
considerConstraintsForTrajectoryOptimization = consider;
@@ -316,6 +348,13 @@ NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryContr
config->maxVelocity = maxVelocity;
config->jointNames = jointNames;
config->considerConstraints = considerConstraintsForTrajectoryOptimization;
+ config->isPreview = isPreview;
+
+ ARMARX_IMPORTANT << config->PID_p << "\n"
+ << config->PID_i << "\n" << config->PID_i << "\n"
+ << config->maxVelocity << "\n" << config->jointNames << "\n"
+ << config->considerConstraints << "\n" << config->isPreview << "\n";
+
trajController = NJointTrajectoryControllerPtr::dynamicCast(
robotUnit->createNJointController(
"NJointTrajectoryController", controllerName,
@@ -355,6 +394,33 @@ bool TrajectoryControllerSubUnit::controllerExists()
return false;
}
+void TrajectoryControllerSubUnit::previewRun()
+{
+ if (controllerExists())
+ {
+ if (jointTrajController->isControllerActive())
+ {
+ ARMARX_DEBUG << "Preview ... ";
+
+ std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(jointTrajController->getCurrentTrajTime(), 1);
+ NameValueMap targetPositionValues;
+
+ auto dimNames = jointTraj->getDimensionNames();
+
+ for (size_t i = 0; i < dimNames.size(); i++)
+ {
+ const auto& jointName = dimNames.at(i);
+ if (std::find(usedJoints.begin(), usedJoints.end(), jointName) != usedJoints.end())
+ {
+ targetPositionValues[jointName] = states[i][0];
+ }
+ }
+
+ debugDrawer->updateRobotConfig("Preview", "previewRobot", targetPositionValues);
+ }
+ }
+}
+
void TrajectoryControllerSubUnit::setup(RobotUnit* rUnit)
{
ARMARX_CHECK_EXPRESSION(!robotUnit);
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
index 7b3ba80aa..71195a63e 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -129,6 +129,7 @@ namespace armarx
NJointTrajectoryControllerPtr createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
void assureTrajectoryController();
bool controllerExists();
+ void previewRun();
RobotUnit* robotUnit = NULL;
NJointTrajectoryControllerPtr jointTrajController;
@@ -155,6 +156,10 @@ namespace armarx
float maxVelocity;
bool considerConstraintsForTrajectoryOptimization = false;
+ bool isPreview = false;
+ DebugDrawerInterfacePrx debugDrawer;
+ PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
+
Mutex dataMutex;
};
}
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
index 570f51f8a..e4e299be9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
@@ -39,6 +39,7 @@ module armarx
float PID_i = 0;
float PID_d = 0;
bool considerConstraints = true;
+ bool isPreview = false;
};
interface NJointTrajectoryControllerInterface extends NJointControllerInterface
--
GitLab