From c523a5366ca7c2e68ac777e397578ad0b3007edd Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Thu, 5 Jul 2018 17:27:35 +0200
Subject: [PATCH] added constructor with waypoints

---
 .../PositionControllerHelper.cpp              | 32 +++++++++++--------
 .../PositionControllerHelper.h                |  7 ++--
 2 files changed, 23 insertions(+), 16 deletions(-)

diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 229077ce6..15d53995d 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -1,6 +1,6 @@
 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -27,16 +27,22 @@
 
 using namespace armarx;
 
-PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr &tcp, const VelocityControllerHelperPtr &velocityControllerHelper, const Eigen::Matrix4f &target)
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
     : posController(tcp), velocityControllerHelper(velocityControllerHelper)
 {
     waypoints.push_back(target);
     currentWaypointIndex = 0;
 }
 
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints)
+{
+    currentWaypointIndex = 0;
+}
+
 void PositionControllerHelper::update()
 {
-    if(!isLastWaypoint() && isTargetNear())
+    if (!isLastWaypoint() && isCurrentTargetNear())
     {
         currentWaypointIndex++;
     }
@@ -44,28 +50,28 @@ void PositionControllerHelper::update()
     velocityControllerHelper->setTargetVelocity(cv);
 }
 
-void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f> &waypoints)
+void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
 {
     this->waypoints = waypoints;
     currentWaypointIndex = 0;
 }
 
-void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr> &waypoints)
+void PositionControllerHelper::setNewWaypoints(const std::vector<PosePtr>& waypoints)
 {
     this->waypoints.clear();
-    for(const PosePtr& pose : waypoints)
+    for (const PosePtr& pose : waypoints)
     {
         this->waypoints.push_back(pose->toEigen());
     }
     currentWaypointIndex = 0;
 }
 
-void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f &waypoint)
+void PositionControllerHelper::addWaypoint(const Eigen::Matrix4f& waypoint)
 {
     this->waypoints.push_back(waypoint);
 }
 
-void PositionControllerHelper::setTarget(const Eigen::Matrix4f &target)
+void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
 {
     waypoints.clear();
     waypoints.push_back(target);
@@ -82,24 +88,24 @@ float PositionControllerHelper::getOrientationError() const
     return posController.getOrientationError(getCurrentTarget());
 }
 
-bool PositionControllerHelper::isTargetReached() const
+bool PositionControllerHelper::isCurrentTargetReached() const
 {
     return getPositionError() < thresholdPositionReached && getOrientationError() < thresholdOrientationReached;
 }
 
-bool PositionControllerHelper::isTargetNear() const
+bool PositionControllerHelper::isCurrentTargetNear() const
 {
     return getPositionError() < thresholdPositionNear && getOrientationError() < thresholdOrientationNear;
 }
 
 bool PositionControllerHelper::isFinalTargetReached() const
 {
-    return isLastWaypoint() && isTargetReached();
+    return isLastWaypoint() && isCurrentTargetReached();
 }
 
 bool PositionControllerHelper::isFinalTargetNear() const
 {
-    return isLastWaypoint() && isTargetNear();
+    return isLastWaypoint() && isCurrentTargetNear();
 }
 
 bool PositionControllerHelper::isLastWaypoint() const
@@ -107,7 +113,7 @@ bool PositionControllerHelper::isLastWaypoint() const
     return currentWaypointIndex + 1 >= waypoints.size();
 }
 
-const Eigen::Matrix4f &PositionControllerHelper::getCurrentTarget() const
+const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
 {
     return waypoints.at(currentWaypointIndex);
 }
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index cedc9b540..915d9d3e3 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -1,6 +1,6 @@
 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -43,6 +43,7 @@ namespace armarx
     {
     public:
         PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target);
+        PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
 
         void update();
 
@@ -55,8 +56,8 @@ namespace armarx
 
         float getOrientationError() const;
 
-        bool isTargetReached() const;
-        bool isTargetNear() const;
+        bool isCurrentTargetReached() const;
+        bool isCurrentTargetNear() const;
         bool isFinalTargetReached() const;
         bool isFinalTargetNear() const;
 
-- 
GitLab