diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 2bbf03b242fa4be240287ca624050274f0af19ca..89a1d7833a64a2bdab3d46d00f07a619f60f32fb 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -40,6 +40,7 @@ set(LIB_FILES
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+ NJointControllers/NJointTCPController.cpp
Devices/SensorDevice.cpp
Devices/ControlDevice.cpp
@@ -79,6 +80,7 @@ set(LIB_HEADERS
NJointControllers/NJointTrajectoryController.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+ NJointControllers/NJointTCPController.h
Devices/DeviceBase.h
Devices/SensorDevice.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f0ef41fa60c6d2881a1575312b3854a925ad2d9
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -0,0 +1,199 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointTCPController.h"
+#include <VirtualRobot/RobotNodeSet.h>
+#include "../RobotUnit.h"
+
+#define DEFAULT_TCP_STRING "default TCP"
+
+namespace armarx
+{
+
+ NJointControllerRegistration<NJointTCPController> registrationControllerNJointTCPController("NJointTCPController");
+
+ std::string NJointTCPController::getClassName(const Ice::Current&) const
+ {
+ return "NJointTCPController";
+ }
+
+ void NJointTCPController::rtPreActivateController()
+ {
+ xVel = 0;
+ yVel = 0;
+ zVel = 0;
+ rollVel = 0;
+ pitchVel = 0;
+ yawVel = 0;
+ }
+
+ 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);
+ Eigen::VectorXf jointTargetVelocities = jacobiInv * x;
+ for (size_t i = 0; i < targets.size(); ++i)
+ {
+ targets.at(i)->velocity = jointTargetVelocities(i);
+ }
+ }
+
+
+ ::armarx::WidgetDescription::WidgetSeq createSliders()
+ {
+ using namespace armarx::WidgetDescription;
+ ::armarx::WidgetDescription::WidgetSeq widgets;
+ auto addSlider = [&](const std::string & label, float limit)
+ {
+ widgets.emplace_back(new Label(false, label));
+ {
+ FloatSliderPtr c_x = new FloatSlider;
+ c_x->name = label;
+ c_x->min = -limit;
+ c_x->defaultValue = 0.0f;
+ c_x->max = limit;
+ widgets.emplace_back(c_x);
+ }
+ };
+
+ addSlider("x", 100);
+ addSlider("y", 100);
+ addSlider("z", 100);
+ addSlider("r", 0.5);
+ addSlider("p", 0.5);
+ addSlider("y", 0.5);
+ return widgets;
+ }
+
+ WidgetDescription::WidgetPtr NJointTCPController::GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>& robot, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>&)
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+
+ LabelPtr label = new Label;
+ label->text = "nodeset name";
+ layout->children.emplace_back(label);
+ StringComboBoxPtr box = new StringComboBox;
+ box->defaultIndex = 0;
+
+ box->options = robot->getRobotNodeSetNames();
+ box->name = "nodeSetName";
+ layout->children.emplace_back(box);
+
+ LabelPtr labelTCP = new Label;
+ labelTCP->text = "tcp name";
+ layout->children.emplace_back(labelTCP);
+ StringComboBoxPtr boxTCP = new StringComboBox;
+ boxTCP->defaultIndex = 0;
+
+ boxTCP->options = robot->getRobotNodeNames();
+ boxTCP->options.insert(boxTCP->options.begin(), DEFAULT_TCP_STRING);
+ boxTCP->name = "tcpName";
+ layout->children.emplace_back(boxTCP);
+
+ LabelPtr labelMode = new Label;
+ labelMode->text = "mode";
+ layout->children.emplace_back(labelMode);
+ StringComboBoxPtr boxMode = new StringComboBox;
+ boxMode->defaultIndex = 0;
+
+ boxMode->options = {"Position", "Orientation", "Both"};
+ boxMode->name = "mode";
+ layout->children.emplace_back(boxMode);
+
+
+ // auto sliders = createSliders();
+ // layout->children.insert(layout->children.end(),
+ // sliders.begin(),
+ // sliders.end());
+ layout->children.emplace_back(new HSpacer);
+ return layout;
+ }
+
+ NJointTCPControllerConfigPtr NJointTCPController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
+ {
+ return new NJointTCPControllerConfig {values.at("nodeSetName")->getString(), values.at("tcpName")->getString()};
+ }
+
+ NJointTCPController::NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, NJointTCPControllerConfigPtr config, const boost::shared_ptr<VirtualRobot::Robot>& r)
+ {
+ ARMARX_CHECK_EXPRESSION(prov);
+ RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty());
+ auto nodeset = robotUnit->getRtRobot()->getRobotNodeSet(config->nodeSetName);
+ ARMARX_CHECK_EXPRESSION_W_HINT(nodeset, config->nodeSetName);
+ for (size_t i = 0; i < nodeset->getSize(); ++i)
+ {
+ auto jointName = nodeset->getNode(i)->getName();
+ 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>());
+ };
+ 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);
+ }
+
+ void NJointTCPController::rtPostDeactivateController()
+ {
+
+ }
+
+ WidgetDescription::StringWidgetDictionary NJointTCPController::getFunctionDescriptions(const Ice::Current&) const
+ {
+ using namespace armarx::WidgetDescription;
+ HBoxLayoutPtr layout = new HBoxLayout;
+ auto sliders = createSliders();
+ layout->children.insert(layout->children.end(),
+ sliders.begin(),
+ sliders.end());
+ return {{"ControllerConfig", layout}};
+ }
+
+ void NJointTCPController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&)
+ {
+ if (name == "ControllerConfig")
+ {
+ xVel = valueMap.at("x")->getFloat();
+ yVel = valueMap.at("y")->getFloat();
+ zVel = valueMap.at("z")->getFloat();
+ rollVel = valueMap.at("r")->getFloat();
+ pitchVel = valueMap.at("p")->getFloat();
+ yawVel = valueMap.at("y")->getFloat();
+ }
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..356457a7ea461d0f0755ff4b71328b2d1109be3e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -0,0 +1,98 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+
+#include "NJointController.h"
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointTCPController);
+ TYPEDEF_PTRS_HANDLE(NJointTCPControllerConfig);
+
+ class NJointTCPControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ NJointTCPControllerConfig(std::string const& nodeSetName, const std::string& tcpName):
+ nodeSetName(nodeSetName),
+ tcpName(tcpName)
+ {}
+
+ const std::string nodeSetName;
+ const std::string tcpName;
+ };
+
+
+ class NJointTCPController : public NJointController
+ {
+ public:
+ using ConfigPtrT = NJointTCPControllerConfigPtr;
+ NJointTCPController(NJointControllerDescriptionProviderInterfacePtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current&) const override;
+ WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
+ void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current&) override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ static WidgetDescription::WidgetPtr GenerateConfigDescription(
+ const boost::shared_ptr<VirtualRobot::Robot>&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&);
+
+ static NJointTCPControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ NJointTCPController(
+ NJointControllerDescriptionProviderInterfacePtr prov,
+ NJointTCPControllerConfigPtr config,
+ const boost::shared_ptr<VirtualRobot::Robot>& r);
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+ private:
+ 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;
+
+
+
+
+
+ };
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 9325ae45b38fce80796974759fc18a71ab439984..2efa4a636ac1370101e116142b1cf16847eddd68 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -23,6 +23,7 @@ namespace armarx
cfg->PID_i, cfg->PID_d,
cfg->maxVelocity)));
}
+ currentPos.resize(cfg->jointNames.size());
}
std::string NJointTrajectoryController::getClassName(const Ice::Current&) const
@@ -33,40 +34,18 @@ namespace armarx
void NJointTrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
TIMING_START(TrajectoryControl);
- const Trajectory& trajectory = *rtGetControlStruct().trajectory;
- if (startTime.toMicroSeconds() == 0)
+ TrajectoryController& contr = *rtGetControlStruct().trajectory;
+ int i = 0;
+ for (auto& sv : sensors)
{
- startTime = sensorValuesTimestamp;
+ currentPos(i) = sv->position;
+ i++;
}
- auto timeSinceStart = sensorValuesTimestamp - startTime;
- if (timeSinceStart.toSecondsDouble() > duration)
+ auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble(), currentPos);
+ bool finished = contr.getCurrentTimestamp() >= contr.getTraj()->rbegin()->getTimestamp();
+ for (size_t i = 0; i < targets.size(); ++i)
{
- for (size_t d = 0; d < targets.size(); ++d)
- {
- targets.at(d)->velocity = 0;
- }
- }
- else
- {
- auto positions = trajectory.getStates(timeSinceStart.toSecondsDouble(), 0);
- auto velocities = trajectory.getStates(timeSinceStart.toSecondsDouble(), 1);
- // size_t dim = trajectory->dim();
-
- for (size_t d = 0; d < targets.size(); ++d)
- {
- ARMARX_CHECK_EXPRESSION(d < targets.size());
- ARMARX_CHECK_EXPRESSION(d < PIDs.size());
- ARMARX_CHECK_EXPRESSION(d < sensors.size());
- ARMARX_CHECK_EXPRESSION(d < positions.size());
- ARMARX_CHECK_EXPRESSION(d < velocities.size());
- PIDControllerPtr& pid = PIDs.at(d);
- pid->update(timeSinceLastIteration.toSecondsDouble(),
- sensors.at(d)->position,
- positions.at(d));
- double newVel = pid->getControlValue();
- newVel += velocities.at(1);
- targets.at(d)->velocity = newVel;
- }
+ targets.at(i)->velocity = finished ? 0.0f : newVelocities(i);
}
TIMING_END(TrajectoryControl);
@@ -101,7 +80,7 @@ namespace armarx
TrajectoryPtr newTraj = new Trajectory();
Ice::DoubleSeq newTimestamps;
- duration = timeOptimalTraj.getDuration();
+ double duration = timeOptimalTraj.getDuration();
for (double t = 0.0; t < duration; t += timeStep)
{
newTimestamps.push_back(t);
@@ -135,7 +114,7 @@ namespace armarx
TIMING_END(TrajectoryOptimization);
ARMARX_INFO << "Trajectory duration: " << newTraj->getTimeLength();
LockGuardType guard {controlDataMutex};
- getWriterControlStruct().trajectory = newTraj;
+ getWriterControlStruct().trajectory.reset(new TrajectoryController(newTraj, cfg->PID_p, cfg->PID_i, cfg->PID_d));
writeControlStruct();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 337ba7e0171072c4b76c66de0cdf7ce225d09c4b..5bde47d194952216ea67decd9881d2c0c7ae8122 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -5,18 +5,17 @@
#include <VirtualRobot/Robot.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotAPI/libraries/core/TrajectoryController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
namespace armarx
{
- TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
class NJointTrajectoryControllerControlData
{
public:
- TrajectoryPtr trajectory = TrajectoryPtr(new Trajectory());
+ TrajectoryControllerPtr trajectory;
};
class NJointTrajectoryController :
@@ -38,10 +37,10 @@ namespace armarx
private:
// TrajectoryPtr trajectory;
IceUtil::Time startTime;
- double duration = 0;
std::vector<PIDControllerPtr> PIDs;
std::vector<ControlTarget1DoFActuatorVelocity*> targets;
std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+ Eigen::VectorXf currentPos;
NJointTrajectoryControllerConfigPtr cfg;
virtual void onInitComponent() override {}
virtual void onConnectComponent() override {}
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index d4b664fbc4187f504993d8b99b80b83835931ce2..5d95a14df4202afb817f3e466fd6192bfd75e65f 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -23,6 +23,7 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
set(LIB_FILES
PIDController.cpp
Trajectory.cpp
+ TrajectoryController.cpp
Pose.cpp
FramedPose.cpp
LinkedPose.cpp
@@ -41,6 +42,7 @@ set(LIB_HEADERS
EigenStl.h
PIDController.h
Trajectory.h
+ TrajectoryController.h
VectorHelpers.h
Pose.h
FramedPose.h
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5781ecb570979260cb4db190f8b1dbe0051cf6d
--- /dev/null
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -0,0 +1,78 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "TrajectoryController.h"
+
+namespace armarx
+{
+
+ TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd) :
+ traj(traj)
+ {
+ pid.reset(new MultiDimPIDController(kp, ki, kd));
+
+ ARMARX_CHECK_EXPRESSION(traj);
+ currentTimestamp = traj->begin()->getTimestamp();
+ positions.resize(traj->dim(), 1);
+ veloctities.resize(traj->dim(), 1);
+ }
+
+ Eigen::VectorXf TrajectoryController::update(double deltaT, const Eigen::VectorXf currentPosition)
+ {
+ ARMARX_CHECK_EXPRESSION(pid);
+ ARMARX_CHECK_EXPRESSION(traj);
+ ARMARX_CHECK_EQUAL(currentPosition.rows(), traj->dim());
+ int dim = traj->dim();
+ currentTimestamp = currentTimestamp + deltaT;
+ for (int i = 0; i < dim; ++i)
+ {
+ positions(i) = traj->getState(currentTimestamp, i, 0);
+ veloctities(i) = traj->getState(currentTimestamp, i, 1);
+ }
+ pid->update(deltaT, currentPosition, positions);
+ veloctities += pid->getControlValue();
+ return veloctities;
+ }
+
+ MultiDimPIDControllerPtr TrajectoryController::getPid() const
+ {
+ return pid;
+ }
+
+ void TrajectoryController::setPid(const MultiDimPIDControllerPtr& value)
+ {
+ pid = value;
+ }
+
+ double TrajectoryController::getCurrentTimestamp() const
+ {
+ return currentTimestamp;
+ }
+
+ TrajectoryPtr TrajectoryController::getTraj() const
+ {
+ return traj;
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
new file mode 100644
index 0000000000000000000000000000000000000000..f8a80e6db1b7e6ec0edbbf2960240e81ba5ffa4a
--- /dev/null
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -0,0 +1,55 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "PIDController.h"
+#include "Trajectory.h"
+#pragma once
+
+namespace armarx
+{
+
+ class TrajectoryController
+ {
+ public:
+ TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f);
+ Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
+ MultiDimPIDControllerPtr getPid() const;
+ void setPid(const MultiDimPIDControllerPtr& value);
+
+ double getCurrentTimestamp() const;
+
+ TrajectoryPtr getTraj() const;
+
+ protected:
+ TrajectoryPtr traj;
+ MultiDimPIDControllerPtr pid;
+ double currentTimestamp;
+ Eigen::VectorXf positions;
+ Eigen::VectorXf veloctities;
+
+ };
+ typedef std::shared_ptr<TrajectoryController> TrajectoryControllerPtr;
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index b657d0f3a8d9a93c1a6856db492172bab695a521..8c8cf2eaa858a490a570f2b4ba0c65e4f7277ced 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -26,6 +26,8 @@
#include <ArmarXCore/core/test/IceTestHelper.h>
#include <ArmarXCore/util/json/JSONObject.h>
#include "../Trajectory.h"
+#include "../TrajectoryController.h"
+#include <random>
using namespace armarx;
@@ -257,6 +259,28 @@ BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
ARMARX_INFO_S << VAROUT(selection);
}
+BOOST_AUTO_TEST_CASE(TrajectoryControllerTest)
+{
+ FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+
+ TrajectoryController c(traj, 0, 1);
+ float t = 0.0f;
+ float deltaT = 0.001f;
+
+ std::default_random_engine generator;
+ std::normal_distribution<float> distribution(1.0, 0.001);
+ Eigen::VectorXf pos = Eigen::VectorXf::Zero(traj->dim(), 1);
+ while (t < 1.0f)
+ {
+ Eigen::VectorXf vel = c.update(deltaT, pos);
+ pos += vel * deltaT * distribution(generator);
+ ARMARX_INFO /*<< deactivateSpam(0.01)*/ << VAROUT(t) << VAROUT(pos);
+ t += deltaT;
+ }
+
+
+}