grasp_candidate_drawer.cpp

Go to the documentation of this file.

#include <SimoxUtility/math/pose/pose.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <utility>
 
#include "grasp_candidate_drawer.h"
 
namespace armarx
{
    grasp_candidate_drawer::grasp_candidate_drawer(
        armarx::RobotNameHelperPtr rnh,
        const VirtualRobot::RobotPtr& robot)
        : rnh(std::move(rnh)), robot{robot}
    {}
 
    void grasp_candidate_drawer::reset_colors()
    {
        _color_offset = 0;
    }
 
    void grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc, armarx::viz::Layer& l)
    {
        draw(gc, l, simox::color::GlasbeyLUT::at(_color_offset, 255));
    }
    void grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l)
    {
        draw(gch, l, simox::color::GlasbeyLUT::at(_color_offset, 255));
    }
    void grasp_candidate_drawer::draw(const armarx::grasping::GraspCandidatePtr& gc,  armarx::viz::Layer& l, const armarx::viz::Color& color)
    {
        ARMARX_CHECK_NOT_NULL(gc);
        ARMARX_CHECK_NOT_NULL(robot);
        draw(armarx::GraspCandidateHelper{gc, robot}, l, color);
    }
    void grasp_candidate_drawer::draw(const armarx::GraspCandidateHelper& gch, armarx::viz::Layer& l, const armarx::viz::Color& color)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(gch.getGraspCandidate());
        ARMARX_CHECK_NOT_NULL(robot);
        const auto& s = side(gch);
        const std::string name = "grasp_" + s.name + "_" + std::to_string(_color_offset++);
        l.add(armarx::viz::Robot(name + "_robot")
              .file(s.hand_model_package, s.hand_model_path)
              .overrideColor(color)
              .pose(gch.getGraspPoseInGlobal() * s.tcp_2_hroot));
        const Eigen::Vector3f targ = gch.getGraspPositionInGlobal();
        const Eigen::Vector3f appr = gch.getApproachVector();
        const Eigen::Vector3f from = targ - appr * length_approach;
        l.add(armarx::viz::Arrow(name + "_approach")
              .color(color)
              .width(width_approach)
              .fromTo(from, targ));
    }
 
    void grasp_candidate_drawer::draw(
        const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
        float ax_len, float ax_width,
        const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
    {
        draw(side.name, side, l, ax_len, ax_width, color_up, color_trans);
    }
    void grasp_candidate_drawer::draw(
        const std::string& name,
        const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
        float ax_len, float ax_width,
        const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
    {
        draw(side, l, side.nh_arm_w_rob.getTCP()->getGlobalPose(),
             ax_len, ax_width, color_up, color_trans);
    }
    void grasp_candidate_drawer::draw(
        const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
        const Eigen::Matrix4f& gpose, float ax_len, float ax_width,
        const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
    {
        draw("", side, l, gpose, ax_len, ax_width, color_up, color_trans);
    }
    void grasp_candidate_drawer::draw(
        const std::string& name,
        const box_to_grasp_candidates::side_data& side, armarx::viz::Layer& l,
        const Eigen::Matrix4f& gpose, float ax_len, float ax_width,
        const armarx::viz::Color& color_up, const armarx::viz::Color& color_trans)
    {
        const Eigen::Vector3f hand_up =
            simox::math::orientation(gpose) * side.hand_up.normalized() * ax_len;
 
        const Eigen::Vector3f transversal_outward =
            simox::math::orientation(gpose) * side.hand_transverse.normalized() * ax_len;
 
        const Eigen::Vector3f tcp_pos = simox::math::position(gpose);
 
        l.add(armarx::viz::Arrow(name + "hand_up")
              .color(color_up)
              .width(ax_width)
              .fromTo(tcp_pos, tcp_pos + hand_up));
        l.add(armarx::viz::Arrow(name + "transversal_outward")
              .color(color_trans)
              .width(ax_width)
              .fromTo(tcp_pos, tcp_pos + transversal_outward));
    }
 
    const grasp_candidate_drawer::side_data&
    grasp_candidate_drawer::side(const armarx::GraspCandidateHelper& gch)
    {
        ARMARX_TRACE;
        const std::string& s = gch.getGraspCandidate()->side;
        if (_sides.count(s))
        {
            return  _sides.at(s);
        }
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(robot);
        const auto nh = rnh->getArm(s);
        const auto nh_w_rob = nh.addRobot(robot);
        auto& data = _sides[s];
        data.name = s;
        data.hand_model_package = nh.getHandModelPackage();
        data.hand_model_path = nh.getHandModelPath();
        data.tcp_2_hroot = nh_w_rob.getTcp2HandRootTransform();
        return data;
    }
}