From b849d975ce557b2b704e9bf09ba951c6a5ca0c7b Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Tue, 24 Jul 2018 14:05:22 +0200
Subject: [PATCH] added skipToClosestWaypoint
---
.../PositionControllerHelper.cpp | 39 +++++++++++++++++--
.../PositionControllerHelper.h | 5 +++
.../VelocityControllerHelper.cpp | 22 +++++++++--
.../VelocityControllerHelper.h | 6 ++-
4 files changed, 63 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index 15d53995d..a0fd892e9 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/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);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index 915d9d3e3..b1ccef061 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index 22d7b732f..82caa5746 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -1,6 +1,6 @@
/*
* 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();
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
index 078766999..b2f2ce30e 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h
@@ -1,6 +1,6 @@
/*
* 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;
};
}
--
GitLab