diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index de555d6f62ccbc533487c91e37a55440947b1f7f..824c8063e03b459c1eeabd706577e7122284099e 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -42,6 +42,7 @@ set(LIB_FILES
math/TimeSeriesUtils.cpp
CartesianVelocityController.cpp
CartesianPositionController.cpp
+ CartesianWaypointController.cpp
CartesianFeedForwardPositionController.cpp
CartesianVelocityRamp.cpp
JointVelocityRamp.cpp
@@ -85,6 +86,7 @@ set(LIB_HEADERS
math/TimeSeriesUtils.h
CartesianVelocityController.h
CartesianPositionController.h
+ CartesianWaypointController.h
CartesianFeedForwardPositionController.h
CartesianVelocityRamp.h
JointVelocityRamp.h
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b7f29e761f5df89151b333a61a026b64f59b287
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -0,0 +1,259 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, 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/>.
+ *
+ * @author Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
+#include "CartesianWaypointController.h"
+
+namespace armarx
+{
+ CartesianWaypointController::CartesianWaypointController(
+ const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp
+ ) :
+ ctrlCartesianVelWithRamps
+ {
+ rns,
+ currentJointVelocity,
+ VirtualRobot::IKSolver::CartesianSelection::All,
+ maxPositionAcceleration,
+ maxOrientationAcceleration,
+ maxNullspaceAcceleration,
+ tcp ? tcp : rns->getTCP()
+ },
+ ctrlCartesianPos2Vel(tcp ? tcp : rns->getTCP()),
+ currentWaypointIndex(0)
+ {
+ ARMARX_CHECK_NOT_NULL(rns);
+ _out = Eigen::VectorXf::Zero(rns->getSize());
+ _jnv = Eigen::VectorXf::Zero(rns->getSize());
+ }
+
+ const Eigen::VectorXf& CartesianWaypointController::calculate(float dt)
+ {
+ //calculate cartesian velocity + some management stuff
+ if (!isLastWaypoint() && isCurrentTargetNear())
+ {
+ currentWaypointIndex++;
+ }
+ cartesianVelocity = ctrlCartesianPos2Vel.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All) + feedForwardVelocity;
+
+ if (autoClearFeedForward)
+ {
+ clearFeedForwardVelocity();
+ }
+
+ //calculate joint velocity
+ if (nullSpaceControlEnabled)
+ {
+ //avoid joint limits
+ _jnv = KpJointLimitAvoidance * ctrlCartesianVelWithRamps.controller.calculateJointLimitAvoidance() +
+ ctrlCartesianVelWithRamps.controller.calculateNullspaceVelocity(
+ cartesianVelocity,
+ jointLimitAvoidanceScale,
+ VirtualRobot::IKSolver::All
+ );
+ }
+ else
+ {
+ //don't avoid joint limits
+ _jnv *= 0;
+ }
+ _out = ctrlCartesianVelWithRamps.calculate(cartesianVelocity, _jnv, dt);
+ return _out;
+ }
+
+ void CartesianWaypointController::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+ {
+ this->waypoints = waypoints;
+ currentWaypointIndex = 0;
+ }
+
+ void CartesianWaypointController::addWaypoint(const Eigen::Matrix4f& waypoint)
+ {
+ this->waypoints.push_back(waypoint);
+ }
+
+ void CartesianWaypointController::setTarget(const Eigen::Matrix4f& target)
+ {
+ waypoints.clear();
+ waypoints.push_back(target);
+ currentWaypointIndex = 0;
+ }
+
+ void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+ {
+ this->feedForwardVelocity = feedForwardVelocity;
+ }
+
+ void CartesianWaypointController::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+ {
+ feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+ feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+ }
+
+ void CartesianWaypointController::clearFeedForwardVelocity()
+ {
+ feedForwardVelocity = Eigen::Vector6f::Zero();
+ }
+
+ float CartesianWaypointController::getPositionError() const
+ {
+ return ctrlCartesianPos2Vel.getPositionError(getCurrentTarget());
+ }
+
+ float CartesianWaypointController::getOrientationError() const
+ {
+ return ctrlCartesianPos2Vel.getOrientationError(getCurrentTarget());
+ }
+
+ bool CartesianWaypointController::isCurrentTargetReached() const
+ {
+ return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
+ }
+
+ bool CartesianWaypointController::isCurrentTargetNear() const
+ {
+ return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
+ }
+
+ bool CartesianWaypointController::isFinalTargetReached() const
+ {
+ return isLastWaypoint() && isCurrentTargetReached();
+ }
+
+ bool CartesianWaypointController::isFinalTargetNear() const
+ {
+ return isLastWaypoint() && isCurrentTargetNear();
+ }
+
+ bool CartesianWaypointController::isLastWaypoint() const
+ {
+ return currentWaypointIndex + 1 >= waypoints.size();
+ }
+
+ const Eigen::Matrix4f& CartesianWaypointController::getCurrentTarget() const
+ {
+ return waypoints.at(currentWaypointIndex);
+ }
+
+ const Eigen::Vector3f CartesianWaypointController::getCurrentTargetPosition() const
+ {
+ return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex));
+ }
+
+ size_t CartesianWaypointController::skipToClosestWaypoint(float rad2mmFactor)
+ {
+ float dist = FLT_MAX;
+ size_t minIndex = 0;
+ for (size_t i = 0; i < waypoints.size(); i++)
+ {
+ float posErr = ctrlCartesianPos2Vel.getPositionError(waypoints.at(i));
+ float oriErr = ctrlCartesianPos2Vel.getOrientationError(waypoints.at(i));
+ float d = posErr + oriErr * rad2mmFactor;
+ if (d < dist)
+ {
+ minIndex = i;
+ dist = d;
+ }
+ }
+ currentWaypointIndex = minIndex;
+ return currentWaypointIndex;
+ }
+
+ void CartesianWaypointController::setNullSpaceControl(bool enabled)
+ {
+ nullSpaceControlEnabled = enabled;
+ }
+
+ std::string CartesianWaypointController::getStatusText()
+ {
+ std::stringstream ss;
+ ss.precision(2);
+ ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
+ return ss.str();
+ }
+
+ // bool CartesianWaypointController::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
+ // {
+ // CartesianPositionController posHelper(tcp);
+
+ // CartesianVelocityController cartesianVelocityController{rns, tcp, args.invJacMethod};
+
+ // const float errorOriInitial = posHelper.getOrientationError(target);
+ // const float errorPosInitial = posHelper.getPositionError(target);
+
+ // const float stepLength = args.stepLength;
+ // const float eps = args.eps;
+
+ // std::vector<float> initialJointAngles = rns->getJointValues();
+
+ // for (int i = 0; i < args.loops; i++)
+ // {
+ // Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
+ // Eigen::VectorXf nullspaceVel = cartesianVelocityController.calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
+ // const float nullspaceLen = nullspaceVel.norm();
+ // if (nullspaceLen > stepLength)
+ // {
+ // nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
+ // }
+ // Eigen::VectorXf jointVelocities = cartesianVelocityController.calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
+
+ // //jointVelocities = jointVelocities * stepLength;
+ // float len = jointVelocities.norm();
+ // if (len > stepLength)
+ // {
+ // jointVelocities = jointVelocities / len * stepLength;
+ // }
+
+ // for (size_t n = 0; n < rns->getSize(); n++)
+ // {
+ // rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
+ // }
+ // if (len < eps)
+ // {
+ // break;
+ // }
+ // }
+
+ // float errorOriAfter = posHelper.getOrientationError(target);
+ // float errorPosAfter = posHelper.getPositionError(target);
+ // if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
+ // {
+ // return true;
+ // }
+ // else
+ // {
+ // rns->setJointValues(initialJointAngles);
+ // return false;
+ // }
+ // }
+}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.h b/source/RobotAPI/libraries/core/CartesianWaypointController.h
new file mode 100644
index 0000000000000000000000000000000000000000..c42772fcab483141672a963c2ed97c36aa78b984
--- /dev/null
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.h
@@ -0,0 +1,119 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, 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/>.
+ *
+ * @author Raphael Grimm (raphael dot grimm at kit dot edu)
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <memory>
+
+#include <Eigen/Dense>
+
+#include <VirtualRobot/Robot.h>
+
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include "CartesianVelocityControllerWithRamp.h"
+
+namespace Eigen
+{
+ using Vector6f = Matrix<float, 6, 1>;
+}
+
+
+namespace armarx
+{
+
+ class CartesianWaypointController;
+ using CartesianWaypointControllerPtr = std::shared_ptr<CartesianWaypointController>;
+
+ class CartesianWaypointController
+ {
+ public:
+ CartesianWaypointController(const VirtualRobot::RobotNodeSetPtr& rns,
+ const Eigen::VectorXf& currentJointVelocity,
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration,
+ float maxNullspaceAcceleration,
+ const VirtualRobot::RobotNodePtr& tcp = nullptr
+ );
+
+
+ void setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
+ {
+ ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
+ }
+
+ const Eigen::VectorXf& calculate(float dt);
+
+ void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+ void addWaypoint(const Eigen::Matrix4f& waypoint);
+ void setTarget(const Eigen::Matrix4f& target);
+ void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
+ void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void clearFeedForwardVelocity();
+
+ float getPositionError() const;
+
+ float getOrientationError() const;
+
+ bool isCurrentTargetReached() const;
+ bool isCurrentTargetNear() const;
+ bool isFinalTargetReached() const;
+ bool isFinalTargetNear() const;
+
+ bool isLastWaypoint() const;
+
+ const Eigen::Matrix4f& getCurrentTarget() const;
+ const Eigen::Vector3f getCurrentTargetPosition() const;
+
+ size_t skipToClosestWaypoint(float rad2mmFactor);
+
+ void setNullSpaceControl(bool enabled);
+
+ std::string getStatusText();
+
+ CartesianVelocityControllerWithRamp ctrlCartesianVelWithRamps;
+ CartesianPositionController ctrlCartesianPos2Vel;
+
+ std::vector<Eigen::Matrix4f> waypoints;
+ size_t currentWaypointIndex = 0.f;
+
+ float thresholdPositionReached = 0.f;
+ float thresholdOrientationReached = 0.f;
+ float thresholdPositionNear = 0.f;
+ float thresholdOrientationNear = 0.f;
+
+ Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
+ Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
+ bool autoClearFeedForward = true;
+
+
+ bool nullSpaceControlEnabled = true;
+ float jointLimitAvoidanceScale = 0.f;
+ float KpJointLimitAvoidance = 0.f;
+
+ private:
+ Eigen::VectorXf _out;
+ Eigen::VectorXf _jnv;
+ };
+}