GraspCandidateVisu.cpp

Go to the documentation of this file.

#include "GraspCandidateVisu.h"
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/components/ArViz/Client/elements/RobotHand.h>
 
 
namespace armarx::grasping
{
    GraspCandidateVisu::GraspCandidateVisu()
    {
    }
 
 
    void GraspCandidateVisu::visualize(
            const grasping::GraspCandidateDict& candidates,
            viz::Client& arviz)
    {
        viz::Layer graspsLayer = arviz.layer("Grasps");
        viz::Layer graspsNonReachableLayer = arviz.layer("Grasps Non-Reachable");
 
        visualize(candidates, graspsLayer, graspsNonReachableLayer);
        arviz.commit({graspsLayer, graspsNonReachableLayer});
    }
 
 
    void GraspCandidateVisu::visualize(const std::string &id, const GraspCandidate &candidate,
            viz::Layer& layerReachable,
            viz::Layer& layerNonReachable)
    {
        int alpha = alpha_default;
        if (auto it = alphasByKey.find(id); it != alphasByKey.end())
        {
            alpha = it->second;
        }
 
        ARMARX_CHECK(candidate.tcpPoseInHandRoot) << "Candidate needs to have tcpPoseInHandRoot to be visualized!";
        viz::Robot hand = visualize("Grasp_" + id, candidate, alpha);
 
        if (candidate.reachabilityInfo && candidate.reachabilityInfo->reachable)
        {
            layerReachable.add(hand);
        }
        else
        {
            layerNonReachable.add(hand);
        }
    }
 
    void GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
            viz::Layer& layerReachable,
            viz::Layer& layerNonReachable)
    {
        for (auto& [id, candidate] : candidates)
        {
            visualize(id, *candidate, layerReachable, layerNonReachable);
        }
    }
 
 
    viz::Robot GraspCandidateVisu::visualize(
            const std::string& name,
            const GraspCandidate& candidate)
    {
        return visualize(name, candidate, alpha_default);
    }
 
 
    viz::Robot
    GraspCandidateVisu::visualize(
            const std::string& name,
            const grasping::GraspCandidate& candidate,
            int alpha)
    {
        bool isReachable = candidate.reachabilityInfo && candidate.reachabilityInfo->reachable;
        viz::Color color = isReachable ? viz::Color::green() : viz::Color::orange();
        color.a = alpha;
 
        Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
        Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate.graspPose)->toEigen();
        std::string modelFile = "rt/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
 
        viz::Robot hand = viz::RobotHand(name)
                .file("Armar6RT", modelFile)
                .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
                .overrideColor(color);
 
        return hand;
    }
}