CartesianPositionController: OptimizeNullspace

CartesianPositionController: autoclear feed forward velocity
CartesianVelocityController: Limit max joint velocity
and descriptive comment

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

@@ -25,6 +25,8 @@
 
 #include <ArmarXCore/core/time/TimeUtil.h>
 
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+
 using namespace armarx;
 
 PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
@@ -55,6 +57,10 @@ void PositionControllerHelper::update()
     }
     Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
     velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
+    if(autoClearFeedForward)
+    {
+        clearFeedForwardVelocity();
+    }
 }
 
 void PositionControllerHelper::setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints)
@@ -172,3 +178,59 @@ std::string PositionControllerHelper::getStatusText()
     ss << std::fixed << "Waypoint: " << (currentWaypointIndex + 1) << "/" << waypoints.size() << " distance: " << getPositionError() << " mm " << VirtualRobot::MathTools::rad2deg(getOrientationError()) << " deg";
     return ss.str();
 }
+
+bool PositionControllerHelper::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
+{
+    CartesianPositionController posHelper(tcp);
+
+    CartesianVelocityControllerPtr cartesianVelocityController;
+    cartesianVelocityController.reset(new CartesianVelocityController(rns, tcp, args.invJacMethod));
+
+    float errorOriInitial = posHelper.getOrientationError(target);
+    float errorPosInitial = posHelper.getPositionError(target);
+
+    float stepLength = args.stepLength;
+    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);
+        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/RobotStatechartHelpers/PositionControllerHelper.h

@@ -82,6 +82,19 @@ namespace armarx
 
         std::string getStatusText();
 
+        struct NullspaceOptimizationArgs
+        {
+            int loops = 100;
+            float stepLength = 0.05f;
+            float eps = 0.001;
+            float maxOriErrorIncrease = 1.f / 180 * M_PI;
+            float maxPoseErrorIncrease = 1;
+            VirtualRobot::IKSolver::CartesianSelection cartesianSelection = VirtualRobot::IKSolver::All;
+            VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod = VirtualRobot::JacobiProvider::eSVD;
+        };
+
+        static bool OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args = NullspaceOptimizationArgs());
+
         CartesianPositionController posController;
         VelocityControllerHelperPtr velocityControllerHelper;
 
@@ -93,5 +106,6 @@ namespace armarx
         float thresholdPositionNear = 0;
         float thresholdOrientationNear = 0;
         Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
+        bool autoClearFeedForward = true;
     };
 }

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -35,6 +35,7 @@ namespace armarx
     class CartesianPositionController;
     typedef boost::shared_ptr<CartesianPositionController> CartesianPositionControllerPtr;
 
+    // This is a P-controller. In: Target pose, out Cartesian velocity.
     class CartesianPositionController
     {
     public:
@@ -47,7 +48,6 @@ namespace armarx
         Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
         Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
 
-        //CartesianVelocityController velocityController;
         float KpPos = 1;
         float KpOri = 1;
         float maxPosVel = -1;

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -28,6 +28,8 @@
 
 #include <RobotAPI/libraries/core/math/MathUtils.h>
 
+#include <VirtualRobot/math/Helpers.h>
+
 using namespace armarx;
 
 CartesianVelocityController::CartesianVelocityController(const VirtualRobot::RobotNodeSetPtr& rns, const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::JacobiProvider::InverseJacobiMethod invJacMethod)
@@ -81,6 +83,12 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
 
     jointVel += nsv;
 
+
+    if(maximumJointVelocities.rows() > 0)
+    {
+        jointVel = ::math::Helpers::LimitVectorLength(jointVel, maximumJointVelocities);
+    }
+
     return jointVel;
 }
 

source/RobotAPI/libraries/core/CartesianVelocityController.h

@@ -54,6 +54,7 @@ namespace armarx
         VirtualRobot::RobotNodeSetPtr rns;
         VirtualRobot::DifferentialIKPtr ik;
         VirtualRobot::RobotNodePtr tcp;
+        Eigen::VectorXf maximumJointVelocities;
 
     private:
         Eigen::VectorXf calculateRegularization(VirtualRobot::IKSolver::CartesianSelection mode);