diff --git a/include/mc_rtc/gui/Convex.h b/include/mc_rtc/gui/Convex.h new file mode 100644 index 0000000000..6b4ad94bdf --- /dev/null +++ b/include/mc_rtc/gui/Convex.h @@ -0,0 +1,143 @@ +/* + * Copyright 2024 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include +#include +#include +#include "mc_rtc/gui/types.h" +#include +#include +#include +#include +#include + +namespace mc_rtc::gui +{ +/** Helper to convert an sch::S_Polyhedron to a Polyhedron */ +inline std::vector> convexToPolyhedron(std::shared_ptr poly_, + const sva::PTransformd & poseWorld) +{ + mc_rtc::log::info("Visual from convex: {}", fmt::ptr(poly_)); + auto poly = std::dynamic_pointer_cast(poly_); + const auto & pa = *poly->getPolyhedronAlgorithm(); + const auto triangles = pa.triangles_; + const auto vertexes = pa.vertexes_; + auto polyhedron = std::vector>{}; + polyhedron.reserve(triangles.size()); + for(unsigned int i = 0; i < triangles.size(); i++) + { + const auto a = vertexes[triangles[i].a]->getCoordinates(); + const auto b = vertexes[triangles[i].b]->getCoordinates(); + const auto c = vertexes[triangles[i].c]->getCoordinates(); + sva::PTransformd va(Eigen::Vector3d(a[0], a[1], a[2])); + sva::PTransformd vb(Eigen::Vector3d(b[0], b[1], b[2])); + sva::PTransformd vc(Eigen::Vector3d(c[0], c[1], c[2])); + const auto normal = triangles[i].normal; + auto cross = (a - b) ^ (a - c); + auto dot = normal * (cross / cross.norm()); + bool reversePointOrder = dot < 0; + std::array vertexOrder = {va, vb, vc}; + if(reversePointOrder) { vertexOrder = {vc, vb, va}; } + for(size_t j = 0; j < vertexOrder.size(); j++) { vertexOrder[j] = vertexOrder[j] * poseWorld; } + polyhedron.push_back({vertexOrder[0].translation(), vertexOrder[1].translation(), vertexOrder[2].translation()}); + } + return polyhedron; +} + +inline rbd::parsers::Visual convexToVisual(const sch::S_Box & box) +{ + rbd::parsers::Visual visual; + visual.origin = sva::PTransformd::Identity(); + visual.geometry.type = rbd::parsers::Geometry::Type::BOX; + auto boxGeom = rbd::parsers::Geometry::Box{}; + box.getBoxParameters(boxGeom.size.x(), boxGeom.size.y(), boxGeom.size.z()); + visual.geometry.data = boxGeom; + return visual; +} + +inline rbd::parsers::Visual convexToVisual(const sch::S_Cylinder & cylinder) +{ + + rbd::parsers::Visual visual; + visual.origin = sva::PTransformd::Identity(); + visual.geometry.type = rbd::parsers::Geometry::Type::CYLINDER; + auto cylinderGeom = rbd::parsers::Geometry::Cylinder{}; + cylinderGeom.radius = cylinder.getRadius(); + cylinderGeom.length = (cylinder.getP2() - cylinder.getP1()).norm(); + visual.geometry.data = cylinderGeom; + return visual; +} + +/** Helper function to create a Polyhedron from an sch::S_Polyhedron */ +// Returns either a Polyhedron element of a Visual element depending on the type of the +// type of convex object +// Problem: this can only be known at runtime +template +auto Convex(const std::string & name, const PolyhedronConfig & config, GetConvex get_convex_fn, GetPos get_pos_fn) +{ + auto convert = [get_convex_fn, get_pos_fn]() { return convexToPolyhedron(get_convex_fn(), get_pos_fn()); }; + return details::PolyhedronTrianglesListImpl(name, config, convert); +} + +/** Helper to create a Polyhedron from an sch::S_Polyhedron with a default configuration */ +template +auto Convex(const std::string & name, GetConvex get_convex_fn, GetPos get_pos_fn) +{ + PolyhedronConfig config; + config.triangle_color = {0, 0.9, 0, 0.5}; + config.vertices_config.color = {0, 1, 0, 0.5}; + config.show_vertices = false; + config.edge_config.color = {0, 1, 0, 0.5}; + config.show_edges = false; + return Convex(name, config, get_convex_fn, get_pos_fn); +} + +template +auto ConvexBox(const std::string & name, GetConvex get_convex_fn, GetPos get_pos_fn) +{ + auto convert = [get_convex_fn]() { return convexToVisual(get_convex_fn()); }; + return details::VisualImpl(name, convert, get_pos_fn); +} + +template +auto ConvexCylinder(const std::string & name, GetConvex get_convex_fn, GetPos get_pos_fn) +{ + auto convert = [get_convex_fn]() { return convexToVisual(get_convex_fn()); }; + return details::VisualImpl(name, convert, get_pos_fn); +} +} // namespace mc_rtc::gui +// +// visualization_msgs::Marker fromCylinder(const std::string & frame_id, +// const std::string & name, +// size_t id, +// sch::S_Cylinder & cylinder, +// const sva::PTransformd & colTrans) +// { +// auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::CYLINDER); +// marker.scale.x = 2 * cylinder.getRadius(); +// marker.scale.y = 2 * cylinder.getRadius(); +// marker.scale.z = (cylinder.getP2() - cylinder.getP1()).norm(); +// auto midP = cylinder.getP1() + (cylinder.getP2() - cylinder.getP1()) / 2; +// Eigen::Vector3d midPV3{midP.m_x, midP.m_y, midP.m_z}; +// sva::PTransformd cylinderCenter{Eigen::Matrix3d::Identity(), midPV3}; +// setMarkerPose(marker, colTrans * cylinderCenter); +// return marker; +// } +// +// visualization_msgs::Marker fromSphere(const std::string & frame_id, +// const std::string & name, +// size_t id, +// sch::S_Sphere & sphere, +// const sva::PTransformd & colTrans) +// { +// auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::SPHERE); +// marker.scale.x = 2 * sphere.getRadius(); +// marker.scale.y = 2 * sphere.getRadius(); +// marker.scale.z = 2 * sphere.getRadius(); +// setMarkerPose(marker, colTrans); +// return marker; +// } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2d9bc90415..2f190a0209 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -357,7 +357,7 @@ target_include_directories( ) install_mc_rtc_lib(mc_rbdyn) -set(mc_rtc_gui_SRC mc_rtc/gui/StateBuilder.cpp) +set(mc_rtc_gui_SRC mc_rtc/gui/StateBuilder.cpp mc_rtc/gui/Convex.cpp) set(mc_rtc_gui_HDR ../include/mc_rtc/gui/api.h diff --git a/src/mc_control/MCController.cpp b/src/mc_control/MCController.cpp index 32239faeee..d90084473f 100644 --- a/src/mc_control/MCController.cpp +++ b/src/mc_control/MCController.cpp @@ -25,6 +25,9 @@ #include #include +#include +#include + #include namespace bfs = boost::filesystem; @@ -417,6 +420,43 @@ void MCController::addRobotToGUI(const mc_rbdyn::Robot & r) gui()->addElement( {"Robots"}, mc_rtc::gui::Robot(r.name(), [name, this]() -> const mc_rbdyn::Robot & { return this->outputRobot(name); })); + + for(const auto & [convexName, convexPair] : r.convexes()) + { + const auto & bodyName = convexPair.first; + const auto & object = convexPair.second; + const auto & convexName_ = convexName; + + auto getConvexPose = [bodyName, convexName_, &r]() + { + const sva::PTransformd X_0_b = r.frame(bodyName).position(); + const sva::PTransformd & X_b_c = r.collisionTransform(convexName_); + sva::PTransformd X_0_c = X_b_c * X_0_b; + return X_0_c; + }; + + if(auto poly = std::dynamic_pointer_cast(object)) + { + gui()->addElement({"Robots", r.name(), "Convex"}, mc_rtc::gui::Convex( + convexName, [poly]() { return poly; }, getConvexPose)); + } + else if(auto box = std::dynamic_pointer_cast(object)) + { + gui()->addElement({"Robots", r.name(), "Convex"}, mc_rtc::gui::ConvexBox( + convexName, [box]() { return *box; }, getConvexPose)); + } + else if(auto cylinder = std::dynamic_pointer_cast(object)) + { + gui()->addElement({"Robots", r.name(), "Convex"}, + mc_rtc::gui::ConvexCylinder( + convexName, [cylinder]() { return *cylinder; }, getConvexPose)); + } + // else if(sch::S_Sphere * sphere = dynamic_cast(object)) + // { + // markers.markers.push_back(fromSphere(tf_prefix + frame, col.first, ++id, *sphere, colTrans)); + // } + else { mc_rtc::log::warning("Cannot display {} collision object", convexName); } + } } void MCController::addRobotToLog(const mc_rbdyn::Robot & r)