added skipToClosestWaypoint

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

@@ -28,16 +28,23 @@
 using namespace armarx;
 
 PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target)
-    : posController(tcp), velocityControllerHelper(velocityControllerHelper)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
 {
     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)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper), waypoints(waypoints), currentWaypointIndex(0)
 {
-    currentWaypointIndex = 0;
+}
+
+PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints)
+    : posController(tcp), velocityControllerHelper(velocityControllerHelper), currentWaypointIndex(0)
+{
+    for (const PosePtr& pose : waypoints)
+    {
+        this->waypoints.push_back(pose->toEigen());
+    }
 }
 
 void PositionControllerHelper::update()
@@ -117,3 +124,27 @@ const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const
 {
     return waypoints.at(currentWaypointIndex);
 }
+
+size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor)
+{
+    float dist = FLT_MAX;
+    size_t minIndex = 0;
+    for (size_t i = 0; i < waypoints.size(); i++)
+    {
+        float posErr = posController.getPositionError(waypoints.at(i));
+        float oriErr = posController.getOrientationError(waypoints.at(i));
+        float d = posErr + oriErr * rad2mmFactor;
+        if (d < dist)
+        {
+            minIndex = i;
+            dist = d;
+        }
+    }
+    currentWaypointIndex = minIndex;
+    return currentWaypointIndex;
+}
+
+void PositionControllerHelper::setNullSpaceControl(bool enabled)
+{
+    velocityControllerHelper->setNullSpaceControl(enabled);
+}

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

@@ -44,6 +44,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);
+        PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
 
         void update();
 
@@ -65,6 +66,10 @@ namespace armarx
 
         const Eigen::Matrix4f& getCurrentTarget() const;
 
+        size_t skipToClosestWaypoint(float rad2mmFactor);
+
+        void setNullSpaceControl(bool enabled);
+
         CartesianPositionController posController;
         VelocityControllerHelperPtr velocityControllerHelper;
 

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp

 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -28,14 +28,14 @@
 using namespace armarx;
 
 
-VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, const std::string &nodeSetName, const std::string &controllerName)
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, const std::string& nodeSetName, const std::string& controllerName)
     : robotUnit(robotUnit), controllerName(controllerName)
 {
     config = new NJointCartesianVelocityControllerWithRampConfig(nodeSetName, "", CartesianSelectionMode::eAll, 500, 1, 2, 1, 2);
     init();
 }
 
-VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx &robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string &controllerName)
+VelocityControllerHelper::VelocityControllerHelper(const RobotUnitInterfacePrx& robotUnit, NJointCartesianVelocityControllerWithRampConfigPtr config, const std::string& controllerName)
     : robotUnit(robotUnit), controllerName(controllerName)
 {
     this->config = config;
@@ -58,11 +58,25 @@ void VelocityControllerHelper::init()
     controller->activateController();
 }
 
-void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf &cv)
+void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
 {
     controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
 }
 
+void VelocityControllerHelper::setNullSpaceControl(bool enabled)
+{
+    if (enabled)
+    {
+        controller->setJointLimitAvoidanceScale(config->jointLimitAvoidanceScale);
+        controller->setKpJointLimitAvoidance(config->KpJointLimitAvoidance);
+    }
+    else
+    {
+        controller->setJointLimitAvoidanceScale(0);
+        controller->setKpJointLimitAvoidance(0);
+    }
+}
+
 void VelocityControllerHelper::cleanup()
 {
     controller->deactivateController();

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h

 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -45,6 +45,8 @@ namespace armarx
 
         void setTargetVelocity(const Eigen::VectorXf& cv);
 
+        void setNullSpaceControl(bool enabled);
+
         void cleanup();
 
         NJointCartesianVelocityControllerWithRampConfigPtr config;
@@ -52,5 +54,7 @@ namespace armarx
         RobotUnitInterfacePrx robotUnit;
         std::string controllerName;
         bool controllerCreated = false;
+
+        float initialKp;
     };
 }