From 6519206d6baf25151c83cdfcd3bb791a939a8eda Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 3 Mar 2023 20:32:59 +0100
Subject: [PATCH] reach pre grasp skill + minor cleanup
---
.../Component.cpp | 50 ++-
.../Component.cpp | 9 +-
.../geometric_planning/ArVizDrawer.cpp | 39 +--
.../geometric_planning/CMakeLists.txt | 2 +
.../manipulation/geometric_planning/util.cpp | 105 ++++++
.../manipulation/geometric_planning/util.h | 33 +-
.../skills/ApproachHandleDirect.cpp | 272 ----------------
.../armarx/manipulation/skills/CMakeLists.txt | 5 +-
.../armarx/manipulation/skills/ReachGrasp.cpp | 301 ++++++++++++++++++
.../{ApproachHandleDirect.h => ReachGrasp.h} | 27 +-
.../skills/aron/ReachGraspParams.xml | 33 ++
source/armarx/manipulation/skills/constants.h | 3 +-
12 files changed, 499 insertions(+), 380 deletions(-)
create mode 100644 source/armarx/manipulation/geometric_planning/util.cpp
delete mode 100644 source/armarx/manipulation/skills/ApproachHandleDirect.cpp
create mode 100644 source/armarx/manipulation/skills/ReachGrasp.cpp
rename source/armarx/manipulation/skills/{ApproachHandleDirect.h => ReachGrasp.h} (78%)
create mode 100644 source/armarx/manipulation/skills/aron/ReachGraspParams.xml
diff --git a/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp b/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp
index 965dd127..01af8f05 100644
--- a/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp
+++ b/source/armarx/manipulation/components/articulated_object_augmented_visualization/Component.cpp
@@ -94,11 +94,11 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
const auto descriptions =
virtualRobotReaderPlugin->get().queryDescriptions(Clock::Now());
- ARMARX_INFO << "Found the following robot descriptions: ";
+ ARMARX_VERBOSE << "Found the following robot descriptions: ";
for (const auto& description : descriptions)
{
- ARMARX_INFO << "Loading robot `" << description.name << "` from "
- << description.xml;
+ ARMARX_VERBOSE << "Loading robot `" << description.name << "` from "
+ << description.xml;
const auto robot = VirtualRobot::RobotIO::loadRobot(
description.xml.toSystemPath(),
VirtualRobot::RobotIO::RobotDescription::eStructure);
@@ -111,7 +111,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
}
}
- ARMARX_INFO << "Visualizing paths for nodes";
+ ARMARX_VERBOSE << "Visualizing paths for nodes";
Metronome metronome(Frequency::Hertz(2));
while (true)
{
@@ -148,11 +148,11 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
// const auto descriptions = articulatedObjectReader.queryDescriptions(armem::Time::Now());
- // ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ // ARMARX_VERBOSE << "Found " << descriptions.size() << " articulated object descriptions";
// for (const auto& description : descriptions)
// {
- // ARMARX_INFO << "- " << description.name;
+ // ARMARX_VERBOSE << "- " << description.name;
// }
// std::vector<VirtualRobot::RobotPtr> articulatedObjects;
@@ -181,10 +181,10 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
const auto objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
const auto descriptions = articulatedObjectReader.queryDescriptions(armem::Time::Now());
- ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+ ARMARX_VERBOSE << "Found " << descriptions.size() << " articulated object descriptions";
for (const auto& description : descriptions)
{
- ARMARX_INFO << "- " << description.name;
+ ARMARX_VERBOSE << "- " << description.name;
}
std::vector<armarx::objpose::ObjectPose> articulatedObjectPoses;
@@ -205,12 +205,12 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
}
- ARMARX_INFO << "Found " << articulatedObjectPoses.size()
+ ARMARX_VERBOSE << "Found " << articulatedObjectPoses.size()
<< " articulated objects in the scene";
for (const auto& objectPose : articulatedObjectPoses)
{
- ARMARX_INFO << "- " << objectPose.objectID;
+ ARMARX_VERBOSE << "- " << objectPose.objectID;
}
const auto now = Clock::Now();
@@ -299,12 +299,11 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
auto layer = arviz.layer("grasps " + articulatedObject->getType() + "/" +
articulatedObject->getName());
- for (const auto& node : articulatedObject->getRobotNodes())
+ for (const auto& [node, graspSets] : geometric_planning::allGraspSets(*articulatedObject))
{
- const auto graspSets = node->getAllGraspSets();
if (not graspSets.empty())
{
- ARMARX_IMPORTANT << "Found grasp sets attached to node " << node->getName();
+ ARMARX_VERBOSE << "Found grasp sets attached to node " << node->getName();
for (const auto& graspSet : graspSets)
{
for (const auto& grasp : graspSet->getGrasps())
@@ -312,7 +311,7 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
const std::string side = graspSet->getName();
ARMARX_TRACE;
- ARMARX_INFO << "- " << grasp->getName();
+ ARMARX_VERBOSE << "- " << grasp->getName();
drawGrasp(side, grasp, core::Pose(node->getGlobalPose()), layer);
}
}
@@ -320,7 +319,6 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
}
ARMARX_TRACE;
- ARMARX_INFO << VAROUT(layer.size());
arviz.commit(layer);
}
}
@@ -350,16 +348,14 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
ARMARX_TRACE;
- struct Foo
+ struct Info
{
Eigen::Isometry3f tcp_T_hand_root;
std::map<std::string, float> jointValues;
-
};
- // FIXME for both left and right!
- const Foo foo = [&]
- (){
+ const Info info = [&]()
+ {
ARMARX_TRACE;
const armarx::RobotNameHelper::Arm arm = robotNameHelper->getArm(side);
@@ -372,16 +368,16 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
const auto eef = robot->getEndEffector(eefName);
- ARMARX_INFO << "preshape: " << grasp->getPreshapeName();
+ ARMARX_VERBOSE << "preshape: " << grasp->getPreshapeName();
const auto preshape = eef->getPreshape(grasp->getPreshapeName());
- Foo foo;
+ Info foo;
if (preshape != nullptr)
{
foo.jointValues = preshape->getRobotNodeJointValueMap();
- ARMARX_INFO << "preshape with joint values " << foo.jointValues;
+ ARMARX_VERBOSE << "preshape with joint values " << foo.jointValues;
}
ARMARX_TRACE;
@@ -394,10 +390,6 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
}();
ARMARX_TRACE;
- // FIXME this does not work. weak_ptr exception
- // const auto vizElement = viz::RobotHand(grasp->getName())
- // .fileBySide("Right", robotNameHelper)
- // .tcpPose(global_T_tcp, robot);
std::string modelFile = "armar6_rt/robotmodel/Armar6-SH/Armar6-" + side + "Hand-v3.xml";
@@ -406,8 +398,8 @@ namespace armarx::manipulation::components::articulated_object_augmented_visuali
const auto vizElement =
viz::Robot(side + "/" + grasp->getName())
.file("armar6_rt", modelFile)
- .pose(Eigen::Matrix4f{(global_T_tcp * foo.tcp_T_hand_root).matrix()})
- .joints(foo.jointValues)
+ .pose(Eigen::Matrix4f{(global_T_tcp * info.tcp_T_hand_root).matrix()})
+ .joints(info.jointValues)
.overrideColor(simox::Color::azure());
ARMARX_TRACE;
diff --git a/source/armarx/manipulation/components/articulated_objects_skill_provider/Component.cpp b/source/armarx/manipulation/components/articulated_objects_skill_provider/Component.cpp
index 37db9d32..c75696b4 100644
--- a/source/armarx/manipulation/components/articulated_objects_skill_provider/Component.cpp
+++ b/source/armarx/manipulation/components/articulated_objects_skill_provider/Component.cpp
@@ -42,7 +42,7 @@
#include "armarx/manipulation/skills/meta/OpenFridge.h"
#include <armarx/manipulation/core/aron/Hand.aron.generated.h>
#include <armarx/manipulation/skills/ApproachHandle.h>
-#include <armarx/manipulation/skills/ApproachHandleDirect.h>
+#include <armarx/manipulation/skills/ReachGrasp.h>
#include <armarx/manipulation/skills/SetJointValues.h>
#include <armarx/manipulation/skills/meta/OpenDishwasherDoor.h>
@@ -114,7 +114,7 @@ namespace armarx::manipulation::components::articulated_objects_skill_provider
.articulatedObjectReader = articulatedObjectReaderPlugin->get(),
.arviz = arviz}));
- addSkill(std::make_unique<skills::ApproachHandleDirect>(skills::ApproachHandleDirect::Context{
+ addSkill(std::make_unique<skills::ReachGrasp>(skills::ReachGrasp::Context{
.robot = *robot,
.articulatedObjectReader = articulatedObjectReaderPlugin->get(),
.arviz = arviz}));
@@ -125,6 +125,11 @@ namespace armarx::manipulation::components::articulated_objects_skill_provider
addSkill(std::make_unique<skills::ReleaseHandle>(
skills::ReleaseHandle::Context{.robot = *robot, .arviz = arviz}));
+ addSkill(std::make_unique<skills::ReachGrasp>(skills::ReachGrasp::Context{
+ .robot = *robot,
+ .articulatedObjectReader = articulatedObjectReaderPlugin->get(),
+ .arviz = arviz}));
+
//
// Public skills
//
diff --git a/source/armarx/manipulation/geometric_planning/ArVizDrawer.cpp b/source/armarx/manipulation/geometric_planning/ArVizDrawer.cpp
index 17a5899c..0e0591bb 100644
--- a/source/armarx/manipulation/geometric_planning/ArVizDrawer.cpp
+++ b/source/armarx/manipulation/geometric_planning/ArVizDrawer.cpp
@@ -48,21 +48,6 @@ namespace armarx::manipulation::geometric_planning
arviz.commit(l);
}
- // void
- // ArVizDrawer::draw(const std::vector<const geometric_planning::ParametricPath*>& paths)
- // {
- // auto l = arviz.layer("parametric_paths");
-
- // std::for_each(paths.begin(),
- // paths.end(),
- // [&](const auto* path)
- // {
- // ARMARX_CHECK_NOT_NULL(path);
- // draw(l, *path);
- // });
-
- // arviz.commit(l);
- // }
std::vector<Eigen::Vector3f>
to3D(const std::vector<Eigen::Vector2f>& points2d)
@@ -87,16 +72,12 @@ namespace armarx::manipulation::geometric_planning
const core::Pose& frame,
const std::string& name)
{
-
const Eigen::Vector3f from = frame * line.getPosition(line.parameterRange().min);
const Eigen::Vector3f to = frame * line.getPosition(line.parameterRange().max);
layer.add(viz::Sphere(name + "_from").scale(2).color(viz::Color::red()).position(from));
layer.add(viz::Sphere(name + "_to").scale(2).color(viz::Color::green()).position(to));
- // Eigen::Vector3f normal =
- // (core::Pose(virtualDishwasher->getGlobalPose()) * p.frame).translation();
-
ARMARX_DEBUG << "from " << from;
ARMARX_DEBUG << "to " << to;
@@ -108,10 +89,6 @@ namespace armarx::manipulation::geometric_planning
.color(armarx::viz::Color::fromRGBA(0, 128, 255, 128));
layer.add(arrow);
- // layer.add(viz::Sphere("line_from").position(from).radius(50).color(
- // viz::Color::fromRGBA(0, 255, 0, 50)));
- // layer.add(viz::Sphere("line_to").position(to).radius(50).color(
- // viz::Color::fromRGBA(255, 0, 0, 50)));
}
@@ -136,10 +113,6 @@ namespace armarx::manipulation::geometric_planning
ARMARX_DEBUG << "Parameter range is " << circleSegment.parameterRange().min << ", "
<< circleSegment.parameterRange().max;
- // auto polygon = viz::Arrow("path" + std::to_string(++ii))
- // .fromTo(from, to)
- // .color(viz::Color::fromRGBA(0, 128, 255, 128));
-
const float completion =
(circleSegment.parameterRange().max - circleSegment.parameterRange().min) / (2 * M_PI);
@@ -149,11 +122,6 @@ namespace armarx::manipulation::geometric_planning
.completion(completion)
.color(armarx::viz::Color::fromRGBA(0, 128, 255, 128));
- // layer.add(viz::Sphere("circle_from").position(from).radius(50).color(
- // viz::Color::fromRGBA(0, 255, 0, 50)));
- // layer.add(
- // viz::Sphere("circle_to").position(to).radius(50).color(viz::Color::fromRGBA(255, 0, 0, 50)));
-
layer.add(arrow);
}
@@ -178,7 +146,7 @@ namespace armarx::manipulation::geometric_planning
if (circleSegment != nullptr)
{
- ARMARX_INFO << "Circle";
+ ARMARX_VERBOSE << "Circle";
draw(layer, *circleSegment, core::Pose(path.frame->getGlobalPose()), name);
return;
}
@@ -186,10 +154,7 @@ namespace armarx::manipulation::geometric_planning
const auto* line = dynamic_cast<simox::geometric_planning::Line*>(path.path.get());
if (line != nullptr)
{
- ARMARX_INFO << "Line";
-
- // ARMARX_DEBUG << "global root pose" << path.global_T_root.matrix();
- // ARMARX_DEBUG << "frame" << path.root_T_frame.matrix();
+ ARMARX_VERBOSE << "Line";
draw(layer, *line, core::Pose(path.frame->getGlobalPose()), name);
return;
}
diff --git a/source/armarx/manipulation/geometric_planning/CMakeLists.txt b/source/armarx/manipulation/geometric_planning/CMakeLists.txt
index f83ca7be..e646be65 100644
--- a/source/armarx/manipulation/geometric_planning/CMakeLists.txt
+++ b/source/armarx/manipulation/geometric_planning/CMakeLists.txt
@@ -23,8 +23,10 @@ armarx_add_library(geometric_planning
ArmarXGuiComponentPlugins
SOURCES
ArVizDrawer.cpp
+ util.cpp
HEADERS
ArVizDrawer.h
+ util.h
)
diff --git a/source/armarx/manipulation/geometric_planning/util.cpp b/source/armarx/manipulation/geometric_planning/util.cpp
new file mode 100644
index 00000000..9757a035
--- /dev/null
+++ b/source/armarx/manipulation/geometric_planning/util.cpp
@@ -0,0 +1,105 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "util.h"
+
+#include <algorithm>
+#include <optional>
+#include <tuple>
+#include <vector>
+
+#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
+#include <VirtualRobot/Robot.h>
+
+namespace armarx::manipulation::geometric_planning
+{
+
+
+ std::vector<std::string>
+ nodesMatching(const std::vector<std::string>& allNodeNames, const std::string& identifier)
+ {
+
+ std::vector<std::string> handleNodeNames;
+ std::copy_if(allNodeNames.begin(),
+ allNodeNames.end(),
+ std::back_inserter(handleNodeNames),
+ [&identifier](const auto& nodeName)
+ {
+ // the second check is a bit hacky: we can define transformation nodes in URDF which might match, e.g. "parent->child"
+ return simox::alg::ends_with(nodeName, identifier) and
+ not simox::alg::contains(nodeName, '>');
+ });
+
+ return handleNodeNames;
+ }
+
+ std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
+ findGraspByName(const VirtualRobot::Robot& robot,
+ const std::string& graspSetName,
+ const std::string& graspName)
+ {
+ for (const auto& [node, graspSets] : allGraspSets(robot))
+ {
+ if (not graspSets.empty())
+ {
+ // ARMARX_VERBOSE << "Found grasp sets attached to node " << node->getName();
+ for (const auto& graspSet : graspSets)
+ {
+ // check if grasp set name matches if provided
+ if (not graspSetName.empty() and graspSet->getName() != graspSetName)
+ {
+ continue;
+ }
+
+ for (const auto& grasp : graspSet->getGrasps())
+ {
+ if (graspName == grasp->getName())
+ {
+ return std::make_tuple(node, grasp);
+ }
+ }
+ }
+ }
+ }
+
+ return std::nullopt;
+ }
+
+
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>>
+ allGraspSets(const VirtualRobot::Robot& robot)
+ {
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>> nodeGraspSets;
+
+ for (const auto& node : robot.getRobotNodes())
+ {
+ const auto graspSets = node->getAllGraspSets();
+ for (const auto& graspSet : graspSets)
+ {
+ nodeGraspSets[node].push_back(graspSet);
+ }
+ }
+
+ return nodeGraspSets;
+ }
+
+} // namespace armarx::manipulation::geometric_planning
diff --git a/source/armarx/manipulation/geometric_planning/util.h b/source/armarx/manipulation/geometric_planning/util.h
index e33a179f..a0a9ce4f 100644
--- a/source/armarx/manipulation/geometric_planning/util.h
+++ b/source/armarx/manipulation/geometric_planning/util.h
@@ -21,30 +21,23 @@
#pragma once
-#include <algorithm>
-#include <vector>
-#include <SimoxUtility/algorithm/string/string_tools.h>
+#include <map>
+
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx::manipulation::geometric_planning
{
- inline std::vector<std::string>
- nodesMatching(const std::vector<std::string>& allNodeNames, const std::string& identifier)
- {
-
- std::vector<std::string> handleNodeNames;
- std::copy_if(allNodeNames.begin(),
- allNodeNames.end(),
- std::back_inserter(handleNodeNames),
- [&identifier](const auto& nodeName)
- {
- // the second check is a bit hacky: we can define transformation nodes in URDF which might match, e.g. "parent->child"
- return simox::alg::ends_with(nodeName, identifier) and
- not simox::alg::contains(nodeName, '>');
- });
-
- return handleNodeNames;
- }
+ std::vector<std::string> nodesMatching(const std::vector<std::string>& allNodeNames,
+ const std::string& identifier);
+
+ std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
+ findGraspByName(const VirtualRobot::Robot& robot,
+ const std::string& graspSetName,
+ const std::string& graspName);
+
+ std::map<VirtualRobot::RobotNodePtr, std::vector<VirtualRobot::GraspSetPtr>>
+ allGraspSets(const VirtualRobot::Robot& robot);
} // namespace armarx::manipulation::geometric_planning
diff --git a/source/armarx/manipulation/skills/ApproachHandleDirect.cpp b/source/armarx/manipulation/skills/ApproachHandleDirect.cpp
deleted file mode 100644
index 37b05363..00000000
--- a/source/armarx/manipulation/skills/ApproachHandleDirect.cpp
+++ /dev/null
@@ -1,272 +0,0 @@
-#include "ApproachHandleDirect.h"
-
-#include <memory>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
-#include <IceUtil/Time.h>
-#include <SimoxUtility/color/Color.h>
-
-#include <ArmarXCore/observers/variant/DatafieldRef.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
-
-#include "RobotAPI/components/ArViz/Client/Client.h"
-#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h"
-#include "RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h"
-#include "RobotAPI/libraries/armem_objects/types.h"
-#include "RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h"
-#include <RobotAPI/components/ArViz/Client/Elements.h>
-#include <RobotAPI/libraries/core/CartesianVelocityController.h>
-#include <RobotAPI/libraries/core/CartesianWaypointController.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include "armarx/manipulation/core/arm.h"
-#include "armarx/manipulation/skills/constants.h"
-#include <armarx/manipulation/core/aron_conversions.h>
-#include <armarx/manipulation/core/types.h>
-
-namespace armarx::manipulation::skills
-{
- ApproachHandleDirect::ApproachHandleDirect(const ApproachHandleDirect::Context& ctx) :
- Base(DefaultSkillDescription(), armarx::Frequency(armarx::Duration::MilliSeconds(20))),
- ctx(ctx)
- {
- }
-
- ApproachHandleDirect::~ApproachHandleDirect() = default;
-
-
- armarx::skills::PeriodicSkill::StepResult
- ApproachHandleDirect::step(const SpecializedMainInput& in)
- {
- ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
-
- const Eigen::Vector3f robot_V_tcp_handle = approachDirection(in.params);
-
- Eigen::Vector6f velocity;
- velocity << robot_V_tcp_handle.x(), robot_V_tcp_handle.y(), robot_V_tcp_handle.z(), 0, 0, 0;
-
- velocityController->setTargetVelocity(in.params.approachVelocity * velocity);
-
- draw(in.params);
-
- if (finished(in.params))
- {
- return {.status = armarx::skills::ActiveOrTerminatedSkillStatus::Succeeded, .data = {}};
- }
-
- return {.status = armarx::skills::ActiveOrTerminatedSkillStatus::Running, .data = {}};
- }
-
- armarx::skills::Skill::ExitResult
- ApproachHandleDirect::exit(const SpecializedExitInput& in)
- {
- stop();
- return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
- }
-
- armarx::skills::Skill::InitResult
- ApproachHandleDirect::init(const SpecializedInitInput& in)
- {
- ARMARX_INFO << "ApproachHandleDirect::initialize";
-
- ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
-
- core::Hand hand{};
- fromAron(in.params.hand, hand);
-
- const core::Arm arm = core::getArm(hand);
-
- ARMARX_INFO << "Approaching handle with " << core::HandNames.to_name(hand) << " hand.";
- ARMARX_INFO << "Arm is " << core::ArmNames.to_name(arm);
-
- const auto armRns = ctx.robot.arm(arm);
-
- ARMARX_INFO << "Arm RNS " << armRns->getName();
-
- const std::string controllerName = "ApproachHandleDirectController" + armRns->getName();
-
- velocityController = std::make_shared<armarx::VelocityControllerHelper>(
- ctx.robot.robotUnit(), armRns->getName(), controllerName);
-
-
- ARMARX_INFO << "Querying descriptions";
- const auto descriptions = ctx.articulatedObjectReader.queryDescriptions(Clock::Now());
-
- ARMARX_INFO << "Available descriptions are:";
- for (const auto& description : descriptions)
- {
- ARMARX_INFO << description;
- }
-
- ObjectID objectID;
- fromAron(in.params.object, objectID);
-
- const std::string dishwasherName = objectID.dataset() + "/" + objectID.className();
- ARMARX_INFO << "Searching for object name `" << dishwasherName << "`";
-
- const auto isMatchingDescription = [&objectID](const auto& description) -> bool
- { return objectID.equalClass(ObjectID(description.name)); };
-
- const auto descriptionIt =
- std::find_if(descriptions.begin(), descriptions.end(), isMatchingDescription);
- ARMARX_CHECK(descriptionIt != descriptions.end())
- << "Description " << objectID << " not available";
-
- ARMARX_INFO << "getting object";
- std::optional<armarx::armem::articulated_object::ArticulatedObject> dishwasherOpt =
- ctx.articulatedObjectReader.get(*descriptionIt, Clock::Now(), "0");
- ARMARX_CHECK(dishwasherOpt);
-
- armarx::armem::articulated_object::ArticulatedObject dishwasher = *dishwasherOpt;
- dishwasher.instance = "0"; // first available dishwasher FIXME objectID available
-
-
- const std::string xmlFilename = dishwasher.description.xml.toSystemPath();
- ARMARX_INFO << "Loading (virtual) robot '" << dishwasher.description.name
- << "' from XML file '" << xmlFilename << "'";
-
- object = VirtualRobot::RobotIO::loadRobot(xmlFilename);
- ARMARX_CHECK_NOT_NULL(object);
-
-
- simox::geometric_planning::ArticulatedObjectDoorHelper::Params doorHelperParams;
- simox::geometric_planning::ArticulatedObjectDoorHelper doorHelper(object, doorHelperParams);
-
- interactionInfo = doorHelper.planInteractionExtended(
- in.params.objectNodeSet, Pose(armRns->getTCP()->getGlobalPose()));
-
- draw(in.params);
-
- return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
- }
-
- void
- ApproachHandleDirect::draw(const ApproachHandleDirect::ParamsT& params)
- {
- core::Hand hand{};
- fromAron(params.hand, hand);
-
- const core::Pose global_T_tcp_at_handle =
- core::Pose(interactionInfo.rns.handleSurfaceProjection->getGlobalPose()) *
- core::Pose(interactionInfo.handleSurfaceProjection_T_tcp_at_handle);
-
- const Eigen::Vector3f global_P_tcp_at_handle = global_T_tcp_at_handle.translation();
- const Eigen::Vector3f global_P_tcp =
- ctx.robot.arm(core::getArm(hand))->getTCP()->getGlobalPosition();
-
- auto layer = ctx.arviz.layer("skills.approach_handle");
-
- layer.add(armarx::viz::Sphere("tcp")
- .position(global_P_tcp)
- .radius(parameters.sphereVisuRadius)
- .color(simox::Color::red(parameters.visuAlpha)));
-
- layer.add(armarx::viz::Sphere("tcp_at_handle")
- .position(global_P_tcp_at_handle)
- .radius(parameters.sphereVisuRadius)
- .color(simox::Color::green(parameters.visuAlpha)));
-
- layer.add(armarx::viz::Arrow("movement_direction")
- .fromTo(global_P_tcp, global_P_tcp + approachVector(params))
- .width(parameters.arrowVisuWidth));
-
- ctx.arviz.commit(layer);
- }
-
- Eigen::Vector3f
- ApproachHandleDirect::approachVector(const ApproachHandleDirect::ParamsT& params)
- {
- core::Hand hand{};
- fromAron(params.hand, hand);
-
- const Pose global_T_tcp_at_handle =
- Pose(interactionInfo.rns.handleSurfaceProjection->getGlobalPose()) *
- Pose(interactionInfo.handleSurfaceProjection_T_tcp_at_handle);
-
- // TODO apply offset in tcp_at_handle frame ... => y direction...
- const Eigen::Vector3f global_P_tcp_at_handle =
- global_T_tcp_at_handle.translation() + Eigen::Vector3f::UnitZ() * 100;
-
- const Eigen::Vector3f global_P_tcp =
- ctx.robot.arm(getArm(hand))->getTCP()->getGlobalPosition();
- const Eigen::Vector3f global_V_tcp_handle = (global_P_tcp_at_handle - global_P_tcp);
-
- // -> tcp frame
- armarx::FramedDirection framedDirection(
- global_V_tcp_handle, armarx::GlobalFrame, ctx.robot.name());
-
- Eigen::Vector3f robot_V_tcp_handle =
- framedDirection.toRootFrame(ctx.robot.robot())->toEigen();
-
- return robot_V_tcp_handle;
- }
-
- Eigen::Vector3f
- ApproachHandleDirect::approachDirection(const ApproachHandleDirect::ParamsT& params)
- {
- return approachVector(params).normalized();
- }
-
- bool
- ApproachHandleDirect::stop()
- {
- ARMARX_INFO << "ApproachHandleDirect::initialize";
-
- velocityController->cleanup();
-
- return true;
- }
-
- bool
- ApproachHandleDirect::finished(const ApproachHandleDirect::ParamsT& params)
- {
- // Force limit
-
- core::Hand hand{};
- fromAron(params.hand, hand);
-
- const auto ft = ctx.robot.getForceTorqueInHand(hand);
- ARMARX_CHECK(ft.has_value())
- << "No force/torque data available for hand " << core::HandNames.to_name(hand);
-
- const armarx::FramedDirection framedForce = ft->force;
-
- const Eigen::Vector3f global_V_force = framedForce.toGlobalEigen(ctx.robot.robot());
- const Eigen::Vector3f global_V_approach_dir = approachDirection(params);
-
- // define a frame where x is pointing into the movement direction
- const Eigen::Matrix3f global_R_approach =
- Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitX(), global_V_approach_dir)
- .toRotationMatrix();
-
- // Now the contact can be evaluated along the x-axis. Other forces should be minimal.
- const Eigen::Vector3f approach_V_force = global_R_approach.inverse() * global_V_force;
-
- const float force = std::abs(approach_V_force.x()); // x
- const float forcePerturbation = approach_V_force.tail<2>().norm(); // y and z
-
- ARMARX_DEBUG << VAROUT(force);
- ARMARX_DEBUG << VAROUT(forcePerturbation);
-
- const bool forceLimitReached = force > params.contactForceThreshold;
-
- // TODO(fabian.reister): Evaluate position -> success / failure
- const bool positionLimitReached =
- approachVector(params).norm() < params.positionLimit;
-
- return forceLimitReached and positionLimitReached;
- }
-
-
- armarx::skills::SkillDescription
- ApproachHandleDirect::DefaultSkillDescription()
- {
- return {constants::ApproachHandleDirect,
- "",
- {},
- armarx::core::time::Duration::MilliSeconds(999999), // ??
- ParamType::ToAronType()};
- }
-} // namespace armarx::manipulation::skills
diff --git a/source/armarx/manipulation/skills/CMakeLists.txt b/source/armarx/manipulation/skills/CMakeLists.txt
index 6718f929..9d3e992f 100644
--- a/source/armarx/manipulation/skills/CMakeLists.txt
+++ b/source/armarx/manipulation/skills/CMakeLists.txt
@@ -2,6 +2,7 @@ armarx_add_aron_library(skills_aron
ARON_FILES
aron/Open.xml
aron/GraspHandleParams.xml
+ aron/ReachGraspParams.xml
aron/ParametricPathExecutorParams.xml
aron/SetJointValuesParams.xml
aron/ReleaseHandleParams.xml
@@ -37,7 +38,7 @@ armarx_add_library(skills
SOURCES
GraspHandle.cpp
ApproachHandle.cpp
- ApproachHandleDirect.cpp
+ ReachGrasp.cpp
ParametricPathExecutor.cpp
EstablishDoorContact.cpp
ShapeHand.cpp
@@ -50,7 +51,7 @@ armarx_add_library(skills
HEADERS
ParametricPathExecutor.h
ApproachHandle.h
- ApproachHandleDirect.h
+ ReachGrasp.h
EstablishDoorContact.h
ShapeHand.h
GraspHandle.h
diff --git a/source/armarx/manipulation/skills/ReachGrasp.cpp b/source/armarx/manipulation/skills/ReachGrasp.cpp
new file mode 100644
index 00000000..d8e1cada
--- /dev/null
+++ b/source/armarx/manipulation/skills/ReachGrasp.cpp
@@ -0,0 +1,301 @@
+#include "ReachGrasp.h"
+
+#include <memory>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/Grasping/Grasp.h>
+
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h"
+#include "RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
+#include "RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/libraries/core/CartesianVelocityController.h>
+#include <RobotAPI/libraries/core/CartesianWaypointController.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#include "armarx/manipulation/core/arm.h"
+#include "armarx/manipulation/geometric_planning/util.h"
+#include "armarx/manipulation/skills/constants.h"
+#include <armarx/manipulation/core/aron_conversions.h>
+#include <armarx/manipulation/core/types.h>
+
+namespace armarx::manipulation::skills
+{
+ ReachGrasp::ReachGrasp(const ReachGrasp::Context& ctx) :
+ Base(DefaultSkillDescription(), armarx::Frequency(armarx::Duration::MilliSeconds(20))),
+ ctx(ctx)
+ {
+ }
+
+ ReachGrasp::~ReachGrasp() = default;
+
+
+ armarx::skills::PeriodicSkill::StepResult
+ ReachGrasp::step(const SpecializedMainInput& in)
+ {
+ ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
+
+ // const Eigen::Vector3f robot_V_tcp_handle = approachDirection(in.params);
+
+ // Eigen::Vector6f velocity;
+ // velocity << robot_V_tcp_handle.x(), robot_V_tcp_handle.y(), robot_V_tcp_handle.z(), 0, 0, 0;
+
+ // FIXME update target
+ // velocityController->setTargetVelocity(in.params.approachVelocity * velocity);
+
+ draw(in.params);
+
+ if (finished(in.params))
+ {
+ return {.status = armarx::skills::ActiveOrTerminatedSkillStatus::Succeeded, .data = {}};
+ }
+
+ return {.status = armarx::skills::ActiveOrTerminatedSkillStatus::Running, .data = {}};
+ }
+
+ armarx::skills::Skill::ExitResult
+ ReachGrasp::exit(const SpecializedExitInput& in)
+ {
+ stop();
+ return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
+ }
+
+ armarx::skills::Skill::InitResult
+ ReachGrasp::init(const SpecializedInitInput& in)
+ {
+ ARMARX_INFO << "ReachGrasp::initialize";
+
+ ARMARX_CHECK(ctx.robot.synchronize(Clock::Now()));
+
+ core::Hand hand{};
+ fromAron(in.params.hand, hand);
+
+ const core::Arm arm = core::getArm(hand);
+
+ ARMARX_INFO << "Approaching handle with " << core::HandNames.to_name(hand) << " hand.";
+ ARMARX_INFO << "Arm is " << core::ArmNames.to_name(arm);
+
+ const auto armRns = ctx.robot.arm(arm);
+
+ ARMARX_INFO << "Arm RNS " << armRns->getName();
+
+ const std::string controllerName = "ReachGraspController" + armRns->getName();
+
+ // velocityController = std::make_shared<armarx::VelocityControllerHelper>(
+ // ctx.robot.robotUnit(), armRns->getName(), controllerName);
+
+
+ ARMARX_INFO << "Querying descriptions";
+ const auto descriptions = ctx.articulatedObjectReader.queryDescriptions(Clock::Now());
+
+ ARMARX_INFO << "Available descriptions are:";
+ for (const auto& description : descriptions)
+ {
+ ARMARX_INFO << description;
+ }
+
+ ObjectID objectID;
+ fromAron(in.params.object, objectID);
+
+ const std::string objectName = objectID.dataset() + "/" + objectID.className();
+ ARMARX_INFO << "Searching for object with name `" << objectName << "`";
+
+ const auto isMatchingDescription = [&objectID](const auto& description) -> bool
+ { return objectID.equalClass(ObjectID(description.name)); };
+
+ const auto descriptionIt =
+ std::find_if(descriptions.begin(), descriptions.end(), isMatchingDescription);
+ ARMARX_CHECK(descriptionIt != descriptions.end())
+ << "Description " << objectID << " not available";
+
+ ARMARX_INFO << "getting object";
+ std::optional<armarx::armem::articulated_object::ArticulatedObject> objectOpt =
+ ctx.articulatedObjectReader.get(*descriptionIt, Clock::Now(), objectID.instanceName());
+ ARMARX_CHECK(objectOpt);
+
+ armarx::armem::articulated_object::ArticulatedObject articulatedObject = *objectOpt;
+ articulatedObject.instance =
+ objectID.instanceName(); // first available dishwasher FIXME objectID available
+
+
+ const std::string xmlFilename = articulatedObject.description.xml.toSystemPath();
+ ARMARX_INFO << "Loading (virtual) robot '" << articulatedObject.description.name
+ << "' from XML file '" << xmlFilename << "'";
+
+ object = VirtualRobot::RobotIO::loadRobot(xmlFilename);
+ ARMARX_CHECK_NOT_NULL(object);
+
+ // TODO do this continuously
+ object->setJointValues(articulatedObject.config.jointMap);
+ object->setGlobalPose(articulatedObject.config.globalPose.matrix());
+ object->setName(objectID.str()); // this is optional, name not used directly
+ object->applyJointValues();
+
+ const std::optional<std::tuple<VirtualRobot::RobotNodePtr, VirtualRobot::GraspPtr>>
+ graspInfo = geometric_planning::findGraspByName(*object, in.params.graspSetName, in.params.graspName);
+ ARMARX_CHECK(graspInfo.has_value()) << in.params.graspName << " not found";
+ std::tie(graspNode, grasp) = graspInfo.value();
+
+ // simox::geometric_planning::ArticulatedObjectDoorHelper::Params doorHelperParams;
+ // simox::geometric_planning::ArticulatedObjectDoorHelper doorHelper(object, doorHelperParams);
+
+ // interactionInfo = doorHelper.planInteractionExtended(
+ // in.params.objectNodeSet, Pose(armRns->getTCP()->getGlobalPose()));
+
+ draw(in.params);
+
+ return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
+ }
+
+ void
+ ReachGrasp::draw(const ReachGrasp::ParamsT& params)
+ {
+ core::Hand hand{};
+ fromAron(params.hand, hand);
+
+ auto layer = ctx.arviz.layer("skills.reach_grasp");
+
+ {
+ ARMARX_CHECK_NOT_NULL(grasp);
+
+ ARMARX_TRACE;
+ const auto& robot = ctx.robot.robot();
+
+ const auto global_T_tcp = grasp->getTcpPoseGlobal(graspNode->getGlobalPose());
+
+
+
+ ARMARX_TRACE;
+
+ struct Info
+ {
+ Eigen::Isometry3f tcp_T_hand_root;
+ std::map<std::string, float> jointValues;
+ };
+
+ const Info info = [&]()
+ {
+ ARMARX_TRACE;
+ const armarx::RobotNameHelper::Arm& arm = ctx.robot.armHelper(core::getArm(hand));
+
+ ARMARX_TRACE;
+ const armarx::RobotNameHelper::RobotArm& robotArm = ctx.robot.robotArmHelper(core::getArm(hand));
+
+ const auto eefName = arm.getEndEffector();
+ const auto eef = robot->getEndEffector(eefName);
+
+ ARMARX_VERBOSE << "preshape: " << grasp->getPreshapeName();
+ const auto preshape = eef->getPreshape(grasp->getPreshapeName());
+
+ Info info;
+
+ if (preshape != nullptr)
+ {
+ info.jointValues = preshape->getRobotNodeJointValueMap();
+ ARMARX_VERBOSE << "preshape with joint values " << info.jointValues;
+ }
+
+ ARMARX_TRACE;
+ const Eigen::Isometry3f tcp_T_hand_root(
+ robotArm.getTCP()->getPoseInRootFrame().inverse() *
+ robot->getRobotNode(arm.getHandRootNode())->getPoseInRootFrame());
+ info.tcp_T_hand_root = tcp_T_hand_root;
+
+ return info;
+ }();
+
+ ARMARX_TRACE;
+
+ const armarx::RobotNameHelper::Arm& arm = ctx.robot.armHelper(core::getArm(hand));
+ const std::string armName = core::getArmName(hand);
+
+ ARMARX_TRACE;
+ const auto vizElement =
+ viz::Robot(armName + "/" + grasp->getName())
+ .file(arm.getHandModelPackage(), arm.getHandModelPath())
+ .pose(Eigen::Matrix4f{(global_T_tcp * info.tcp_T_hand_root).matrix()})
+ .joints(info.jointValues)
+ .overrideColor(simox::Color::cyan());
+
+ ARMARX_TRACE;
+ layer.add(vizElement);
+ }
+
+ ctx.arviz.commit(layer);
+ }
+
+
+ bool
+ ReachGrasp::stop()
+ {
+ ARMARX_INFO << "ReachGrasp::initialize";
+
+ // velocityController->cleanup();
+
+ return true;
+ }
+
+ bool
+ ReachGrasp::finished(const ReachGrasp::ParamsT& params)
+ {
+ // Force limit
+
+ // core::Hand hand{};
+ // fromAron(params.hand, hand);
+
+ // const auto ft = ctx.robot.getForceTorqueInHand(hand);
+ // ARMARX_CHECK(ft.has_value())
+ // << "No force/torque data available for hand " << core::HandNames.to_name(hand);
+
+ // const armarx::FramedDirection framedForce = ft->force;
+
+ // const Eigen::Vector3f global_V_force = framedForce.toGlobalEigen(ctx.robot.robot());
+ // const Eigen::Vector3f global_V_approach_dir = approachDirection(params);
+
+ // // define a frame where x is pointing into the movement direction
+ // const Eigen::Matrix3f global_R_approach =
+ // Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f::UnitX(), global_V_approach_dir)
+ // .toRotationMatrix();
+
+ // // Now the contact can be evaluated along the x-axis. Other forces should be minimal.
+ // const Eigen::Vector3f approach_V_force = global_R_approach.inverse() * global_V_force;
+
+ // const float force = std::abs(approach_V_force.x()); // x
+ // const float forcePerturbation = approach_V_force.tail<2>().norm(); // y and z
+
+ // ARMARX_DEBUG << VAROUT(force);
+ // ARMARX_DEBUG << VAROUT(forcePerturbation);
+
+ // const bool forceLimitReached = force > params.contactForceThreshold;
+
+ // // TODO(fabian.reister): Evaluate position -> success / failure
+ // const bool positionLimitReached =
+ // approachVector(params).norm() < params.positionLimit;
+
+ // return forceLimitReached and positionLimitReached;
+
+ // FIXME implement pos limit
+ return false;
+ }
+
+
+ armarx::skills::SkillDescription
+ ReachGrasp::DefaultSkillDescription()
+ {
+ return {constants::ReachGrasp,
+ "",
+ {},
+ armarx::core::time::Duration::MilliSeconds(999999), // ??
+ ParamType::ToAronType()};
+ }
+} // namespace armarx::manipulation::skills
diff --git a/source/armarx/manipulation/skills/ApproachHandleDirect.h b/source/armarx/manipulation/skills/ReachGrasp.h
similarity index 78%
rename from source/armarx/manipulation/skills/ApproachHandleDirect.h
rename to source/armarx/manipulation/skills/ReachGrasp.h
index ccd17567..b652e6fe 100644
--- a/source/armarx/manipulation/skills/ApproachHandleDirect.h
+++ b/source/armarx/manipulation/skills/ReachGrasp.h
@@ -28,7 +28,7 @@
#include <GeometricPlanning/ArticulatedObjectDoorHelper.h>
#include <armarx/manipulation/core/Robot.h>
-#include <armarx/manipulation/skills/aron/ApproachHandleParams.aron.generated.h>
+#include <armarx/manipulation/skills/aron/ReachGraspParams.aron.generated.h>
namespace armarx
{
@@ -44,12 +44,12 @@ namespace armarx::viz
namespace armarx::manipulation::skills
{
- class ApproachHandleDirect :
- virtual public armarx::skills::PeriodicSpecializedSkill<arondto::ApproachHandleParams>
+ class ReachGrasp :
+ virtual public armarx::skills::PeriodicSpecializedSkill<arondto::ReachGraspParams>
{
public:
- using ParamsT = arondto::ApproachHandleParams;
- using Base = armarx::skills::PeriodicSpecializedSkill<arondto::ApproachHandleParams>;
+ using ParamsT = arondto::ReachGraspParams;
+ using Base = armarx::skills::PeriodicSpecializedSkill<ParamsT>;
struct Context
{
@@ -73,9 +73,9 @@ namespace armarx::manipulation::skills
int visuAlpha = 128.F;
};
- ApproachHandleDirect(const Context& ctx);
+ ReachGrasp(const Context& ctx);
- ~ApproachHandleDirect() override;
+ ~ReachGrasp() override;
private:
@@ -95,21 +95,14 @@ namespace armarx::manipulation::skills
bool finished(const ParamsT& params);
- Eigen::Vector3f approachDirection(const ParamsT& params);
-
- Eigen::Vector3f approachVector(const ParamsT& params);
-
private:
Context ctx;
Parameters parameters;
- // FIXME use impedance controller instead
- armarx::VelocityControllerHelperPtr velocityController;
-
- simox::geometric_planning::ArticulatedObjectDoorHelper::DoorInteractionContextExtended
- interactionInfo;
-
VirtualRobot::RobotPtr object;
+
+ VirtualRobot::GraspPtr grasp;
+ VirtualRobot::RobotNodePtr graspNode;
};
} // namespace armarx::manipulation::skills
diff --git a/source/armarx/manipulation/skills/aron/ReachGraspParams.xml b/source/armarx/manipulation/skills/aron/ReachGraspParams.xml
new file mode 100644
index 00000000..79a0de5a
--- /dev/null
+++ b/source/armarx/manipulation/skills/aron/ReachGraspParams.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <AronIncludes>
+ <PackagePath package="RobotAPI" path="libraries/ArmarXObjects/aron/ObjectID.xml" />
+ <Include include="armarx/manipulation/core/aron/Hand.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+ <Object name='armarx::manipulation::skills::arondto::ReachGraspParams'>
+
+ <!-- <ObjectChild key='objectEntityId'>
+ <string />
+ </ObjectChild> -->
+
+ <ObjectChild key='hand'>
+ <armarx::manipulation::core::arondto::Hand />
+ </ObjectChild>
+
+ <ObjectChild key='object'>
+ <armarx::arondto::ObjectID />
+ </ObjectChild>
+
+ <ObjectChild key='graspSetName'>
+ <string />
+ </ObjectChild>
+
+ <ObjectChild key='graspName'>
+ <string />
+ </ObjectChild>
+
+ </Object>
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/manipulation/skills/constants.h b/source/armarx/manipulation/skills/constants.h
index ff09edb1..09756ca2 100644
--- a/source/armarx/manipulation/skills/constants.h
+++ b/source/armarx/manipulation/skills/constants.h
@@ -31,7 +31,8 @@ namespace armarx::manipulation::skills::constants
inline const std::string OpenFridge = "OpenFridge";
inline const std::string ApproachHandle = "ApproachHandle";
- inline const std::string ApproachHandleDirect = "ApproachHandleDirect";
+ // inline const std::string ApproachHandleDirect = "ApproachHandleDirect";
+ inline const std::string ReachGrasp = "ReachGrasp";
inline const std::string EstablishDoorContact = "EstablishDoorContact";
inline const std::string GraspHandle = "GraspHandle";
inline const std::string MoveTCP = "MoveTCP";
--
GitLab