Add CartesianWaypointController

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

source/RobotAPI/libraries/core/CartesianWaypointController.cpp

+/*
+ * 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;
+    //        }
+    //    }
+}

source/RobotAPI/libraries/core/CartesianWaypointController.h

+/*
+ * 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;
+    };
+}