diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp index ff55654b049adfe37980d243b1717e69c5938d73..ea5d2df495d8f427695f8778562035439f632397 100644 --- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp +++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp @@ -18,12 +18,13 @@ #include <ArmarXCore/core/time/Metronome.h> #include <ArmarXCore/core/time/forward_declarations.h> -#include "RobotAPI/components/ArViz/Client/elements/Robot.h" -#include "RobotAPI/libraries/armem/core/MemoryID.h" +#include <RobotAPI/components/ArViz/Client/elements/Color.h> +#include <RobotAPI/components/ArViz/Client/elements/Robot.h> #include <RobotAPI/libraries/ArmarXObjects/aron_conversions/armarx.h> #include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h> #include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h> #include <RobotAPI/libraries/GraspingUtility/aron_conversions.h> +#include <RobotAPI/libraries/armem/core/MemoryID.h> #include <RobotAPI/libraries/armem/core/error.h> #include <RobotAPI/libraries/armem/core/ice_conversions.h> #include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h> @@ -48,6 +49,11 @@ namespace armarx::armem::server::grasp defs->optional(enableVisualizeKnownGraspCandidates, "p.enableVisualizeKnownGraspCandidates"); + defs->optional(enableVisualizeKnownGraspCandidatesPreposes, + "p.enableVisualizeKnownGraspCandidatesPreposes"); + defs->optional(enableVisualizeKnownGraspCandidatesApproach, + "p.enableVisualizeKnownGraspCandidatesApproach"); + defs->optional(frequencyHzVisualizeKnownGraspCandidates, "p.frequencyHzVisualizeKnownGraspCandidates"); @@ -1016,16 +1022,55 @@ namespace armarx::armem::server::grasp const Eigen::Isometry3f eef_T_hand_root{ handRootNode->getPoseInFrame(tcp)}; - const Eigen::Isometry3f pose = + const Eigen::Isometry3f global_T_grasp_pose = Eigen::Isometry3f{object.objectPoseGlobal} * - Eigen::Isometry3f{grasp.pose}.inverse() * eef_T_hand_root; + Eigen::Isometry3f{grasp.pose}.inverse(); + + // visualize grasp pose + { + viz::Robot graspHandVisu(visuName + "_grasp"); + graspHandVisu.useCollisionModel() + .file(visuRobotPartXML) + .pose( + Eigen::Isometry3f{global_T_grasp_pose * eef_T_hand_root} + .matrix()); + + layer.add(graspHandVisu); + } + + // visualize prepose if available + if (grasp.prepose.has_value()) + { + const Eigen::Isometry3f global_T_prepose = + Eigen::Isometry3f{object.objectPoseGlobal} * + Eigen::Isometry3f{grasp.prepose.value()}.inverse(); - viz::Robot handVisu(visuName); - handVisu.useCollisionModel() - .file(visuRobotPartXML) - .pose(pose.matrix()); + // visualize as prepose as hand + if (enableVisualizeKnownGraspCandidatesPreposes) + { + viz::Robot graspHandVisuPrepose(visuName + "_prepose"); + graspHandVisuPrepose.useCollisionModel() + .file(visuRobotPartXML) + .pose(Eigen::Isometry3f{global_T_prepose * + eef_T_hand_root} + .matrix()) + .overrideColor(viz::Color::blue()); + + layer.add(graspHandVisuPrepose); + } + + // visualize approach direction (arrow between poses) + if (enableVisualizeKnownGraspCandidatesApproach) + { + viz::Arrow arrow(visuName + "_approach"); + arrow + .fromTo(global_T_prepose.translation(), + global_T_grasp_pose.translation()) + .color(viz::Color::green()); - layer.add(handVisu); + layer.add(arrow); + } + } } } } diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h index 40ef25686fc4c8b545bd0aa49ed787c4035ab7c5..d6ea24e6ecf3f421f62f2b8c904e7e15ff678c3b 100644 --- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h +++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h @@ -120,6 +120,10 @@ namespace armarx::armem::server::grasp bool enableRemoteGui{true}; bool enableVisualizeKnownGraspCandidates = false; + bool enableVisualizeKnownGraspCandidatesPreposes = false; + bool enableVisualizeKnownGraspCandidatesApproach = false; + + std::size_t frequencyHzVisualizeKnownGraspCandidates = 5; void visualizeGraspCandidates();