split update of PositionControllerHelper into read and write parts

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

@@ -52,11 +52,21 @@ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNode
 }
 
 void PositionControllerHelper::update()
+{
+    updateRead();
+    updateWrite();
+}
+
+void PositionControllerHelper::updateRead()
 {
     if (!isLastWaypoint() && isCurrentTargetNear())
     {
         currentWaypointIndex++;
     }
+}
+
+void PositionControllerHelper::updateWrite()
+{
     Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
     velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
     if (autoClearFeedForward)
@@ -109,6 +119,15 @@ void PositionControllerHelper::clearFeedForwardVelocity()
     feedForwardVelocity = Eigen::Vector6f::Zero();
 }
 
+void PositionControllerHelper::immediateHardStop(bool clearTargets)
+{
+    velocityControllerHelper->controller->immediateHardStop();
+    if(clearTargets)
+    {
+        setTarget(posController.getTcp()->getPoseInRootFrame());
+    }
+}
+
 float PositionControllerHelper::getPositionError() const
 {
     return posController.getPositionError(getCurrentTarget());

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

@@ -53,7 +53,13 @@ namespace armarx
         PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
         PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
 
+        // read data and write targets, call this if you are unsure, anywhere in your control loop.
+        // use updateRead and updateWrite for better performance
         void update();
+        // read data, call this after robot sync
+        void updateRead();
+        // write targets, call this at the end of your control loop, before the sleep
+        void updateWrite();
 
         void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
         void setNewWaypoints(const std::vector<PosePtr>& waypoints);
@@ -62,6 +68,7 @@ namespace armarx
         void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
         void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
         void clearFeedForwardVelocity();
+        void immediateHardStop(bool clearTargets = true);
 
         float getPositionError() const;
 

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -100,3 +100,8 @@ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Mat
     Eigen::AngleAxisf aa(oriDir);
     return aa.axis() * aa.angle();
 }
+
+VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+{
+    return tcp;
+}

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -47,6 +47,7 @@ namespace armarx
         float getOrientationError(const Eigen::Matrix4f& targetPose) const;
         Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
         Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
+        VirtualRobot::RobotNodePtr getTcp() const;
 
         float KpPos = 1;
         float KpOri = 1;