From f9c4d5453f783cf8f65e673c687b7b69aff8fe64 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Wed, 23 Mar 2022 13:45:29 +0100
Subject: [PATCH] update NJointCartesianWaypointController
- add setWaypointAx to access the controller from python
---
.../NJointCartesianWaypointController.cpp | 13 +++++++++++++
.../NJointCartesianWaypointController.h | 1 +
.../RobotUnit/NJointCartesianWaypointController.ice | 1 +
3 files changed, 15 insertions(+)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 4658f41d7..bd02cd045 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -312,6 +312,19 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
+ void NJointCartesianWaypointController::setWaypointAx(const Ice::FloatSeq& data, const Ice::Current&)
+ {
+ Eigen::Matrix4f wp(data.data());
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.wps.clear();
+ w.wps.emplace_back(wp);
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new waypoint\n" << wp;
+ }
+
+
void NJointCartesianWaypointController::setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
const std::vector<Eigen::Matrix4f>& wps,
const Ice::Current&)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 4933edd9d..22440abdc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -55,6 +55,7 @@ namespace armarx
void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
+ void setWaypointAx(const Ice::FloatSeq& data, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index bf00909d6..f02c8e8ee 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -56,6 +56,7 @@ module armarx
void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
void setWaypoints(Eigen::Matrix4fSeq wps);
void setWaypoint(Eigen::Matrix4f wp);
+ void setWaypointAx(Ice::FloatSeq wp);
void setConfigAndWaypoints(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
void setConfigAndWaypoint(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
--
GitLab