grasp_candidate_drawer.cpp

Go to the documentation of this file.

#include "grasp_candidate_drawer.h"
 
#include <utility>
 
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Nodes/RobotNode.h>
 
#include <ArmarXCore/util/CPPUtility/trace.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;
    }
} // namespace armarx