diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp index 15d53995d75eab81a2179c5982e93704fc4be8cc..a0fd892e9a748e84e6af30841e6eb22f7a36b542 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 915d9d3e3212fba4cdb2b009478f868dcb549040..b1ccef061e7bd34e0bc6f9eaa7809b79783da32f 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 22d7b732fcb966d797cf93184aad3b862e22abe1..82caa574643aa2302f0f88a9278f38343e14d28f 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 07876699976dd9445d4405a3efe559cbdb2b33bc..b2f2ce30e0341a1a17ce6c4c1484af9743300b74 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; }; }