diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp index 4658f41d72431535920580dbb6bda4e0c70c0b6e..bd02cd045bb37da2a51156a60fec1b35b61a2dbf 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 4933edd9d22e5138a7ef06d09fa15aa4f908e339..22440abdc4123b86e5ec28e07bfb396b6102dfe2 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 bf00909d6f5b95ee23fec2f982aae5d4d6ed6ec8..f02c8e8ee8720f7a45fb730f155986016cdb0331 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);