Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI

source/RobotAPI/libraries/core/CartesianWaypointController.cpp

@@ -92,12 +92,18 @@ namespace armarx
         return _out;
     }
 
-    void CartesianWaypointController::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
+    void CartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
     {
         this->waypoints = waypoints;
         currentWaypointIndex = 0;
     }
 
+    void CartesianWaypointController::swapWaypoints(std::vector<Eigen::Matrix4f>& waypoints)
+    {
+        std::swap(this->waypoints, waypoints);
+        currentWaypointIndex = 0;
+    }
+
     void CartesianWaypointController::addWaypoint(const Eigen::Matrix4f& waypoint)
     {
         this->waypoints.push_back(waypoint);

source/RobotAPI/libraries/core/CartesianWaypointController.h

@@ -65,7 +65,8 @@ namespace armarx
 
         const Eigen::VectorXf& calculate(float dt);
 
-        void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+        void setWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
+        void swapWaypoints(std::vector<Eigen::Matrix4f>& waypoints);
         void addWaypoint(const Eigen::Matrix4f& waypoint);
         void setTarget(const Eigen::Matrix4f& target);
         void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);

source/RobotAPI/libraries/core/SimpleDiffIK.h

@@ -86,6 +86,7 @@ namespace armarx
 
         static Result CalculateDiffIK(const Eigen::Matrix4f targetPose, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
 
+        ///@brief Use this to check a trajectory of waypoints
         static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
 
     };