From 8f20b5c465734f141f61b869250d866cbca655b5 Mon Sep 17 00:00:00 2001 From: armar6-user <armar6-user@kit.edu> Date: Mon, 19 Nov 2018 11:26:22 +0100 Subject: [PATCH] convenience functions --- .../components/DebugDrawer/DebugDrawerHelper.cpp | 10 ++++++++++ .../components/DebugDrawer/DebugDrawerHelper.h | 3 +++ .../PositionControllerHelper.cpp | 9 ++++++++- .../RobotStatechartHelpers/PositionControllerHelper.h | 3 ++- 4 files changed, 23 insertions(+), 2 deletions(-) diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp index a4ee8ad72..c1bb72439 100644 --- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp +++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp @@ -75,6 +75,11 @@ void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& debugDrawerPrx->setTextVisu(layerName, name, text, makeGlobal(p1), color, size); } +void DebugDrawerHelper::drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width) +{ + debugDrawerPrx->setArrowVisu(layerName, name, makeGlobal(pos), makeGlobalDirection(direction), color, length, width); +} + PosePtr DebugDrawerHelper::makeGlobal(const Eigen::Matrix4f& pose) { return new Pose(rn->getGlobalPose(pose)); @@ -84,3 +89,8 @@ Vector3Ptr DebugDrawerHelper::makeGlobal(const Eigen::Vector3f& position) { return new Vector3(rn->getGlobalPosition(position)); } + +Vector3Ptr DebugDrawerHelper::makeGlobalDirection(const Eigen::Vector3f& direction) +{ + return new Vector3(math::Helpers::TransformDirection(rn->getGlobalPose(), direction)); +} diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h index 3bdaa4933..5d95dd9c8 100644 --- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h +++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h @@ -59,8 +59,11 @@ namespace armarx void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size); + void drawArrow(const std::string& name, const Eigen::Vector3f& pos, const Eigen::Vector3f& direction, const DrawColor& color, float length, float width); + PosePtr makeGlobal(const Eigen::Matrix4f& pose); Vector3Ptr makeGlobal(const Eigen::Vector3f& position); + Vector3Ptr makeGlobalDirection(const Eigen::Vector3f& direction); Defaults defaults; diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp index 629d427a6..a54cf2fe8 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp +++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp @@ -27,6 +27,8 @@ #include <RobotAPI/libraries/core/CartesianVelocityController.h> +#include <VirtualRobot/math/Helpers.h> + using namespace armarx; PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const Eigen::Matrix4f& target) @@ -57,7 +59,7 @@ void PositionControllerHelper::update() } Eigen::VectorXf cv = posController.calculate(getCurrentTarget(), VirtualRobot::IKSolver::All); velocityControllerHelper->setTargetVelocity(cv + feedForwardVelocity); - if(autoClearFeedForward) + if (autoClearFeedForward) { clearFeedForwardVelocity(); } @@ -147,6 +149,11 @@ const Eigen::Matrix4f& PositionControllerHelper::getCurrentTarget() const return waypoints.at(currentWaypointIndex); } +const Eigen::Vector3f PositionControllerHelper::getCurrentTargetPosition() const +{ + return math::Helpers::GetPosition(waypoints.at(currentWaypointIndex)); +} + size_t PositionControllerHelper::skipToClosestWaypoint(float rad2mmFactor) { float dist = FLT_MAX; diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h index 92d20fa31..b90b2a615 100644 --- a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h +++ b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h @@ -75,6 +75,7 @@ namespace armarx bool isLastWaypoint() const; const Eigen::Matrix4f& getCurrentTarget() const; + const Eigen::Vector3f getCurrentTargetPosition() const; size_t skipToClosestWaypoint(float rad2mmFactor); @@ -84,7 +85,7 @@ namespace armarx struct NullspaceOptimizationArgs { - NullspaceOptimizationArgs(){} + NullspaceOptimizationArgs() {} int loops = 100; float stepLength = 0.05f; float eps = 0.001; -- GitLab