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

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

 /*
  * 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);
 }

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

 /*
  * 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;
 

source/RobotAPI/libraries/core/CartesianVelocityController.cpp

@@ -58,21 +58,21 @@ Eigen::VectorXf CartesianVelocityController::calculate(const Eigen::VectorXf& ca
     inv = ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
 
     //    ARMARX_IMPORTANT << "nullspaceVel " << nullspaceVel.transpose();
-    //    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
+    Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
     //ARMARX_IMPORTANT << "The rank of the jacobi is " << lu_decomp.rank();
     //ARMARX_IMPORTANT << "Null space: " << lu_decomp.kernel();
-    //    Eigen::MatrixXf nullspace = lu_decomp.kernel();
+    Eigen::MatrixXf nullspace = lu_decomp.kernel();
 
 
-    Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
+    //    Eigen::MatrixXf nullspace = Eigen::MatrixXf::Identity(jacobi.cols(), jacobi.cols()) - inv * jacobi;
 
-    //    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-    //    for (int i = 0; i < nullspace.cols(); i++)
-    //    {
-    //        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
-    //    }
+    Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
+    for (int i = 0; i < nullspace.cols(); i++)
+    {
+        nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) / nullspace.col(i).squaredNorm();
+    }
 
-    Eigen::VectorXf nsv = nullspace * nullspaceVel;
+    //    Eigen::VectorXf nsv = nullspace * nullspaceVel;
 
     //nsv /= kernel.cols();
 

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp

@@ -45,22 +45,13 @@ void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::V
     Eigen::MatrixXf nullspace = lu_decomp.kernel();
     Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
 
-
-    //        int r = lu_decomp.rank();
-
-    //        Eigen::MatrixXf nullspace = Eigen::MatrixXf::Zero(r, V.cols());
-    //        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
-
-
-    //    for (int i = 0; i < nullspace.cols(); i++)
-    //    {
-    //        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
-    //    }
-
+    for (int i = 0; i < nullspace.cols(); i++)
+    {
+        nsv += nullspace.col(i) * nullspace.col(i).dot(currentJointVelocity) / nullspace.col(i).squaredNorm();
+    }
     cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
-    //    nullSpaceVelocityRamp.setState(nsv);
-    nullSpaceVelocityRamp.setState(currentJointVelocity - inv * jacobi * currentJointVelocity);
-    //    ARMARX_IMPORTANT << "nsv " << currentJointVelocity - inv* jacobi* currentJointVelocity;
+    nullSpaceVelocityRamp.setState(nsv);
+
 }
 
 Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)