From 076545257e37ac7de7ba5a5c9973b6c878cfe221 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 10 May 2019 14:49:56 +0200
Subject: [PATCH] split update of PositionControllerHelper into read and write
parts
---
.../PositionControllerHelper.cpp | 19 +++++++++++++++++++
.../PositionControllerHelper.h | 7 +++++++
.../core/CartesianPositionController.cpp | 5 +++++
.../core/CartesianPositionController.h | 1 +
4 files changed, 32 insertions(+)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
index a54cf2fe8..9b9d1e5f9 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp
@@ -52,11 +52,21 @@ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNode
}
void PositionControllerHelper::update()
+{
+ updateRead();
+ updateWrite();
+}
+
+void PositionControllerHelper::updateRead()
{
if (!isLastWaypoint() && isCurrentTargetNear())
{
currentWaypointIndex++;
}
+}
+
+void PositionControllerHelper::updateWrite()
+{
Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All);
velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity);
if (autoClearFeedForward)
@@ -109,6 +119,15 @@ void PositionControllerHelper::clearFeedForwardVelocity()
feedForwardVelocity = Eigen::Vector6f::Zero();
}
+void PositionControllerHelper::immediateHardStop(bool clearTargets)
+{
+ velocityControllerHelper->controller->immediateHardStop();
+ if(clearTargets)
+ {
+ setTarget(posController.getTcp()->getPoseInRootFrame());
+ }
+}
+
float PositionControllerHelper::getPositionError() const
{
return posController.getPositionError(getCurrentTarget());
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
index e4012fbb5..afda18b69 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h
@@ -53,7 +53,13 @@ namespace armarx
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);
+ // read data and write targets, call this if you are unsure, anywhere in your control loop.
+ // use updateRead and updateWrite for better performance
void update();
+ // read data, call this after robot sync
+ void updateRead();
+ // write targets, call this at the end of your control loop, before the sleep
+ void updateWrite();
void setNewWaypoints(const std::vector<Eigen::Matrix4f>& waypoints);
void setNewWaypoints(const std::vector<PosePtr>& waypoints);
@@ -62,6 +68,7 @@ namespace armarx
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
void clearFeedForwardVelocity();
+ void immediateHardStop(bool clearTargets = true);
float getPositionError() const;
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 4559e139c..f6f9dc70a 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -100,3 +100,8 @@ Eigen::Vector3f CartesianPositionController::getOrientationDiff(const Eigen::Mat
Eigen::AngleAxisf aa(oriDir);
return aa.axis() * aa.angle();
}
+
+VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
+{
+ return tcp;
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 2c4b46c5c..d4921c62a 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -47,6 +47,7 @@ namespace armarx
float getOrientationError(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getPositionDiff(const Eigen::Matrix4f& targetPose) const;
Eigen::Vector3f getOrientationDiff(const Eigen::Matrix4f& targetPose) const;
+ VirtualRobot::RobotNodePtr getTcp() const;
float KpPos = 1;
float KpOri = 1;
--
GitLab