From 03c5cf5b20b00fa4cc38d7869cc5d1e77f025f29 Mon Sep 17 00:00:00 2001 From: Guglielmo Cervettini <101020439+G-Cervettini@users.noreply.github.com> Date: Mon, 16 Oct 2023 09:41:07 +0200 Subject: [PATCH] Adding methods for handling primitive shapes to MeshcatVisualizer (#1117) --- .../include/iDynTree/MeshcatVisualizer.h | 104 ++++++-- src/visualization/src/MeshcatVisualizer.cpp | 242 ++++++++++++++++-- 2 files changed, 314 insertions(+), 32 deletions(-) diff --git a/src/visualization/include/iDynTree/MeshcatVisualizer.h b/src/visualization/include/iDynTree/MeshcatVisualizer.h index 7654a464de..349f049b91 100644 --- a/src/visualization/include/iDynTree/MeshcatVisualizer.h +++ b/src/visualization/include/iDynTree/MeshcatVisualizer.h @@ -16,13 +16,13 @@ namespace iDynTree { -/** - * MeshcatVisualizer is an iDynTree-based wrapper to the [meshcat-cpp](https://github.com/ami-iit/meshcat-cpp) visualizer. - * \note Only meshes are supported and the color is taken from the iDynTree::Model - */ -class MeshcatVisualizer -{ -public: + /** + * MeshcatVisualizer is an iDynTree-based wrapper to the [meshcat-cpp](https://github.com/ami-iit/meshcat-cpp) visualizer. + * \note Only meshes are supported and the color is taken from the iDynTree::Model + */ + class MeshcatVisualizer + { + public: MeshcatVisualizer(); ~MeshcatVisualizer(); @@ -33,21 +33,21 @@ class MeshcatVisualizer * @return True in case of success false otherwise/ * @warning Only meshes are supported. The support to the primary shapes needs to be added. */ - bool loadModel(const iDynTree::Model& model, - const std::string& modelName); + bool loadModel(const iDynTree::Model &model, + const std::string &modelName); - /** + /** * Set the state of an already existing model in the visualizer. * @param world_T_base pose of the base of the model. * @param jointPositions position of the joints. * @param modelName the name of the model specified in MeshcatVisualizer::loadModel(), * @return True in case of success false otherwise. */ - bool setModelState(const iDynTree::Transform& world_T_base, - const iDynTree::VectorDynSize& jointPositions, - const std::string& modelName); + bool setModelState(const iDynTree::Transform &world_T_base, + const iDynTree::VectorDynSize &jointPositions, + const std::string &modelName); - /** + /** * Set the state of an already existing model in the visualizer. * @param world_T_base 4x4 matrix representing the homogeneous transformation, * @param jointPositions position of the joints, @@ -58,16 +58,88 @@ class MeshcatVisualizer const iDynTree::Span &jointPositions, const std::string &modelName); + /** + * Load a sphere mesh in the visualizer. + * @param radius radius of the sphere. + * @param color the color of the mesh. + * @param color needs to be a vector of 4 double between 0 and 1 representing RGBA. + * @param name the name of the sphere. Each geometry you add needs to have an unique name. + * @return True in case of success false otherwise. + */ + bool loadSphere(const double radius, + const iDynTree::Span &color, + const std::string &name); + + /** + * Load a cylinder mesh in the visualizer. + * @param radius radius of the cylinder. + * @param height height of the cylinder. + * @param color the color of the mesh. + * @param color needs to be a vector of 4 double between 0 and 1 representing RGBA. + * @param name the name of the cylinder. Each geometry you add needs to have an unique name. + * @return True in case of success false otherwise. + */ + bool loadCylinder(const double radius, const double height, + const iDynTree::Span &color, + const std::string &name); + + /** + * Load a box mesh in the visualizer. + * @param width width of the box. + * @param depth depth of the box. + * @param height height of the box. + * @param color the color of the mesh. + * @param color needs to be a vector of 4 double between 0 and 1 representing RGBA. + * @param name the name of the box. Each geometry you add needs to have an unique name. + * @return True in case of success false otherwise. + */ + bool loadBox(const double width, const double depth, const double height, + const iDynTree::Span &color, + const std::string &name); + + /** + * Load an ellipsoid mesh in the visualizer. + * @param a a-axis of the ellipsoid. + * @param b b-axis of the ellipsoid. + * @param c c-axis of the ellipsoid. + * @param color the color of the mesh. + * @param color needs to be a vector of 4 double between 0 and 1 representing RGBA. + * @param name the name of the ellipsoid. Each geometry you add needs to have an unique name. + * @return True in case of success false otherwise. + */ + bool loadEllipsoid(const double a, const double b, const double c, + const iDynTree::Span &color, + const std::string &name); + + /** + * set the pose of a primitive geometry mesh in the visualizer. + * @param world_T_geometry pose of the geometry. + * @param geometryName the name of the geometry specified in MeshcatVisualizer::loadSphere() || MeshcatVisualizer::loadCylinder() || MeshcatVisualizer::loadBox() || MeshcatVisualizer::loadEllipsoid(). + * @return True in case of success false otherwise. + * Implementations available: for iDynTree::Transform, for iDynTree::MatrixView. + */ + bool setPrimitiveGeometryTransform(const iDynTree::Transform &world_T_geometry, + const std::string &geometryName); + + /** + * set the pose of a primitive geometry mesh in the visualizer. + * @param world_T_geometry pose of the geometry. + * @param geometryName the name of the geometry specified in MeshcatVisualizer::loadSphere() || MeshcatVisualizer::loadCylinder() || MeshcatVisualizer::loadBox() || MeshcatVisualizer::loadEllipsoid(). + * @return True in case of success false otherwise. + * Implementations available: for iDynTree::Transform, for iDynTree::MatrixView. + */ + bool setPrimitiveGeometryTransform(const iDynTree::MatrixView &world_T_geometry, + const std::string &geometryName); /** * Utility function to make the meshcat interface run forever (until the user stops the * application) */ void join(); -private: + private: class Impl; std::unique_ptr m_pimpl; -}; + }; } // namespace iDynTree diff --git a/src/visualization/src/MeshcatVisualizer.cpp b/src/visualization/src/MeshcatVisualizer.cpp index 2096bd8da2..9e5a190392 100644 --- a/src/visualization/src/MeshcatVisualizer.cpp +++ b/src/visualization/src/MeshcatVisualizer.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include using namespace iDynTree; @@ -27,17 +29,19 @@ struct MeshcatVisualizer::Impl { ::MeshcatCpp::Meshcat meshcat; - struct ModelData + struct MultiBodyModelData { iDynTree::Model model; iDynTree::Traversal traversal; iDynTree::LinkPositions linkPositions; }; + std::unordered_map storedMultiBodyModels; + std::unordered_set storedGeometries; - std::unordered_map storedModels; [[nodiscard]] inline bool modelExists(const std::string &modelName) { - return this->storedModels.find(modelName) != this->storedModels.end(); + return this->storedMultiBodyModels.find(modelName) != this->storedMultiBodyModels.end() || + this->storedGeometries.find(modelName) != this->storedGeometries.end(); } [[nodiscard]] inline bool isMesh(const iDynTree::SolidShape &geometry) @@ -63,7 +67,6 @@ struct MeshcatVisualizer::Impl return format == "dae" || format == "stl" || format == "obj"; } - // extact the path of the mesh from the meshcat tree std::string getMeshPathInMeshcatTree(const std::string &modelName, const std::string &linkName, @@ -80,7 +83,7 @@ struct MeshcatVisualizer::Impl return modelName + "/" + linkName + "/" + fileName.substr(pos_slash + 1, pos_dot - pos_slash - 1); } - bool addModelGeometryToView(ModelData &data, + bool addModelGeometryToView(MultiBodyModelData &data, const std::string &modelName) { iDynTree::Model &model = data.model; @@ -141,7 +144,7 @@ struct MeshcatVisualizer::Impl return true; } - bool updateModelGeometry(ModelData &data, + bool updateModelGeometry(MultiBodyModelData &data, const iDynTree::Transform &basePose, const iDynTree::VectorDynSize &jointPositions, const std::string &modelName) @@ -155,10 +158,9 @@ struct MeshcatVisualizer::Impl { const std::string msg = "Wrong size of the jointPositions vector for the model " + modelName + ". Expected: " + std::to_string(model.getNrOfDOFs()) + - "Provided: " + std::to_string(jointPositions.size()); + "Provided: " + std::to_string(jointPositions.size()); reportError("MeshcatVisualizer::Impl", "updateModelGeometry", msg.c_str()); return false; - } if (!iDynTree::ForwardPositionKinematics( @@ -167,7 +169,6 @@ struct MeshcatVisualizer::Impl const std::string msg = "Unable to solve the inverse kinematics for the model named " + modelName; reportError("MeshcatVisualizer::Impl", "updateModelGeometry", msg.c_str()); return false; - } // update the visual shape @@ -222,9 +223,9 @@ bool MeshcatVisualizer::loadModel(const iDynTree::Model &model, } // add the model - Impl::ModelData data; - m_pimpl->storedModels[modelName].model = model; - auto &storedModel = m_pimpl->storedModels[modelName]; + Impl::MultiBodyModelData data; + m_pimpl->storedMultiBodyModels[modelName].model = model; + auto &storedModel = m_pimpl->storedMultiBodyModels[modelName]; storedModel.model.computeFullTreeTraversal(storedModel.traversal); storedModel.linkPositions.resize(storedModel.model); @@ -243,7 +244,7 @@ bool MeshcatVisualizer::setModelState(const iDynTree::Transform &world_T_base, return false; } - Impl::ModelData &storedModel = m_pimpl->storedModels[modelName]; + Impl::MultiBodyModelData &storedModel = m_pimpl->storedMultiBodyModels[modelName]; return m_pimpl->updateModelGeometry(storedModel, world_T_base, jointPositions, modelName); } @@ -253,9 +254,7 @@ bool MeshcatVisualizer::setModelState(const iDynTree::MatrixView & { if (world_T_base.rows() != world_T_base.cols() || world_T_base.rows() != 4) { - const std::string msg = "world_T_base needs to be a 4x4 matrix. Provided a " - + std::to_string(world_T_base.rows()) + "x" - + std::to_string(world_T_base.cols()) + " matrix."; + const std::string msg = "world_T_base needs to be a 4x4 matrix. Provided a " + std::to_string(world_T_base.rows()) + "x" + std::to_string(world_T_base.cols()) + " matrix."; reportError("MeshcatVisualizer", "setModelState", msg.c_str()); return false; } @@ -265,6 +264,217 @@ bool MeshcatVisualizer::setModelState(const iDynTree::MatrixView & modelName); } +bool MeshcatVisualizer::loadSphere(const double radius, + const iDynTree::Span &color, + const std::string &name) +{ + // check if the model already exists + if (m_pimpl->modelExists(name)) + { + const std::string msg = "The model named " + name + "already exists."; + reportError("MeshcatVisualizer", "loadSphere", msg.c_str()); + return false; + } + + // check if the size of the vector is equal to 4 + if (color.size() != 4) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color.size()); + reportError("MeshcatVisualizer", "loadSphere", msg.c_str()); + return false; + } + + // check if all the elements in color are between 0 and 1 + if (color(0) < 0 || color(0) > 1 || + color(1) < 0 || color(1) > 1 || + color(2) < 0 || color(2) > 1 || + color(3) < 0 || color(3) > 1) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color[0]) + ", " + std::to_string(color[1]) + ", " + std::to_string(color[2]) + ", " + std::to_string(color[3]); + reportError("MeshcatVisualizer", "loadSphere", msg.c_str()); + return false; + } + + MeshcatCpp::Material m = MeshcatCpp::Material::get_default_material(); + m.set_color(uint8_t(color[0] * 255), uint8_t(color[1] * 255), uint8_t(color[2] * 255)); + if (color[3] < 1) + { + m.opacity = color[3]; + m.transparent = true; + } + + m_pimpl->meshcat.set_object(name, MeshcatCpp::Sphere(radius), m); + + m_pimpl->storedGeometries.insert(name); + + return true; +} + +bool MeshcatVisualizer::loadCylinder(const double radius, const double height, + const iDynTree::Span &color, + const std::string &name) +{ + // check if the model already exists + if (m_pimpl->modelExists(name)) + { + const std::string msg = "The model named " + name + "already exists."; + reportError("MeshcatVisualizer", "loadCylinder", msg.c_str()); + return false; + } + + // check if the size of the vector is equal to 4 + if (color.size() != 4) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color.size()); + reportError("MeshcatVisualizer", "loadCylinder", msg.c_str()); + return false; + } + + // check if all the elements in color are between 0 and 1 + if (color(0) < 0 || color(0) > 1 || + color(1) < 0 || color(1) > 1 || + color(2) < 0 || color(2) > 1 || + color(3) < 0 || color(3) > 1) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color[0]) + ", " + std::to_string(color[1]) + ", " + std::to_string(color[2]) + ", " + std::to_string(color[3]); + reportError("MeshcatVisualizer", "loadCylinder", msg.c_str()); + return false; + } + + MeshcatCpp::Material m = MeshcatCpp::Material::get_default_material(); + m.set_color(uint8_t(color[0] * 255), uint8_t(color[1] * 255), uint8_t(color[2] * 255)); + if (color[3] < 1) + { + m.opacity = color[3]; + m.transparent = true; + } + + m_pimpl->meshcat.set_object(name, MeshcatCpp::Cylinder(radius, height), m); + + m_pimpl->storedGeometries.insert(name); + + return true; +} + +bool MeshcatVisualizer::loadBox(const double width, const double depth, const double height, + const iDynTree::Span &color, + const std::string &name) +{ + // check if the model already exists + if (m_pimpl->modelExists(name)) + { + const std::string msg = "The model named " + name + "already exists."; + reportError("MeshcatVisualizer", "loadBox", msg.c_str()); + return false; + } + + // check if the size of the vector is equal to 4 + if (color.size() != 4) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color.size()); + reportError("MeshcatVisualizer", "loadBox", msg.c_str()); + return false; + } + + // check if all the elements in color are between 0 and 1 + if (color(0) < 0 || color(0) > 1 || + color(1) < 0 || color(1) > 1 || + color(2) < 0 || color(2) > 1 || + color(3) < 0 || color(3) > 1) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color[0]) + ", " + std::to_string(color[1]) + ", " + std::to_string(color[2]) + ", " + std::to_string(color[3]); + reportError("MeshcatVisualizer", "loadBox", msg.c_str()); + return false; + } + + MeshcatCpp::Material m = MeshcatCpp::Material::get_default_material(); + m.set_color(uint8_t(color[0] * 255), uint8_t(color[1] * 255), uint8_t(color[2] * 255)); + if (color[3] < 1) + { + m.opacity = color[3]; + m.transparent = true; + } + + m_pimpl->meshcat.set_object(name, MeshcatCpp::Box(width, depth, height), m); + + m_pimpl->storedGeometries.insert(name); + + return true; +} + +bool MeshcatVisualizer::loadEllipsoid(const double a, const double b, const double c, + const iDynTree::Span &color, + const std::string &name) +{ + // check if the model already exists + if (m_pimpl->modelExists(name)) + { + const std::string msg = "The model named " + name + "already exists."; + reportError("MeshcatVisualizer", "loadEllipsoid", msg.c_str()); + return false; + } + + // check if the size of the vector is equal to 4 + if (color.size() != 4) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color.size()); + reportError("MeshcatVisualizer", "loadEllipsoid", msg.c_str()); + return false; + } + + // check if all the elements in color are between 0 and 1 + if (color(0) < 0 || color(0) > 1 || + color(1) < 0 || color(1) > 1 || + color(2) < 0 || color(2) > 1 || + color(3) < 0 || color(3) > 1) + { + const std::string msg = "The color needs to be a vector of 4 elements between 0 and 1. Provided: " + std::to_string(color[0]) + ", " + std::to_string(color[1]) + ", " + std::to_string(color[2]) + ", " + std::to_string(color[3]); + reportError("MeshcatVisualizer", "loadEllipsoid", msg.c_str()); + return false; + } + + MeshcatCpp::Material m = MeshcatCpp::Material::get_default_material(); + m.set_color(uint8_t(color[0] * 255), uint8_t(color[1] * 255), uint8_t(color[2] * 255)); + if (color[3] < 1) + { + m.opacity = color[3]; + m.transparent = true; + } + + m_pimpl->meshcat.set_object(name, MeshcatCpp::Ellipsoid(a, b, c), m); + + m_pimpl->storedGeometries.insert(name); + + return true; +} + +bool MeshcatVisualizer::setPrimitiveGeometryTransform(const iDynTree::Transform &world_T_geometry, + const std::string &geometryName) +{ + return this->setPrimitiveGeometryTransform(iDynTree::make_matrix_view(world_T_geometry.asHomogeneousTransform()), geometryName); +} + +bool MeshcatVisualizer::setPrimitiveGeometryTransform(const iDynTree::MatrixView &world_T_geometry, + const std::string &geometryName) +{ + if (world_T_geometry.rows() != world_T_geometry.cols() || world_T_geometry.rows() != 4) + { + const std::string msg = "world_T_geometry needs to be a 4x4 matrix. Provided a " + std::to_string(world_T_geometry.rows()) + "x" + std::to_string(world_T_geometry.cols()) + " matrix."; + reportError("MeshcatVisualizer", "setPrimitiveGeometryTransform", msg.c_str()); + return false; + } + + if (!m_pimpl->storedGeometries.count(geometryName)) + { + const std::string msg = "Unable to find the geometry named " + geometryName; + reportError("MeshcatVisualizer", "setPrimitiveGeometryTransform", msg.c_str()); + return false; + } + + m_pimpl->meshcat.set_transform(geometryName, world_T_geometry); + + return true; +} void MeshcatVisualizer::join() {