diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 69f3f77896d198d4e02ed08ed05eac395e32d0a3..f85b0daedd4db697ff63a20713aed4fd112930be 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -28,6 +28,7 @@ set(LIB_FILES
RobotUnitObserver.cpp
BasicControllers.cpp
DefaultWidgetDescriptions.cpp
+ RobotSynchronization.cpp
ControlTargets/ControlTargetBase.cpp
@@ -38,6 +39,8 @@ set(LIB_FILES
Units/InertialMeasurementSubUnit.cpp
Units/KinematicSubUnit.cpp
Units/PlatformSubUnit.cpp
+ Units/TrajectoryControllerSubUnit.cpp
+ Units/TCPControllerSubUnit.cpp
JointControllers/JointController.cpp
@@ -71,6 +74,7 @@ set(LIB_HEADERS
RobotUnitObserver.h
BasicControllers.h
DefaultWidgetDescriptions.h
+ RobotSynchronization.h
ControlTargets/ControlTargetBase.h
ControlTargets/ControlTarget1DoFActuator.h
@@ -88,6 +92,8 @@ set(LIB_HEADERS
Units/InertialMeasurementSubUnit.h
Units/KinematicSubUnit.h
Units/PlatformSubUnit.h
+ Units/TrajectoryControllerSubUnit.h
+ Units/TCPControllerSubUnit.h
JointControllers/JointController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index 22e3bfaea68a5bd46cbe9f6e6c43754a1a97ae8d..d2df87384c5dac6572a462f8ab10b87e892b08c9 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -39,20 +39,38 @@ namespace armarx
void NJointTCPController::rtPreActivateController()
{
- xVel = 0;
- yVel = 0;
- zVel = 0;
- rollVel = 0;
- pitchVel = 0;
- yawVel = 0;
+ reinitTripleBuffer(NJointTCPControllerControlData());
}
void NJointTCPController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- Eigen::VectorXf x(6);
- x << this->xVel, yVel, zVel, rollVel, pitchVel, yawVel;
- Eigen::MatrixXf jacobiInv = ik->getPseudoInverseJacobianMatrix(tcp);
+ auto mode = rtGetControlStruct().mode;
+
+ Eigen::VectorXf x;
+ if (mode == VirtualRobot::IKSolver::All)
+ {
+ x.resize(6);
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel, rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ }
+ else if (mode == VirtualRobot::IKSolver::Position)
+ {
+ x.resize(3);
+ x << rtGetControlStruct().xVel, rtGetControlStruct().yVel, rtGetControlStruct().zVel;
+ }
+ else if (mode == VirtualRobot::IKSolver::Orientation)
+ {
+ x.resize(3);
+ x << rtGetControlStruct().rollVel, rtGetControlStruct().pitchVel, rtGetControlStruct().yawVel;
+ }
+ else
+ {
+ // No mode has been set
+ return;
+ }
+
+ Eigen::MatrixXf jacobiInv = ik->getPseudoInverseJacobianMatrix(tcp, mode);
Eigen::VectorXf jointTargetVelocities = jacobiInv * x;
+
for (size_t i = 0; i < targets.size(); ++i)
{
targets.at(i)->velocity = jointTargetVelocities(i);
@@ -155,6 +173,26 @@ namespace armarx
ik.reset(new VirtualRobot::DifferentialIK(nodeset, robotUnit->getRtRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : robotUnit->getRtRobot()->getRobotNode(config->tcpName);
ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName);
+
+ nodeSetName = config->nodeSetName;
+ }
+
+ void NJointTCPController::setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().xVel = xVel;
+ getWriterControlStruct().yVel = yVel;
+ getWriterControlStruct().zVel = zVel;
+ getWriterControlStruct().rollVel = rollVel;
+ getWriterControlStruct().pitchVel = pitchVel;
+ getWriterControlStruct().yawVel = yawVel;
+ getWriterControlStruct().mode = mode;
+ writeControlStruct();
+ }
+
+ std::string NJointTCPController::getNodeSetName() const
+ {
+ return nodeSetName;
}
void NJointTCPController::rtPostDeactivateController()
@@ -177,21 +215,20 @@ namespace armarx
{
if (name == "ControllerTarget")
{
- xVel = valueMap.at("x")->getFloat();
- yVel = valueMap.at("y")->getFloat();
- zVel = valueMap.at("z")->getFloat();
- rollVel = valueMap.at("roll")->getFloat();
- pitchVel = valueMap.at("pitch")->getFloat();
- yawVel = valueMap.at("yaw")->getFloat();
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().xVel = valueMap.at("x")->getFloat();
+ getWriterControlStruct().yVel = valueMap.at("y")->getFloat();
+ getWriterControlStruct().zVel = valueMap.at("z")->getFloat();
+ getWriterControlStruct().rollVel = valueMap.at("roll")->getFloat();
+ getWriterControlStruct().pitchVel = valueMap.at("pitch")->getFloat();
+ getWriterControlStruct().yawVel = valueMap.at("yaw")->getFloat();
+ writeControlStruct();
}
else
{
ARMARX_WARNING << "Unknown function name called: " << name;
}
}
-
-
-
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index e5520daa7ab750e834f17b6ff37e1c1047b7ad37..46eff8bfb2003a4ed9216c86a57c23faaf7a2367 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -26,8 +26,9 @@
#include "NJointController.h"
#include <VirtualRobot/Robot.h>
-#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include "../RobotUnit.h"
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
#include <VirtualRobot/IK/DifferentialIK.h>
namespace armarx
@@ -48,8 +49,22 @@ namespace armarx
const std::string tcpName;
};
+ TYPEDEF_PTRS_HANDLE(NJointTCPControllerControlData);
+ class NJointTCPControllerControlData
+ {
+ public:
+ float xVel = 0;
+ float yVel = 0;
+ float zVel = 0;
+ float rollVel = 0;
+ float pitchVel = 0;
+ float yawVel = 0;
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+ };
- class NJointTCPController : public NJointController
+
+ class NJointTCPController :
+ public NJointControllerWithTripleBuffer<NJointTCPControllerControlData>
{
public:
using ConfigPtrT = NJointTCPControllerConfigPtr;
@@ -72,6 +87,10 @@ namespace armarx
NJointControllerDescriptionProviderInterfacePtr prov,
NJointTCPControllerConfigPtr config,
const boost::shared_ptr<VirtualRobot::Robot>& r);
+
+ // for TCPControlUnit
+ void setVelocities(float xVel, float yVel, float zVel, float rollVel, float pitchVel, float yawVel, VirtualRobot::IKSolver::CartesianSelection mode);
+ std::string getNodeSetName() const;
static ::armarx::WidgetDescription::WidgetSeq createSliders();
protected:
void rtPreActivateController() override;
@@ -80,19 +99,10 @@ namespace armarx
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
std::vector<const SensorValue1DoFActuatorPosition*> sensors;
- std::atomic<float> xVel;
- std::atomic<float> yVel;
- std::atomic<float> zVel;
- std::atomic<float> rollVel;
- std::atomic<float> pitchVel;
- std::atomic<float> yawVel;
VirtualRobot::RobotNodePtr tcp;
VirtualRobot::DifferentialIKPtr ik;
-
-
-
-
+ std::string nodeSetName;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 2efa4a636ac1370101e116142b1cf16847eddd68..34ad724d12f015b5064e4476602998aa3b07647e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -6,24 +6,18 @@ namespace armarx
{
NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
- NJointTrajectoryController::NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ NJointTrajectoryController::NJointTrajectoryController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointTrajectoryControllerConfigPtr");
- // trajectory = TrajectoryPtr::dynamicCast(cfg->trajectory);
- for (size_t i = 0; i < cfg->jointNames.size(); i++)
+ for (std::string jointName : cfg->jointNames)
{
- 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>()));
}
- currentPos.resize(cfg->jointNames.size());
}
std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
@@ -33,22 +27,35 @@ namespace armarx
void NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- TIMING_START(TrajectoryControl);
- TrajectoryController& contr = *rtGetControlStruct().trajectory;
- int i = 0;
- for (auto& sv : sensors)
+ if (rtGetControlStruct().trajectory)
{
- currentPos(i) = sv->position;
- i++;
- }
- auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble(), currentPos);
- bool finished = contr.getCurrentTimestamp() >= contr.getTraj()->rbegin()->getTimestamp();
- for (size_t i = 0; i < targets.size(); ++i)
- {
- targets.at(i)->velocity = finished ? 0.0f : newVelocities(i);
- }
- TIMING_END(TrajectoryControl);
+ TrajectoryController& contr = *rtGetControlStruct().trajectory;
+
+ auto dimNames = contr.getTraj()->getDimensionNames();
+ for (size_t i = 0; i < dimNames.size(); 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 < dimNames.size(); ++i)
+ {
+ const auto& jointName = dimNames.at(i);
+ if (targets.count(jointName) == 1)
+ {
+ targets[jointName]->velocity = (cfg->isPreview || finished) ? 0.0f : newVelocities(i);
+ }
+ }
+
+ if (finished && looping)
+ {
+ direction *= -1.0;
+ }
+ }
}
void NJointTrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
@@ -59,63 +66,112 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(trajectory);
size_t trajectoryDimensions = trajectory->dim();
ARMARX_CHECK_EXPRESSION(trajectoryDimensions == targets.size());
- std::list<Eigen::VectorXd> pathPoints;
- float timeStep = cfg->preSamplingStepMs / 1000;
- Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
- Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
- for (const Trajectory::TrajData& element : *trajectory)
+ if (cfg->considerConstraints)
{
- Eigen::VectorXd waypoint(trajectoryDimensions);
- for (size_t i = 0; i < trajectoryDimensions; ++i)
+ std::list<Eigen::VectorXd> pathPoints;
+ float timeStep = cfg->preSamplingStepMs / 1000;
+ Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
+ Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
+ for (const Trajectory::TrajData& element : *trajectory)
{
- waypoint(i) = element.getPosition(i);
+ Eigen::VectorXd waypoint(trajectoryDimensions);
+ for (size_t i = 0; i < trajectoryDimensions; ++i)
+ {
+ waypoint(i) = element.getPosition(i);
+ }
+ pathPoints.emplace_back(waypoint);
}
- pathPoints.emplace_back(waypoint);
- }
-
- VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
- VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(p, maxVelocities, maxAccelerations);
+ VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(p, maxVelocities, maxAccelerations);
- TrajectoryPtr newTraj = new Trajectory();
- Ice::DoubleSeq newTimestamps;
- double duration = timeOptimalTraj.getDuration();
- for (double t = 0.0; t < duration; t += timeStep)
- {
- newTimestamps.push_back(t);
- }
- newTimestamps.push_back(duration);
+ TrajectoryPtr newTraj = new Trajectory();
- for (size_t d = 0; d < trajectoryDimensions; d++)
- {
- // Ice::DoubleSeq position;
- // for (double t = 0.0; t < duration; t += timeStep)
- // {
- // position.push_back(timeOptimalTraj.getPosition(t)[d]);
- // }
- // position.push_back(timeOptimalTraj.getPosition(duration)[d]);
- // newTraj->addDimension(position, newTimestamps, trajectory->getDimensionName(d));
-
- Ice::DoubleSeq derivs;
+ Ice::DoubleSeq newTimestamps;
+ double duration = timeOptimalTraj.getDuration();
for (double t = 0.0; t < duration; t += timeStep)
{
+ newTimestamps.push_back(t);
+ }
+ newTimestamps.push_back(duration);
+
+ for (size_t d = 0; d < trajectoryDimensions; d++)
+ {
+ // Ice::DoubleSeq position;
+ // for (double t = 0.0; t < duration; t += timeStep)
+ // {
+ // position.push_back(timeOptimalTraj.getPosition(t)[d]);
+ // }
+ // position.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ // newTraj->addDimension(position, newTimestamps, trajectory->getDimensionName(d));
+
+ Ice::DoubleSeq derivs;
+ for (double t = 0.0; t < duration; t += timeStep)
+ {
+ derivs.clear();
+ derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
+ newTraj->addDerivationsToDimension(d, t, derivs);
+ }
derivs.clear();
- derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
- derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
- newTraj->addDerivationsToDimension(d, t, derivs);
+ derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
+ newTraj->addDerivationsToDimension(d, duration, derivs);
}
- derivs.clear();
- derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
- derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
- newTraj->addDerivationsToDimension(d, duration, derivs);
+ newTraj->setDimensionNames(trajectory->getDimensionNames());
+ TIMING_END(TrajectoryOptimization);
+ ARMARX_INFO << "Trajectory duration: " << newTraj->getTimeLength();
+ ARMARX_INFO << VAROUT(newTraj->output());
+
+ trajectory = newTraj;
}
- newTraj->setDimensionNames(trajectory->getDimensionNames());
- TIMING_END(TrajectoryOptimization);
- ARMARX_INFO << "Trajectory duration: " << newTraj->getTimeLength();
+
+ trajEndTime = *trajectory->getTimestamps().rbegin() - *trajectory->getTimestamps().begin();
+ currentPos.resize(trajectory->getDimensionNames().size());
+
LockGuardType guard {controlDataMutex};
- getWriterControlStruct().trajectory.reset(new TrajectoryController(newTraj, cfg->PID_p, cfg->PID_i, cfg->PID_d));
+ getWriterControlStruct().trajectory.reset(new TrajectoryController(trajectory, cfg->PID_p, cfg->PID_i, cfg->PID_d, false));
writeControlStruct();
+ rtUpdateControlStruct();
+ }
+
+ double NJointTrajectoryController::getCurrentTrajTime() const
+ {
+ LockGuardType guard {controlDataMutex};
+ TrajectoryControllerPtr contr = rtGetControlStruct().trajectory;
+ if (contr)
+ {
+ return contr->getCurrentTimestamp();
+ }
+ else
+ {
+ return 0.0;
+ }
+ }
+
+ void NJointTrajectoryController::setLooping(bool looping)
+ {
+ this->looping = looping;
+ }
+
+ double NJointTrajectoryController::getTrajEndTime() const
+ {
+ 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()
@@ -123,4 +179,12 @@ namespace armarx
startTime = IceUtil::Time();
}
+ void NJointTrajectoryController::rtPostDeactivateController()
+ {
+ for (auto& target : targets)
+ {
+ 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 5bde47d194952216ea67decd9881d2c0c7ae8122..20090d066cc467306292a69f77db9c695b7c5187 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>
@@ -12,6 +11,8 @@
namespace armarx
{
+ TYPEDEF_PTRS_HANDLE(NJointTrajectoryController);
+
class NJointTrajectoryControllerControlData
{
public:
@@ -30,17 +31,25 @@ namespace armarx
// NJointController interface
void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
// NJointTrajectoryControllerInterface interface
void setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&) override;
+
+ 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;
+ double trajEndTime;
NJointTrajectoryControllerConfigPtr cfg;
virtual void onInitComponent() override {}
virtual void onConnectComponent() override {}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5f8857e1a34184fd3617fdd97065299aa96a6728
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.cpp
@@ -0,0 +1,78 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::RobotUnit::RobotSynchronization
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotSynchronization.h"
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include "SensorValues/SensorValue1DoFActuator.h"
+
+#include "RobotUnit.h"
+
+using namespace armarx;
+
+RobotSynchronization::RobotSynchronization(RobotUnit* robotUnit, VirtualRobot::RobotPtr& robot, const std::vector<std::string>& synchronizingJoints)
+{
+ this->robotUnit = robotUnit;
+ this->robot = robot;
+ Ice::StringSeq names = robotUnit->getSensorDeviceNames(GlobalIceCurrent);
+
+ for (std::string name : names)
+ {
+ // skip, if the current sensor device does not correspond to a joint.
+ if (!this->robot->hasRobotNode(name))
+ {
+ continue;
+ }
+
+ ConstSensorDevicePtr device = robotUnit->getSensorDevice(name);
+
+ // skip, if the sensor value of the current sensor device can't provide a position.
+ if (!device->getSensorValue()->isA<SensorValue1DoFActuatorPosition>())
+ {
+ continue;
+ }
+
+ std::vector<std::string>::const_iterator it = std::find(synchronizingJoints.begin(), synchronizingJoints.end(), name);
+ if (it == synchronizingJoints.end())
+ {
+ continue;
+ }
+ else
+ {
+ jointSensorDeviceMap[*it] = device;
+ }
+ }
+}
+
+void RobotSynchronization::sync()
+{
+ if (robotUnit->getRobotUnitState() == RobotUnitState::InitializingUnits || robotUnit->getRobotUnitState() == RobotUnitState::Running)
+ {
+ for (auto const& entry : jointSensorDeviceMap)
+ {
+ const SensorValueBase* sv = entry.second->getSensorValue();
+
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
+ robot->getRobotNode(entry.first)->setJointValueNoUpdate(sv->asA<SensorValue1DoFActuatorPosition>()->position);
+ }
+ robot->applyJointValues();
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e0aa926b15d58bdfcb4ad72ab57a7a228b144b4
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotSynchronization.h
@@ -0,0 +1,54 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::RobotUnit::RobotSynchronization
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_UNIT_RobotAPI_RobotSynchronization_H
+#define _ARMARX_UNIT_RobotAPI_RobotSynchronization_H
+
+#include <VirtualRobot/Robot.h>
+#include "Devices/SensorDevice.h"
+
+#include <string>
+#include <vector>
+
+namespace armarx
+{
+ class RobotUnit;
+ class SensorValue1DoFActuatorPosition;
+
+ TYPEDEF_PTRS_SHARED(RobotSynchronization);
+ /**
+ * @brief The RobotSynchronization class
+ */
+ class RobotSynchronization
+ {
+ public:
+ RobotSynchronization(RobotUnit* robotUnit, VirtualRobot::RobotPtr& robot, const std::vector<std::string>& synchronizingJoints);
+ ~RobotSynchronization() {}
+ void sync();
+
+ private:
+ std::map<std::string, ConstSensorDevicePtr> jointSensorDeviceMap;
+ RobotUnit* robotUnit = NULL;
+ VirtualRobot::RobotPtr robot;
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 89a5b3aa6b4488af3a998e1f9209de410df41fda..20f9a6420ce70d79cc65496968e6469bb195ea71 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -44,6 +44,8 @@
#include "Units/KinematicSubUnit.h"
#include "Units/PlatformSubUnit.h"
#include "SensorValues/SensorValue1DoFActuator.h"
+#include "Units/TCPControllerSubUnit.h"
+#include "Units/TrajectoryControllerSubUnit.h"
namespace std
{
@@ -77,7 +79,7 @@ namespace armarx
public:
RobotUnitEmergencyStopMaster(RobotUnit* robotUnit, std::string emergencyStopTopicName) :
robotUnit {robotUnit},
- emergencyStopTopicName {emergencyStopTopicName}
+ emergencyStopTopicName {emergencyStopTopicName}
{}
virtual void setEmergencyStopState(EmergencyStopState state, const Ice::Current& = GlobalIceCurrent) final
@@ -396,6 +398,10 @@ namespace armarx
ARMARX_DEBUG << "ForceTorqueUnit initialized";
initializeInertialMeasurementUnit();
ARMARX_DEBUG << "InertialMeasurementUnit initialized";
+ initializeTrajectoryControllerUnit();
+ ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
+ initializeTcpControllerUnit();
+ ARMARX_DEBUG << "TcpControllerUnit initialized";
}
ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
}
@@ -448,7 +454,7 @@ namespace armarx
}
initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
- initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF, ControlTarget1DoFActuatorTorque, ad.ctrlTor)
+ initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF , ControlTarget1DoFActuatorTorque , ad.ctrlTor)
#undef initializeKinematicUnit_tmp_helper
@@ -466,8 +472,7 @@ namespace armarx
ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
//add it
const std::string configName = getProperty<std::string>("KinematicUnitName");
- const std::string& configDomain = "ArmarX";
- const std::string confPre = configDomain + "." + configName + ".";
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
@@ -475,7 +480,7 @@ namespace armarx
properties->setProperty(confPre + "RobotFileName", robotFileName);
properties->setProperty(confPre + "RobotFileNameProject", robotProjectName);
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, configDomain);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
//fill devices (sensor + controller)
unit->setupData(
@@ -521,14 +526,13 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
//add it
const std::string configName = getProperty<std::string>("PlatformUnitName");
- const std::string& configDomain = "ArmarX";
- const std::string confPre = configDomain + "." + configName + ".";
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "PlatformName", robotPlatformName);
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, configDomain);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, getConfigDomain());
//config
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
config->initialVelocityX = 0;
@@ -591,20 +595,48 @@ namespace armarx
}
//add it
const std::string configName = getProperty<std::string>("ForceTorqueUnitName");
- const std::string& configDomain = "ArmarX";
- const std::string confPre = configDomain + "." + configName + ".";
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "AgentName", robot->getName());
properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, configDomain);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = ftdevs;
//add
addUnit(std::move(unit));
}
+ void RobotUnit::initializeTrajectoryControllerUnit()
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ using UnitT = TrajectoryControllerSubUnit;
+
+ //check if unit is already added
+ if (getUnit(UnitT::ice_staticId()))
+ {
+ return;
+ }
+ (TrajectoryControllerSubUnitPtr::dynamicCast(trajectoryControllerSubUnit))->setup(this);
+ addUnit(trajectoryControllerSubUnit);
+ }
+
+ void RobotUnit::initializeTcpControllerUnit()
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ using UnitT = TCPControllerSubUnit;
+
+ //check if unit is already added
+ if (getUnit(UnitT::ice_staticId()))
+ {
+ return;
+ }
+ (TCPControllerSubUnitPtr::dynamicCast(tcpControllerSubUnit))->setup(this, robot->clone());
+ addUnit(tcpControllerSubUnit);
+ }
void armarx::RobotUnit::initializeInertialMeasurementUnit()
{
using UnitT = InertialMeasurementSubUnit;
@@ -634,13 +666,12 @@ namespace armarx
}
//add it
const std::string configName = getProperty<std::string>("InertialMeasurementUnitName");
- const std::string& configDomain = "ArmarX";
- const std::string confPre = configDomain + "." + configName + ".";
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
//properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
//fill properties
properties->setProperty(confPre + "IMUTopicName", getProperty<std::string>("IMUTopicName").getValue());
- IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, configDomain);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
unit->devs = imudevs;
//add
addUnit(std::move(unit));
@@ -681,6 +712,7 @@ namespace armarx
std::size_t rtLoggingTimestep = getProperty<std::size_t>("RTLoggingPeriodMs");
rtLogging.enabled = rtLoggingTimestep;
+ rtLogging.loggingTaskTimestep = rtLoggingTimestep;
ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
if (isRtLoggingEnabled())
{
@@ -750,13 +782,7 @@ namespace armarx
}
}
}
- //start logging thread
- {
- rtLogging.task = new RTLoggingTaskT([&] {rtLogging.doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
- ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestep;
- rtLogging.task->setDelayWarningTolerance(rtLoggingTimestep * 10);
- rtLogging.task->start();
- }
+ //start logging thread is done in rtinit
//maybe add the default log
{
const auto loggingpath = getProperty<std::string>("RTLoggingDefaultLog").getValue();
@@ -777,6 +803,14 @@ namespace armarx
void armarx::RobotUnit::initRTThread()
{
+ startSelfCollisionAvoidance();
+ if (isRtLoggingEnabled())
+ {
+ rtLogging.task = new RTLoggingTaskT([&] {rtLogging.doLogging();}, rtLogging.loggingTaskTimestep, false, getName() + "_RTLoggingTask");
+ ARMARX_INFO << "starting rt logging with timestep " << rtLogging.loggingTaskTimestep;
+ rtLogging.task->setDelayWarningTolerance(rtLogging.loggingTaskTimestep * 10);
+ rtLogging.task->start();
+ }
auto guard = getGuard();
throwIfStateIsNot(RobotUnitState::WaitingForRTThreadInitialization, __FUNCTION__);
rtThreadId = std::this_thread::get_id();
@@ -1424,6 +1458,7 @@ namespace armarx
return;
}
auto guard = getGuard();
+
throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
//get batch proxies
@@ -1484,8 +1519,8 @@ namespace armarx
},
timestamp
};
-
//publish sensor updates
+
timingMap["publishTimings_SensorUpdates"] = new TimedVariant
{
ARMARX_STOPWATCH(TimestampVariant)
@@ -1639,6 +1674,7 @@ namespace armarx
robotUnitObs->updateChannel(robotUnitObs->controlDevicesChannel);
robotUnitObs->offerOrUpdateDataFieldsFlatCopy(robotUnitObs->sensorDevicesChannel, sensorDevMap);
robotUnitObs->updateChannel(robotUnitObs->sensorDevicesChannel);
+
}
debugObserverBatchPrx->ice_flushBatchRequests();
};
@@ -2357,6 +2393,18 @@ namespace armarx
{
robotUnitObs = Component::create<RobotUnitObserver>(getIceProperties(), "", getConfigDomain());
addPropertyUser(PropertyUserPtr::dynamicCast(robotUnitObs));
+
+ // create TCPControlSubUnit
+ const std::string configNameTCPControlUnit = getProperty<std::string>("TCPControlUnitName");
+ const std::string& configDomain = "ArmarX";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ tcpControllerSubUnit = Component::create<TCPControllerSubUnit>(properties, configNameTCPControlUnit, configDomain);
+
+ // create TrajectoryControllerSubUnit
+ const std::string configNameTrajectoryControllerUnit = getProperty<std::string>("TrajectoryControllerUnitName");
+ const std::string confPre = configDomain + "." + configNameTrajectoryControllerUnit + ".";
+ properties->setProperty(confPre + "KinematicUnitName", getProperty<std::string>("KinematicUnitName").getValue());
+ trajectoryControllerSubUnit = Component::create<TrajectoryControllerSubUnit>(properties, configNameTrajectoryControllerUnit, configDomain);
}
@@ -2507,12 +2555,12 @@ namespace armarx
std::lock_guard<std::mutex> guard {rtLogging.mutex};
FileSystemPathBuilder pb {formatString};
pb.createParentDirectories();
- std::ofstream outCSV{pb.getPath() + ".csv"};
+ std::ofstream outCSV {pb.getPath() + ".csv"};
if (!outCSV)
{
throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".csv'"};
}
- std::ofstream outMsg{pb.getPath() + ".messages"};
+ std::ofstream outMsg {pb.getPath() + ".messages"};
if (!outMsg)
{
throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".messages'"};
@@ -2678,6 +2726,9 @@ namespace armarx
selfCollisionAvoidanceData.collisionModels = colModels;
+ // Setup robot synchronization
+ this->selfCollisionAvoidanceRobotSynchronization.reset(new RobotSynchronization(this, selfCollisionAvoidanceData.robot, selfCollisionAvoidanceData.robot->getRobotNodeNames()));
+
// start the task
int colChecksPerSecond = getProperty<int>("SelfCollisionChecksPerSecond").getValue();
int intervalMs = (1.0 / (float) colChecksPerSecond) * 1000;
@@ -2721,35 +2772,10 @@ namespace armarx
}
auto startTime = std::chrono::high_resolution_clock::now();
- sensorAndControlBufferChanged(); // update the sensor read buffer
- const SensorAndControl& sc = getSensorAndControlBuffer();
- // set joint values
- {
- auto guard = getGuard();
- for (std::size_t i = 0; i < sc.sensors.size(); i++)
- {
- const auto& sensorValueBase = sc.sensors.at(i);
- // skip, if the current sensor device does not correspond to a joint.
- std::string deviceName = sensorDevices.at(i)->getDeviceName();
- if (!selfCollisionAvoidanceData.robot->hasRobotNode(deviceName))
- {
- continue;
- }
- // skip, if the sensor value of the current sensor device can't provide a position.
- if (!sensorValueBase->isA<SensorValue1DoFActuatorPosition>())
- {
- continue;
- }
- const SensorValue1DoFActuatorPosition* sv = sensorValueBase->asA<SensorValue1DoFActuatorPosition>();
- if (sv)
- {
- float position = sv->position;
- // set the joint value
- selfCollisionAvoidanceData.robot->getRobotNode(deviceName)->setJointValueNoUpdate(position);
- }
- }
- }
- selfCollisionAvoidanceData.robot->applyJointValues();
+
+ // update joint values
+ selfCollisionAvoidanceRobotSynchronization->sync();
+
// check distances between all collision models
std::vector<VirtualRobot::SceneObjectSetPtr> conflictingColModels;
std::string distancesString("All collision states:\n");
@@ -2960,7 +2986,6 @@ namespace armarx
debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
onConnectRobotUnit();
- startSelfCollisionAvoidance();
ARMARX_DEBUG << "RobotUnit::onConnectComponent()...done!";
}
@@ -2968,6 +2993,9 @@ namespace armarx
{
ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()";
stopSelfCollisionAvoidance();
+
+ // Disconnecting TCPController and TrajectoryController
+
onDisconnectRobotUnit();
ARMARX_DEBUG << "RobotUnit::onDisconnectComponent()...done!";
}
@@ -3185,122 +3213,123 @@ namespace armarx
o << std::to_string(s);
return o;
}
+}
- void RobotUnit::RtLogging::doLogging()
+void armarx::RobotUnit::RtLogging::doLogging()
+{
+ ARMARX_CHECK_EXPRESSION(enabled);
+ std::lock_guard<std::mutex> guard {mutex};
+ const auto now = IceUtil::Time::now();
+ //remove entries
{
- ARMARX_CHECK_EXPRESSION(enabled);
- std::lock_guard<std::mutex> guard {mutex};
- const auto now = IceUtil::Time::now();
- //remove entries
+ std::vector<std::string> toRemove;
+ toRemove.reserve(entries.size());
+ for (auto& pair : entries)
{
- std::vector<std::string> toRemove;
- toRemove.reserve(entries.size());
- for (auto& pair : entries)
- {
- if (pair.second->stopLogging)
- {
- //can't remove the current elemet
- //(this would invalidate the current iterator)
- toRemove.emplace_back(pair.first);
- }
- }
- for (const auto& rem : toRemove)
+ if (pair.second->stopLogging)
{
- entries.erase(rem);
+ //can't remove the current elemet
+ //(this would invalidate the current iterator)
+ toRemove.emplace_back(pair.first);
}
}
- //remove backlog entries
+ for (const auto& rem : toRemove)
{
- while (!backlog.empty() && backlog.front().writeTimestamp + keepBacklogFor < now)
- {
- backlog.pop_front();
- }
+ entries.erase(rem);
}
- //log all
+ }
+ //remove backlog entries
+ {
+ while (!backlog.empty() && backlog.front().writeTimestamp + keepBacklogFor < now)
{
- ru->controlThreadOutputBuffer.foreachNewLoggingEntry([&](const ControlThreadOutputBuffer::Entry & data)
+ backlog.pop_front();
+ }
+ }
+ //log all
+ {
+ ru->controlThreadOutputBuffer.foreachNewLoggingEntry([&](const ControlThreadOutputBuffer::Entry & data)
+ {
+ //base (marker;iteration;timestamp)
{
- //base (marker;iteration;timestamp)
+ for (auto& pair : entries)
{
- for (auto& pair : entries)
- {
- Entry& e = *pair.second;
- e.stream << "\n"
- << e.marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds();
- e.marker.clear();
- }
+ Entry& e = *pair.second;
+ e.stream << "\n"
+ << e.marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds();
+ e.marker.clear();
}
- //sens
+ }
+ //sens
+ {
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
{
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const SensorValueBase* val = data.sensors.at(idxDev);
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& pair : entries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : entries)
+ Entry& e = *pair.second;
+ if (e.loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- Entry& e = *pair.second;
- if (e.loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- pair.second->stream << ";" << str;
- }
+ pair.second->stream << ";" << str;
}
}
}
}
- //ctrl
+ }
+ //ctrl
+ {
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
{
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ const auto& vals = data.control.at(idxDev);
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- const auto& vals = data.control.at(idxDev);
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ const ControlTargetBase* val = vals.at(idxVal);
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- const ControlTargetBase* val = vals.at(idxVal);
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ const auto str = val->getDataFieldAsString(idxField);
+ for (auto& pair : entries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& pair : entries)
+ Entry& e = *pair.second;
+ if (e.loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- Entry& e = *pair.second;
- if (e.loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- pair.second->stream << ";" << str;
- }
+ pair.second->stream << ";" << str;
}
}
}
}
}
- //store data to backlog
+ }
+ //store data to backlog
+ {
+ if (data.writeTimestamp + keepBacklogFor >= now)
{
- if (data.writeTimestamp + keepBacklogFor >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
+ backlog.emplace_back(data, true); //true for minimal copy
}
- //print + reset messages
+ }
+ //print + reset messages
+ {
+ for (const detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- for (const detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ if (!ptr)
{
- if (!ptr)
- {
- break;
- }
- ptr->print(rtThreadId);
+ break;
}
+ ptr->print(rtThreadId);
}
- });
- }
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
- //flush all
- {
- for (auto& pair : entries)
- {
- pair.second->stream << std::flush;
}
+ });
+ }
+ ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ //flush all
+ {
+ for (auto& pair : entries)
+ {
+ pair.second->stream << std::flush;
}
}
}
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index 0aa65711e6b3d5dd7b3470ba942b270eed7dbf87..d322d8857960ff0808aa10ea19fa2f4cefee9cdd 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -56,6 +56,7 @@
#include "util.h"
#include "util/JointAndNJointControllers.h"
#include "RobotUnitObserver.h"
+#include "RobotSynchronization.h"
namespace armarx
{
@@ -117,6 +118,7 @@ namespace armarx
defineOptionalProperty<std::string>(
"EmergencyStopMasterName", "EmergencyStopMaster",
"The name used to register as an EmergencyStopMaster");
+
defineOptionalProperty<std::string>(
"EmergencyStopTopic", "EmergencyStop",
"The name of the topic over which changes of the emergencyStopState are sent.");
@@ -221,8 +223,14 @@ namespace armarx
"Comma seperated list of pairs of collision models that should be checked against each other by collision avoidance \n"
"(e.g. {rnsColModel1,rnsColModel2};{rnsColModel3,rnsColModel4}; .... "
"says that rnsColModel1 will be only checked against rnsColModel2 and rnsColModel3 will only be checked against rnsColModel4). \n"
- "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.",
- PropertyDefinitionBase::eConstant);
+ "Following predefined descriptors can be used: 'FLOOR' which represents a virtual collision model of the floor.", PropertyDefinitionBase::eConstant);
+
+ defineOptionalProperty<std::string>(
+ "TCPControlUnitName", "TCPControlUnit",
+ "Name of the TCPControlUnit.");
+ defineOptionalProperty<std::string>(
+ "TrajectoryControllerUnitName", "TrajectoryPlayer",
+ "Name of the TrajectoryControllerUnit. The respective component outside of the RobotUnit is TrajectoryPlayer");
}
};
@@ -474,6 +482,9 @@ namespace armarx
DebugObserverInterfacePrx debugObserverPrx;
RobotUnitObserverPtr robotUnitObs;
+ ManagedIceObjectPtr tcpControllerSubUnit;
+ ManagedIceObjectPtr trajectoryControllerSubUnit;
+
//rt logging
private:
struct RtLogging
@@ -502,7 +513,9 @@ namespace armarx
std::deque<detail::ControlThreadOutputBufferEntry> backlog;
IceUtil::Time keepBacklogFor;
- Ice::Int rtThreadId;
+ std::size_t loggingTaskTimestep {0};
+
+ Ice::Int rtThreadId {0};
void doLogging();
};
RtLogging rtLogging;
@@ -566,6 +579,8 @@ namespace armarx
void stopSelfCollisionAvoidance();
void updateSelfCollisionAvoidance();
+ RobotSynchronizationPtr selfCollisionAvoidanceRobotSynchronization;
+
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
// util
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
@@ -670,6 +685,8 @@ namespace armarx
virtual void initializePlatformUnit();
virtual void initializeForceTorqueUnit();
virtual void initializeInertialMeasurementUnit();
+ virtual void initializeTrajectoryControllerUnit();
+ virtual void initializeTcpControllerUnit();
void addUnit(ManagedIceObjectPtr unit);
void finishUnitInitialization();
// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6dca346a5c2bd056a5d402f7edb55926fd455650
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp
@@ -0,0 +1,159 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::TCPControllerSubUnit
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TCPControllerSubUnit.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+using namespace armarx;
+
+void TCPControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+{
+
+}
+
+void TCPControllerSubUnit::setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot)
+{
+ ARMARX_CHECK_EXPRESSION(robot);
+ ARMARX_CHECK_EXPRESSION(!this->coordinateTransformationRobot);
+ coordinateTransformationRobot = robot;
+
+ ARMARX_CHECK_EXPRESSION(!robotUnit);
+ ARMARX_CHECK_EXPRESSION(rUnit);
+ robotUnit = rUnit;
+
+ robotSync.reset(new RobotSynchronization(robotUnit, coordinateTransformationRobot, coordinateTransformationRobot->getRobotNodeNames()));
+}
+
+void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c)
+{
+ ARMARX_WARNING << "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)
+{
+ ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName);
+ std::string tcp;
+ if (tcpName.empty())
+ {
+ tcp = coordinateTransformationRobot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
+ }
+ else
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
+ tcp = tcpName;
+ }
+
+ auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ NJointTCPControllerPtr tcpController;
+ bool nodeSetAlreadyControlled = false;
+ for (NJointControllerPtr controller : activeNJointControllers)
+ {
+ tcpController = NJointTCPControllerPtr::dynamicCast(controller);
+ if (!tcpController)
+ {
+ continue;
+ }
+ if (tcpController->getNodeSetName() == nodeSetName)
+ {
+ nodeSetAlreadyControlled = true;
+ break;
+ }
+ }
+
+ if (!nodeSetAlreadyControlled)
+ {
+ NJointTCPControllerConfigPtr config = new NJointTCPControllerConfig(nodeSetName, tcp);
+ NJointTCPControllerPtr ctrl = NJointTCPControllerPtr::dynamicCast(
+ robotUnit->createNJointController(
+ "NJointTCPController", this->getName() + "_" + tcp + "_" + nodeSetName,
+ config, true, true
+ )
+ );
+ tcpController = ctrl;
+ }
+
+ robotSync->sync();
+
+ float xVel = 0.f;
+ float yVel = 0.f;
+ float zVel = 0.f;
+ float rollVel = 0.f;
+ float pitchVel = 0.f;
+ float yawVel = 0.f;
+ VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All;
+
+ FramedDirectionPtr globalTransVel = FramedDirectionPtr::dynamicCast(translationVelocity);
+ if (globalTransVel)
+ {
+ globalTransVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ xVel = globalTransVel->x;
+ yVel = globalTransVel->y;
+ zVel = globalTransVel->z;
+ }
+
+ FramedDirectionPtr globalOriVel = FramedDirectionPtr::dynamicCast(orientationVelocityRPY);
+ if (globalOriVel)
+ {
+ globalOriVel->changeFrame(coordinateTransformationRobot, coordinateTransformationRobot->getRootNode()->getName());
+ rollVel = globalOriVel->x;
+ pitchVel = globalOriVel->y;
+ yawVel = globalOriVel->z;
+
+ }
+
+ bool noMode = false;
+ if (globalTransVel && globalOriVel)
+ {
+ mode = VirtualRobot::IKSolver::All;
+ }
+ else if (globalTransVel && !globalOriVel)
+ {
+ mode = VirtualRobot::IKSolver::Position;
+ }
+ else if (!globalTransVel && globalOriVel)
+ {
+ mode = VirtualRobot::IKSolver::Orientation;
+ }
+ else
+ {
+ noMode = true;
+ }
+
+ ARMARX_DEBUG << "CartesianSelection-Mode: " << mode;
+
+ // Only activate controller if at least either translationVelocity or orientaionVelocity is set
+ if (!noMode)
+ {
+ ARMARX_CHECK_EXPRESSION(tcpController);
+ tcpController->setVelocities(xVel, yVel, zVel, rollVel, pitchVel, yawVel, mode);
+ if (!tcpController->isControllerActive())
+ {
+ robotUnit->activateNJointController(tcpController->getInstanceName());
+ }
+ }
+}
+
+bool TCPControllerSubUnit::isRequested(const Ice::Current&)
+{
+ return true;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4965451447d376eaf0d1742f038809492257ba5
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.h
@@ -0,0 +1,99 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::TCPControllerSubUnit
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_TCPControllerSubUnit_H
+#define _ARMARX_LIB_RobotAPI_TCPControllerSubUnit_H
+
+#include <RobotAPI/interface/units/TCPControlUnit.h>
+#include "../RobotSynchronization.h"
+
+#include "RobotUnitSubUnit.h"
+#include "../NJointControllers/NJointTCPController.h"
+#include "../util.h"
+
+namespace armarx
+{
+ class RobotUnit;
+
+ TYPEDEF_PTRS_HANDLE(TCPControllerSubUnit);
+ class TCPControllerSubUnit :
+ virtual public RobotUnitSubUnit,
+ virtual public TCPControlUnitInterface,
+ virtual public Component
+ {
+ public:
+ void setup(RobotUnit* rUnit, VirtualRobot::RobotPtr robot);
+
+ // TCPControlUnitInterface interface
+ void setCycleTime(Ice::Int milliseconds, const Ice::Current& c = GlobalIceCurrent) override;
+ void setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const::armarx::FramedDirectionBasePtr& translationVelocity, const::armarx::FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c = GlobalIceCurrent) override;
+ bool isRequested(const Ice::Current& = GlobalIceCurrent) override;
+
+ // RobotUnitSubUnit interface
+ void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
+ private:
+ // mutable std::mutex dataMutex;
+ RobotUnit* robotUnit = NULL;
+ VirtualRobot::RobotPtr coordinateTransformationRobot;
+ RobotSynchronizationPtr robotSync;
+
+ // ManagedIceObject interface
+ protected:
+ void onInitComponent() {}
+ void onConnectComponent() {}
+ std::string getDefaultName() const
+ {
+ return "TCPControlUnit";
+ }
+
+ // UnitResourceManagementInterface interface
+ public:
+ void request(const Ice::Current&)
+ {
+ }
+ void release(const Ice::Current&)
+ {
+ }
+
+ // UnitExecutionManagementInterface interface
+ public:
+ void init(const Ice::Current&) {}
+ void start(const Ice::Current&) {}
+ void stop(const Ice::Current&) {}
+ UnitExecutionState getExecutionState(const Ice::Current&)
+ {
+ return UnitExecutionState::eUndefinedUnitExecutionState;
+ }
+
+ // KinematicUnitListener interface
+ public:
+ void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) {}
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ea102ce2202e4f8c274af36574bafd7c2c5bf9f
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp
@@ -0,0 +1,450 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::TrajectoryControllerSubUnit
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "TrajectoryControllerSubUnit.h"
+
+using namespace armarx;
+
+void TrajectoryControllerSubUnit::onInitComponent()
+{
+ kinematicUnitName = getProperty<std::string>("KinematicUnitName").getValue();
+ usingProxy(kinematicUnitName);
+ robotPoseUnitEnabled = getProperty<bool>("EnableRobotPoseUnit").getValue();
+ robotPoseUnitName = getProperty<std::string>("RobotPoseUnitName").getValue();
+
+ kp = getProperty<float>("Kp").getValue();
+ 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");
+}
+
+void TrajectoryControllerSubUnit::onDisconnectComponent()
+{
+ debugDrawer->clearLayer("Preview");
+}
+
+bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Starting TrajectoryPlayer ...";
+
+ ARMARX_CHECK_EXPRESSION_W_HINT(jointTraj, "No trajectory loaded!");
+ assureTrajectoryController();
+
+ jointTrajController->setTrajectory(this->jointTraj, c);
+ jointTraj = jointTrajController->getTrajectoryCopy();
+ jointTrajController->activateController();
+
+ if (isPreview)
+ {
+ std::string fileName = kinematicUnit->getRobotFilename();
+ ARMARX_INFO << "robot file name : " << fileName;
+ debugDrawer->setRobotVisu("Preview", "previewRobot", fileName, fileName.substr(0, fileName.find("/")), 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;
+}
+
+bool TrajectoryControllerSubUnit::pauseTrajectoryPlayer(const Ice::Current& c)
+{
+ if (controllerExists())
+ {
+ if (jointTrajController->isControllerActive())
+ {
+ ARMARX_DEBUG << "Pausing TrajectoryPlayer ...";
+ jointTrajController->deactivateController();
+ }
+ else
+ {
+ ARMARX_DEBUG << "Resuming TrajectoryPlayer ...";
+ jointTrajController->activateController();
+ }
+ }
+ return true;
+}
+
+bool TrajectoryControllerSubUnit::stopTrajectoryPlayer(const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Stopping TrajectoryPlayer ...";
+
+ if (controllerExists())
+ {
+ if (isPreview)
+ {
+ previewTask->stop();
+ debugDrawer->clearLayer("Preview");
+ }
+
+ jointTrajController->deactivateController();
+ while (jointTrajController->isControllerActive()) {}
+ jointTrajController->deleteController();
+ }
+ return true;
+}
+
+bool TrajectoryControllerSubUnit::resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Resetting TrajectoryPlayer ...";
+
+ if (controllerExists() && jointTrajController->isControllerActive())
+ {
+ jointTrajController->deactivateController();
+ while (jointTrajController->isControllerActive()) {}
+ }
+
+ if (!jointTraj)
+ {
+ ARMARX_INFO << "No trajectory loaded! Cannot reset to FrameZeroPose!";
+ return false;
+ }
+
+ assureTrajectoryController();
+
+ jointTrajController->setTrajectory(this->jointTraj, c);
+ if (moveToFrameZeroPose)
+ {
+ std::vector<Ice::DoubleSeq > states = jointTraj->getAllStates(0.0f, 1);
+ NameValueMap frameZeroPositions;
+ NameControlModeMap controlModes;
+
+ 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())
+ {
+ frameZeroPositions[jointName] = states[i][0];
+ controlModes[jointName] = ePositionControl;
+ }
+ }
+
+ kinematicUnit->switchControlMode(controlModes);
+ kinematicUnit->setJointAngles(frameZeroPositions);
+ }
+ return true;
+}
+
+void TrajectoryControllerSubUnit::loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Loading Trajectory ...";
+
+ ScopedLock lock(dataMutex);
+
+ this->jointTraj = TrajectoryPtr::dynamicCast(jointTraj);
+
+ if (!this->jointTraj)
+ {
+ ARMARX_ERROR << "Error when loading TrajectoryPlayer: cannot load jointTraj !!!";
+ return;
+ }
+
+ this->jointTraj = this->jointTraj->normalize(0, *this->jointTraj->getTimestamps().rbegin() - *this->jointTraj->getTimestamps().begin());
+ usedJoints = this->jointTraj->getDimensionNames();
+ ARMARX_INFO << VAROUT(usedJoints);
+
+ if (usedJoints.size() != this->jointTraj->dim())
+ {
+ ARMARX_ERROR << "Not all trajectory dimensions are named !!! (would cause problems when using kinematicUnit)";
+ return;
+ }
+
+ jointTrajController = createTrajectoryController(usedJoints, true);
+ jointTrajController->setTrajectory(this->jointTraj, c);
+
+ endTime = jointTrajController->getTrajEndTime();
+ trajEndTime = endTime;
+}
+
+void TrajectoryControllerSubUnit::loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& c)
+{
+ this->basePoseTraj = TrajectoryPtr::dynamicCast(basePoseTraj);
+}
+
+void TrajectoryControllerSubUnit::setLoopPlayback(bool loop, const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+
+ if (!controllerExists())
+ {
+ ARMARX_WARNING << "No trajectory has been loaded and therefore no controller exists.";
+ return;
+ }
+ jointTrajController->setLooping(loop);
+}
+
+double TrajectoryControllerSubUnit::getEndTime(const Ice::Current& c)
+{
+ return endTime;
+}
+
+double TrajectoryControllerSubUnit::getTrajEndTime(const Ice::Current& c)
+{
+ return trajEndTime;
+}
+
+double TrajectoryControllerSubUnit::getCurrentTime(const Ice::Current& c)
+{
+ ScopedLock lock(dataMutex);
+
+ if (!controllerExists())
+ {
+ ARMARX_WARNING << "No trajectory has been loaded and therefore no controller exists.";
+ return 0.0;
+ }
+ return jointTrajController->getCurrentTrajTime();
+}
+
+void TrajectoryControllerSubUnit::setEndTime(Ice::Double end, const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Setting end-time ...";
+
+ ScopedLock lock(dataMutex);
+
+ if (!jointTraj)
+ {
+ ARMARX_WARNING << "No trajectory has been loaded!";
+ return;
+ }
+ assureTrajectoryController();
+
+ if (jointTrajController->isControllerActive())
+ {
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting a new end-time.";
+ return;
+ }
+
+ if (endTime == end)
+ {
+ return;
+ }
+
+ bool b = considerConstraintsForTrajectoryOptimization;
+ considerConstraintsForTrajectoryOptimization = false;
+
+ endTime = end;
+ jointTraj = jointTraj->normalize(0, endTime);
+ if (basePoseTraj)
+ {
+ basePoseTraj = basePoseTraj->normalize(0, endTime);
+ }
+
+ jointTrajController->setTrajectory(jointTraj, c);
+
+ considerConstraintsForTrajectoryOptimization = b;
+}
+
+void TrajectoryControllerSubUnit::setIsPreview(bool isPreview, const Ice::Current& c)
+{
+ 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, true);
+ jointTrajController->setTrajectory(jointTraj, c);
+ }
+}
+
+bool TrajectoryControllerSubUnit::setJointsInUse(const std::string& jointName, bool isInUse, const Ice::Current& c)
+{
+ ARMARX_DEBUG << "Setting joints in use ...";
+
+ ScopedLock lock(dataMutex);
+
+ if (controllerExists())
+ {
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting joints in use.";
+ }
+ else
+ {
+ if (isInUse && (std::find(usedJoints.begin(), usedJoints.end(), jointName) == usedJoints.end()))
+ {
+ usedJoints.push_back(jointName);
+ }
+ else if (!isInUse)
+ {
+ auto it = std::find(usedJoints.begin(), usedJoints.end(), jointName);
+ if (it != usedJoints.end())
+ {
+ std::swap(*it, usedJoints.back());
+ usedJoints.pop_back();
+ }
+ }
+
+ if (jointTraj)
+ {
+ jointTrajController = createTrajectoryController(usedJoints, true);
+ jointTrajController->setTrajectory(jointTraj, c);
+ }
+ }
+
+ return isInUse;
+}
+
+void TrajectoryControllerSubUnit::enableRobotPoseUnit(bool isRobotPose, const Ice::Current& c)
+{
+ ARMARX_WARNING << "NYI : TrajectoryControllerSubUnit::enableRobotPoseUnit()";
+}
+
+void TrajectoryControllerSubUnit::update(const SensorAndControl& sc, const JointAndNJointControllers& c)
+{
+
+}
+
+void TrajectoryControllerSubUnit::considerConstraints(bool consider, const Ice::Current& c)
+{
+ if (controllerExists() && jointTrajController->isControllerActive())
+ {
+ ARMARX_WARNING << "TrajectoryController already running. Please reset it before setting whether constraints should be considered.";
+ return;
+ }
+ considerConstraintsForTrajectoryOptimization = consider;
+
+ if (jointTraj)
+ {
+ jointTrajController = createTrajectoryController(usedJoints, false);
+ jointTrajController->setTrajectory(jointTraj, c);
+ }
+}
+
+NJointTrajectoryControllerPtr TrajectoryControllerSubUnit::createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist)
+{
+ ScopedRecursiveLock lock(controllerMutex);
+
+ if (controllerExists() && deleteIfAlreadyExist)
+ {
+ jointTrajController->deactivateController();
+ while (jointTrajController->isControllerActive())
+ {
+ usleep(500);
+ }
+ jointTrajController->deleteController();
+ while (getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
+ {
+ usleep(500);
+ }
+ }
+
+ NJointTrajectoryControllerPtr trajController = jointTrajController;
+ if (!controllerExists() || deleteIfAlreadyExist)
+ {
+ NJointTrajectoryControllerConfigPtr config = new NJointTrajectoryControllerConfig();
+ config->PID_p = kp;
+ config->PID_i = ki;
+ config->PID_d = kd;
+ config->maxVelocity = maxVelocity;
+ config->jointNames = jointNames;
+ config->considerConstraints = considerConstraintsForTrajectoryOptimization;
+ config->isPreview = isPreview;
+
+ trajController = NJointTrajectoryControllerPtr::dynamicCast(
+ robotUnit->createNJointController(
+ "NJointTrajectoryController", controllerName,
+ config, true, true
+ )
+ );
+
+ while (!getArmarXManager()->getIceManager()->isObjectReachable(controllerName))
+ {
+ usleep(500);
+ }
+ }
+ return trajController;
+}
+
+void TrajectoryControllerSubUnit::assureTrajectoryController()
+{
+ ScopedRecursiveLock lock(controllerMutex);
+
+ if (!controllerExists())
+ {
+ jointTrajController = createTrajectoryController(usedJoints, false);
+ }
+ ARMARX_CHECK_EXPRESSION(jointTrajController);
+}
+
+bool TrajectoryControllerSubUnit::controllerExists()
+{
+ ScopedRecursiveLock lock(controllerMutex);
+
+ auto allNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+ NJointTrajectoryControllerPtr trajController;
+ for (NJointControllerPtr controller : allNJointControllers)
+ {
+ trajController = NJointTrajectoryControllerPtr::dynamicCast(controller);
+ if (!trajController)
+ {
+ continue;
+ }
+ if (trajController->getName() == controllerName)
+ {
+ jointTrajController = trajController;
+ return true;
+ }
+ }
+ return false;
+}
+
+void TrajectoryControllerSubUnit::previewRun()
+{
+ if (controllerExists())
+ {
+ if (jointTrajController->isControllerActive())
+ {
+ 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);
+ ARMARX_CHECK_EXPRESSION(rUnit);
+ robotUnit = rUnit;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..571f13f1a7c51d500f3542701a37bf9e0a37a5a1
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.h
@@ -0,0 +1,169 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::TrajectoryControllerSubUnit
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_TrajectoryControllerSubUnit_H
+#define _ARMARX_LIB_RobotAPI_TrajectoryControllerSubUnit_H
+
+#include "RobotUnitSubUnit.h"
+
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
+#include "../NJointControllers/NJointTrajectoryController.h"
+#include "../RobotUnit.h"
+
+#include "KinematicSubUnit.h"
+
+
+namespace armarx
+{
+ class TrajectoryControllerSubUnitPropertyDefinitions:
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ TrajectoryControllerSubUnitPropertyDefinitions(std::string prefix):
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ defineRequiredProperty<std::string>("KinematicUnitName", "Name of the KinematicUnit. Only needed for returning to zeroFramePose while resetting.");
+ defineOptionalProperty<float>("Kp", 1.f, "Proportional gain value of PID Controller");
+ defineOptionalProperty<float>("Ki", 0.0f, "Integral gain value of PID Controller");
+ defineOptionalProperty<float>("Kd", 0.0f, "Differential gain value of PID Controller");
+ defineOptionalProperty<float>("absMaximumVelocity", 80.0f, "The PID will never set a velocity above this threshold (degree/s)");
+ defineOptionalProperty<std::string>("CustomRootNode", "", "If this value is set, the root pose of the motion is set for this node instead of the root joint.");
+
+ defineOptionalProperty<bool>("EnableRobotPoseUnit", false, "Specify whether RobotPoseUnit should be used to move the robot's position. Only useful in simulation.")
+ .setCaseInsensitive(true)
+ .map("true", true)
+ .map("yes", true)
+ .map("1", true)
+ .map("false", false)
+ .map("no", false)
+ .map("0", false);
+ defineOptionalProperty<std::string>("RobotPoseUnitName", "RobotPoseUnit", "Name of the RobotPoseUnit to which the robot position should be sent");
+ }
+ };
+
+
+ TYPEDEF_PTRS_HANDLE(TrajectoryControllerSubUnit);
+ class TrajectoryControllerSubUnit :
+ virtual public RobotUnitSubUnit,
+ virtual public TrajectoryPlayerInterface,
+ virtual public Component
+ {
+ // TrajectoryControllerSubUnitInterface interface
+ public:
+ bool startTrajectoryPlayer(const Ice::Current& = GlobalIceCurrent);
+ bool pauseTrajectoryPlayer(const Ice::Current& = GlobalIceCurrent);
+ bool stopTrajectoryPlayer(const Ice::Current& = GlobalIceCurrent);
+ bool resetTrajectoryPlayer(bool moveToFrameZeroPose, const Ice::Current& = GlobalIceCurrent);
+
+ void loadJointTraj(const TrajectoryBasePtr& jointTraj, const Ice::Current& = GlobalIceCurrent);
+ void loadBasePoseTraj(const TrajectoryBasePtr& basePoseTraj, const Ice::Current& = GlobalIceCurrent);
+
+ void setLoopPlayback(bool loop, const Ice::Current& = GlobalIceCurrent);
+ Ice::Double getEndTime(const Ice::Current& = GlobalIceCurrent);
+ Ice::Double getTrajEndTime(const Ice::Current& = GlobalIceCurrent);
+ Ice::Double getCurrentTime(const Ice::Current& = GlobalIceCurrent);
+ void setEndTime(Ice::Double, const Ice::Current& = GlobalIceCurrent);
+
+ // Within the RobotUnit the NJointTrajectoryController is always in VelocityControl
+ void setIsVelocityControl(bool, const Ice::Current& = GlobalIceCurrent) {}
+
+ void setIsPreview(bool, const Ice::Current& = GlobalIceCurrent);
+ bool setJointsInUse(const std::string&, bool, const Ice::Current& = GlobalIceCurrent);
+ void enableRobotPoseUnit(bool, const Ice::Current& = GlobalIceCurrent);
+
+ void considerConstraints(bool, const Ice::Current& = GlobalIceCurrent);
+
+ // RobotUnitSubUnit interface
+ void update(const SensorAndControl& sc, const JointAndNJointControllers& c);
+
+ void setup(RobotUnit* rUnit);
+
+ // KinematicUnitListener interface
+ void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointAngles(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointVelocities(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) {}
+ void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) {}
+ // ManagedIceObject interface
+ protected:
+ void onInitComponent();
+ void onConnectComponent();
+ void onDisconnectComponent();
+ std::string getDefaultName() const
+ {
+ return "TrajectoryPlayer";
+ }
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions()
+ {
+ return armarx::PropertyDefinitionsPtr
+ {
+ new TrajectoryControllerSubUnitPropertyDefinitions{getConfigIdentifier()}
+ };
+ }
+
+ private:
+ NJointTrajectoryControllerPtr createTrajectoryController(std::vector<std::string>& jointNames, bool deleteIfAlreadyExist);
+ void assureTrajectoryController();
+ bool controllerExists();
+ void previewRun();
+
+ RobotUnit* robotUnit = NULL;
+ NJointTrajectoryControllerPtr jointTrajController;
+ std::string controllerName = this->getName() + "_" + "JointTrajectory";
+
+ TrajectoryPtr jointTraj;
+
+ // so far no usage -> need to implement usage of robotPoseUnit (only for simulation) -> does it really belong here?
+ TrajectoryPtr basePoseTraj;
+ bool robotPoseUnitEnabled;
+ std::string robotPoseUnitName;
+
+ double endTime;
+ double trajEndTime;
+ std::vector<std::string> usedJoints;
+
+ std::string kinematicUnitName;
+ KinematicUnitInterfacePrx kinematicUnit;
+ std::string customRootNode;
+
+ float kp;
+ float ki;
+ float kd;
+ float maxVelocity;
+ bool considerConstraintsForTrajectoryOptimization = false;
+
+ bool isPreview = false;
+ DebugDrawerInterfacePrx debugDrawer;
+ PeriodicTask<TrajectoryControllerSubUnit>::pointer_type previewTask;
+
+ Mutex dataMutex;
+ RecursiveMutex controllerMutex;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index 1581e16558d94ddde0a1f4b9e0a94831bdbe1afc..ceccf18f5508ec86e9e8983fe948cb8e19309c2c 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -64,54 +64,54 @@ namespace armarx
{
namespace introspection
{
- #define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
- std::string DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- return std::to_string(field(i)); \
- } \
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
+#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
+ std::string DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ return std::to_string(field(i)); \
+ } \
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
const Eigen::Vector##Num##Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
const std::string& frame, \
const std::string& agent) \
- { \
- ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant"); \
- std::map<std::string, VariantBasePtr> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace(name + "." + std::to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
- } \
- return result; \
- } \
- std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
- { \
- std::vector<std::string> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace_back(std::to_string(i)); \
- } \
- return result; \
- }
+ { \
+ ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant"); \
+ std::map<std::string, VariantBasePtr> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace(name + "." + std::to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
+ } \
+ return result; \
+ } \
+ std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
+ { \
+ std::vector<std::string> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace_back(std::to_string(i)); \
+ } \
+ return result; \
+ }
- make_DataFieldsInfo_for_eigen_vector(f,2)
- make_DataFieldsInfo_for_eigen_vector(f,4)
- make_DataFieldsInfo_for_eigen_vector(f,5)
- make_DataFieldsInfo_for_eigen_vector(f,6)
+ make_DataFieldsInfo_for_eigen_vector(f, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, 6)
- make_DataFieldsInfo_for_eigen_vector(d,2)
- make_DataFieldsInfo_for_eigen_vector(d,3)
- make_DataFieldsInfo_for_eigen_vector(d,4)
- make_DataFieldsInfo_for_eigen_vector(d,5)
- make_DataFieldsInfo_for_eigen_vector(d,6)
+ make_DataFieldsInfo_for_eigen_vector(d, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, 6)
- make_DataFieldsInfo_for_eigen_vector(i,2)
- make_DataFieldsInfo_for_eigen_vector(i,3)
- make_DataFieldsInfo_for_eigen_vector(i,4)
- make_DataFieldsInfo_for_eigen_vector(i,5)
- make_DataFieldsInfo_for_eigen_vector(i,6)
- #undef make_DataFieldsInfo_for_eigen_vector
+ make_DataFieldsInfo_for_eigen_vector(i, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, 6)
+#undef make_DataFieldsInfo_for_eigen_vector
}
}
@@ -130,9 +130,9 @@ namespace armarx
case 1:
return std::to_string(field.operation);
case 2:
- return field.enabled ? "true" : "false";
+ return std::to_string(field.enabled);
case 3:
- return field.emergencyStop ? "true" : "false";
+ return std::to_string(field.emergencyStop);
}
throw std::logic_error
{
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 33d31e2cdc661bdc42a278892386e0865128de40..d6a550f504c16a6605a9e3d980b4c80e9726f956 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -45,6 +45,7 @@ set(SLICE_FILES
units/RobotUnit/RobotUnitInterface.ice
components/ViewSelectionInterface.ice
+ components/TrajectoryPlayerInterface.ice
visualization/DebugDrawerInterface.ice
diff --git a/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..57f0dcfd505c981c073dcf421bcb817b8dda8fc7
--- /dev/null
+++ b/source/RobotAPI/interface/components/TrajectoryPlayerInterface.ice
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::TrajectoryControllerSubUnit
+ * @author Stefan Reither ( stef dot reither at web dot de )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/interface/core/Trajectory.ice>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.ice>
+
+#ifndef _ARMARX_ROBOTAPI_SUBUNITS_TRAJECTORYCONTROLLER_SLICE_
+#define _ARMARX_ROBOTAPI_SUBUNITS_TRAJECTORYCONTROLLER_SLICE_
+
+module armarx
+{
+ interface TrajectoryPlayerInterface extends KinematicUnitListener
+ {
+ bool startTrajectoryPlayer();
+ bool pauseTrajectoryPlayer();
+ bool stopTrajectoryPlayer();
+ bool resetTrajectoryPlayer(bool moveToFrameZeroPose);
+
+ void loadJointTraj(TrajectoryBase jointTraj);
+ void loadBasePoseTraj(TrajectoryBase basePoseTraj);
+
+ void setLoopPlayback(bool loop);
+ void setIsVelocityControl(bool isVelocity);
+ void setIsPreview(bool isPreview);
+ bool setJointsInUse(string jointName, bool inUse);
+ void enableRobotPoseUnit(bool isRobotPose);
+
+ double getCurrentTime();
+ double getEndTime();
+ double getTrajEndTime();
+
+ void setEndTime(double endTime);
+
+ void considerConstraints(bool consider);
+ };
+};
+
+#endif
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
index a384c49eb909a5fb32065048f6cee063798602ad..e4e299be91408fe7cc5efd0d5dea535f92065cb9 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.ice
@@ -30,14 +30,16 @@ module armarx
{
class NJointTrajectoryControllerConfig extends NJointControllerConfig
{
- float maxAcceleration = 3;
- float maxVelocity = 1.4;
- float maxDeviation = 0.01;
- float preSamplingStepMs = 10;
+ float maxAcceleration = 1.5;
+ float maxVelocity = 0.5;
+ float maxDeviation = 10;
+ float preSamplingStepMs = 50;
Ice::StringSeq jointNames;
float PID_p = 1;
float PID_i = 0;
float PID_d = 0;
+ bool considerConstraints = true;
+ bool isPreview = false;
};
interface NJointTrajectoryControllerInterface extends NJointControllerInterface
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 12745ffe064e27a3d9d4c91b96882bcd313772ac..e39f631ab5ce5849ee88924a077dc294e07c2779 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -148,7 +148,7 @@ double PIDController::getControlValue() const
}
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation) :
+MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -156,14 +156,15 @@ MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, doubl
derivative(0),
previousError(0),
maxControlValue(maxControlValue),
- maxDerivation(maxDerivation)
+ maxDerivation(maxDerivation),
+ threadSafe(threadSafe)
{
reset();
}
void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
processValue = measuredValue;
target = targetValue;
@@ -239,7 +240,7 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
void MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
@@ -259,7 +260,7 @@ const Eigen::VectorXf& MultiDimPIDController::getControlValue() const
void MultiDimPIDController::reset()
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
firstRun = true;
previousError = 0;
integral = 0;
@@ -269,8 +270,20 @@ void MultiDimPIDController::reset()
// target.setZero();
}
+ScopedRecursiveLockPtr MultiDimPIDController::getLock() const
+{
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+}
+
void MultiDimPIDController::setMaxControlValue(double value)
{
- ScopedRecursiveLock lock(mutex);
+ ScopedRecursiveLockPtr lock = getLock();
maxControlValue = value;
}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index e3676c7590e6ded3e67233c352fa415ad6a89130..240baec49b370e3ca92cac98ff1b77d8c703a77b 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -71,7 +71,12 @@ namespace armarx
public Logging
{
public:
- MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxDerivation = std::numeric_limits<double>::max());
+ MultiDimPIDController(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue = std::numeric_limits<double>::max(),
+ double maxDerivation = std::numeric_limits<double>::max(),
+ bool threadSafe = true);
void update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
void update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
const Eigen::VectorXf& getControlValue() const;
@@ -91,6 +96,9 @@ namespace armarx
double maxDerivation;
bool firstRun;
mutable RecursiveMutex mutex;
+ bool threadSafe = true;
+ private:
+ ScopedRecursiveLockPtr getLock() const;
};
typedef boost::shared_ptr<MultiDimPIDController> MultiDimPIDControllerPtr;
}
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index b0ef102793e14ee49100dd080ac7420a89b7a210..d852325e0007d243f033cdb3c524a315b197adf7 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -26,11 +26,10 @@
namespace armarx
{
- TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd) :
+ TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe) :
traj(traj)
{
- pid.reset(new MultiDimPIDController(kp, ki, kd));
-
+ pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe));
ARMARX_CHECK_EXPRESSION(traj);
currentTimestamp = traj->begin()->getTimestamp();
positions.resize(traj->dim(), 1);
@@ -44,17 +43,23 @@ namespace armarx
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
int dim = traj->dim();
currentTimestamp = currentTimestamp + deltaT;
+
+ if (currentTimestamp < 0.0)
+ {
+ currentTimestamp = 0.0;
+ }
+
for (int i = 0; i < dim; ++i)
{
positions(i) = traj->getState(currentTimestamp, i, 0);
- veloctities(i) = traj->getState(currentTimestamp, i, 1);
+ veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
}
- pid->update(deltaT, currentPosition, positions);
+ pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
return veloctities;
}
- MultiDimPIDControllerPtr TrajectoryController::getPid() const
+ const MultiDimPIDControllerPtr& TrajectoryController::getPid() const
{
return pid;
}
@@ -69,7 +74,7 @@ namespace armarx
return currentTimestamp;
}
- TrajectoryPtr TrajectoryController::getTraj() const
+ const TrajectoryPtr& TrajectoryController::getTraj() const
{
return traj;
}
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index f8a80e6db1b7e6ec0edbbf2960240e81ba5ffa4a..f311cc25e38eab0008abc02c0d2484ac898a2ee2 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -32,14 +32,14 @@ namespace armarx
class TrajectoryController
{
public:
- TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f);
+ TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
- MultiDimPIDControllerPtr getPid() const;
+ const MultiDimPIDControllerPtr& getPid() const;
void setPid(const MultiDimPIDControllerPtr& value);
double getCurrentTimestamp() const;
- TrajectoryPtr getTraj() const;
+ const TrajectoryPtr& getTraj() const;
protected:
TrajectoryPtr traj;