added feed forward velocity

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

@@ -54,7 +54,7 @@ void PositionControllerHelper::update()
         currentWaypointIndex++;
     }
     Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
-    velocityControllerHelper->setTargetVelocity(cv);
+    velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
 }
 
 void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
@@ -85,6 +85,22 @@ void PositionControllerHelper::setTarget(const Eigen::Matrix4f& target)
     currentWaypointIndex = 0;
 }
 
+void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity)
+{
+    this->feedForwardVelocity = feedForwardVelocity;
+}
+
+void PositionControllerHelper::setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri)
+{
+    feedForwardVelocity.block<3, 1>(0, 0) = feedForwardVelocityPos;
+    feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
+}
+
+void PositionControllerHelper::clearFeedForwardVelocity()
+{
+    feedForwardVelocity = Eigen::Vector6f::Zero();
+}
+
 float PositionControllerHelper::getPositionError() const
 {
     return posController.getPositionError(getCurrentTarget());

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

@@ -34,8 +34,15 @@
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <RobotAPI/libraries/core/Pose.h>
 
+namespace Eigen
+{
+    typedef Matrix<float, 6, 1> Vector6f;
+}
+
+
 namespace armarx
 {
+
     class PositionControllerHelper;
     typedef boost::shared_ptr<PositionControllerHelper> PositionControllerHelperPtr;
 
@@ -52,6 +59,9 @@ namespace armarx
         void setNewWaypoints(const std::vector<PosePtr>& 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;
 
@@ -82,5 +92,6 @@ namespace armarx
         float thresholdOrientationReached = 0;
         float thresholdPositionNear = 0;
         float thresholdOrientationNear = 0;
+        Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
     };
 }