diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT new file mode 100644 index 0000000..2f21e36 --- /dev/null +++ b/MUJOCO_LOG.TXT @@ -0,0 +1,2 @@ +Tue Dec 9 14:28:38 2025 +WARNING: Nan, Inf or huge value in QACC at DOF 91. The simulation is unstable. Time = 80.6200. diff --git a/include/AppWindow.h b/include/AppWindow.h index 590b804..12225ea 100644 --- a/include/AppWindow.h +++ b/include/AppWindow.h @@ -5,9 +5,11 @@ #include #include +#include "DebugDrawings.h" #include "MujocoContext.h" #include "SimulationThread.h" #include "SimulationViewport.h" + namespace spqr { class AppWindow : public QMainWindow { @@ -27,6 +29,8 @@ class AppWindow : public QMainWindow { std::unique_ptr mujContext; std::unique_ptr viewport; std::unique_ptr sim; + + DebugDrawings* debugDrawings; }; } // namespace spqr diff --git a/include/DebugDrawings.h b/include/DebugDrawings.h new file mode 100644 index 0000000..6339024 --- /dev/null +++ b/include/DebugDrawings.h @@ -0,0 +1,163 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace spqr { +class DebugDrawings { + public: + static void init(mjvScene* s) noexcept; + static void drawDebugDrawings(); + + static void processDebugMessage(std::map); + static void removeGeom(const std::string& idLocal); + + // ---------------- regular geom types (mjtGeom) + /** + * @brief drawCircle: initialize a mjGEOM_CIRCLE in mujoco processing a predefined message from the socker + * with the framework + * @param idLocal: identifier of the debug drawing + * @param center: position of the center of the geom + * @param radius: radius of the circle + * @param color: RGBA color of the geom + */ + static void drawCircle(const std::map& data_map); + + /** + * @brief drawSphere: initialize a mjGEOM_SPHERE in mujoco processing a predefined message from the socker + * with the framework + * @param idLocal: identifier of the debug drawing + * @param center: position of the center of the geom + * @param radius: radius of the sphere + * @param color: RGBA color of the geom + */ + static void drawSphere(const std::map& data_map); + + /** + * @brief drawCylinder: initialize a mjGEOM_SPHERE in mujoco processing a predefined message from the + * socker with the framework + * @param idLocal: identifier of the debug drawing + * @param center: position of the center of the geom + * @param radius: radius of the cylinder + * @param length: half length of the cylinder + * @param color: RGBA color of the geom + */ + static void drawCylinder(const std::map& data_map); + + // ---------------- rendering-only geom types (mjtGeom) + + /** + * @brief drawArrow: initialize a mjGEOM_ARROW in mujoco processing a predefined message from the socker + * with the framework + * @param idLocal: identifier of the debug drawing + * @param start: position of the start point of the arrow + * @param end: position of the end point of the arrow + * @param thickness: thickness of the arrow + * @param color: RGBA color of the geom + */ + static void drawArrow(const std::map& data_map); + + /** + * @brief drawLine: initialize a mjGEOM_LINE in mujoco processing a predefined message from the socker + * with the framework + * @param idLocal: identifier of the debug drawing + * @param start: position of the start point of the line + * @param end: position of the end point of the line + * @param thickness: thickness of the line + * @param color: RGBA color of the geom + */ + static void drawLine(const std::map& data_map); + + private: + enum class drawGeomType { + Sphere, + Cylinder, + Circle, + Arrow, + Line, + + COUNT, // keep this last + }; + + static const char* toString(drawGeomType t) { + switch (t) { + case drawGeomType::Sphere: + return "Sphere"; + case drawGeomType::Cylinder: + return "Cylinder"; + case drawGeomType::Circle: + return "Circle"; + case drawGeomType::Arrow: + return "Arrow"; + case drawGeomType::Line: + return "Line"; + } + return "Unknown"; + } + + constexpr bool isRegularGeom(drawGeomType t) { + switch (t) { + case drawGeomType::Sphere: + case drawGeomType::Cylinder: + case drawGeomType::Circle: + return true; + default: + return false; + } + } + + struct GeomData { + mjvGeom geom; + drawGeomType customType; + }; + + inline static mjvScene* ptrDebugDrawingsScene; // pointer to the mujoco scene where to draw the debug + // drawings + inline static std::map mapIdGeom; + + /** + * @brief drawRegularGeom: draw a 'regular'-defined geometric primitive + * @param idLocal: identifier of the debug drawing + * @param mujocoType: standard MuJoCo geometric type (mjtGeom) + * @param customType: custom geometric type used to select the appropriate mjvGeom + * @param size: size of the geometric primitive + * @param pos: position of the geometric primitive + * @param color: color of the primitive (RGBA), can use QColorConstants. + */ + static void drawRegularGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, + const double size[3], const double pos[3], QColor color); + + /** + * @brief drawRenderOnlyGeom: draw a 'rendering-only'-defined geometric primitive + * @param idLocal: identifier of the debug drawing + * @param mujocoType: standard MuJoCo geometric type (mjtGeom) + * @param customType: custom geometric type used to select the appropriate mjvGeom + * @param size: size of the geometric primitive + * @param start: start position of the geometric primitive + * @param end: end position of the geometric primitive + * @param width: width of the geometric primitive + * @param color: RGBA color of the geometric primitive, can be a QColorConstants + */ + static void drawRenderOnlyGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, + const double size[3], const double start[3], const double end[3], + const double width, QColor color); + + /** + * @brief check is the mjv_Geom already exist. If true return the right mjvGeom into mjvScene->geoms + */ + + static mjvGeom* isGeomExist(mjString idLocal, drawGeomType type); + + /** + * @brief move the mjvGeom object + */ + static void moveGeom(mjvGeom* geom, const double center[3], QColor color); + static void moveGeom(mjvGeom* geom, const double start[3], const double end[3], QColor color); +}; + +} // namespace spqr diff --git a/include/RobotManager.h b/include/RobotManager.h index 0400fc8..86a28c1 100644 --- a/include/RobotManager.h +++ b/include/RobotManager.h @@ -18,6 +18,7 @@ #include #include "Constants.h" +#include "DebugDrawings.h" #include "MujocoContext.h" #include "robots/BoosterK1.h" #include "robots/BoosterT1.h" @@ -149,6 +150,7 @@ class RobotManager { RobotManager& operator=(const RobotManager&) = delete; void _serverInternal(int port) { + std::cout << "Starting RobotManager communication server on port " << port << std::endl; int server_fd = socket(AF_INET, SOCK_STREAM, 0); if (server_fd < 0) throw std::runtime_error("Failed to create socket"); @@ -201,7 +203,9 @@ class RobotManager { msgpack::object_handle oh = msgpack::unpack(buffer, n); auto data_map = oh.get().as>(); auto it = data_map.find("robot_name"); - if (it == data_map.end()) + auto it_debug = data_map.find("drawGeomType"); + + if (it == data_map.end() && it_debug == data_map.end()) continue; std::string messageRecipient = it->second.as(); @@ -210,6 +214,10 @@ class RobotManager { for (auto& r : robots_) { if (r->name == messageRecipient) { r->receiveMessage(data_map); + + if (it_debug != data_map.end()) + DebugDrawings::processDebugMessage(data_map); + auto answ = r->sendMessage(); msgpack::sbuffer sbuf; msgpack::pack(sbuf, answ); diff --git a/include/SimulationViewport.h b/include/SimulationViewport.h index 24934e2..d3794c6 100644 --- a/include/SimulationViewport.h +++ b/include/SimulationViewport.h @@ -8,6 +8,7 @@ #include #include "Constants.h" +#include "DebugDrawings.h" #include "MujocoContext.h" namespace spqr { diff --git a/include/robots/Robot.h b/include/robots/Robot.h index f319591..3b174e4 100644 --- a/include/robots/Robot.h +++ b/include/robots/Robot.h @@ -21,6 +21,29 @@ namespace spqr { struct Team; // Forward declaration +struct DebugMessage { + std::string idLocal; + std::string drawGeomType; + + std::array center + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array start + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + std::array end + = {std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN()}; + + double radius = -1; + double thickness = -1; + double length = -1; + + std::array color = {1.0, 1.0, 1.0, 1.0}; + + bool remove = false; +}; + class Robot { public: Robot(const std::string& name, const std::string& type, uint8_t number, @@ -38,6 +61,21 @@ class Robot { virtual void receiveMessage(const std::map& message) = 0; virtual std::map sendMessage() = 0; + std::map sendDebugMessage(DebugMessage debugMessage) { + buffer_zone_.clear(); + std::map msg; + msg["robot_name"] = msgpack::object(name, buffer_zone_); + msg["idLocal"] = msgpack::object(debugMessage.idLocal, buffer_zone_); + msg["drawGeomType"] = msgpack::object(debugMessage.drawGeomType, buffer_zone_); + msg["center"] = msgpack::object(debugMessage.center, buffer_zone_); + msg["radius"] = msgpack::object(debugMessage.radius, buffer_zone_); + msg["start"] = msgpack::object(debugMessage.start, buffer_zone_); + msg["end"] = msgpack::object(debugMessage.end, buffer_zone_); + msg["color"] = msgpack::object(debugMessage.color, buffer_zone_); + + return msg; + } + std::string name; std::string type; uint8_t number; diff --git a/resources/config/framework_config.yaml b/resources/config/framework_config.yaml index 8cc022c..64cdbd6 100644 --- a/resources/config/framework_config.yaml +++ b/resources/config/framework_config.yaml @@ -1,4 +1,4 @@ image: ubuntu:22.04 volumes: - - "/home/daniaffch/Dev/spqrbooster2026/src/SimBridge/bridge/build:/app/BridgeSubscriber" - - "/home/daniaffch/Dev/spqrbooster2026/src/SimBridge/fake_framework/build:/app/fake_framework" + - "/home/ubuntu/Robocup/spqrbooster2026/src/SimBridge/bridge/build:/app/BridgeSubscriber" + - "/home/ubuntu/Robocup/spqrbooster2026/src/SimBridge/fake_framework/build:/app/fake_framework" diff --git a/resources/meshes/ball/ball.obj b/resources/meshes/ball/ball.obj index adccca1..0a35801 100644 --- a/resources/meshes/ball/ball.obj +++ b/resources/meshes/ball/ball.obj @@ -17601,4 +17601,4 @@ f 4281/258/258 4156/3472/3265 4158/4544/4250 4282/2069/1964 f 4283/2425/2295 4160/4280/4016 4159/2765/2614 4284/2764/2613 f 4285/3084/2912 4162/3083/2911 4164/3042/2873 4286/3041/2872 f 267/2277/299 269/4365/295 2031/4368/4086 2035/2274/2153 -f 567/524/522 2460/1464/1410 2432/2175/2066 478/525/523 \ No newline at end of file +f 567/524/522 2460/1464/1410 2432/2175/2066 478/525/523 diff --git a/src/AppWindow.cpp b/src/AppWindow.cpp index ac4a0a6..237e02f 100644 --- a/src/AppWindow.cpp +++ b/src/AppWindow.cpp @@ -16,6 +16,7 @@ AppWindow::AppWindow(int& argc, char** argv) { std::signal(SIGTERM, signalHandler); std::signal(SIGINT, signalHandler); std::signal(SIGSEGV, signalHandler); + std::signal(SIGSEGV, signalHandler); std::signal(SIGABRT, signalHandler); resize(spqr::initialWindowWidth, spqr::initialWindowHeight); @@ -82,6 +83,9 @@ void AppWindow::loadScene(const QString& yamlFile) { sim = std::make_unique(mujContext->model, mujContext->data); sim->start(); + + DebugDrawings::init(&(mujContext->scene)); + } catch (const std::exception& e) { QMessageBox::critical(this, "Error loading scene", e.what()); } diff --git a/src/DebugDrawings.cpp b/src/DebugDrawings.cpp new file mode 100644 index 0000000..e3cd814 --- /dev/null +++ b/src/DebugDrawings.cpp @@ -0,0 +1,226 @@ +#include "DebugDrawings.h" + +namespace spqr { + +void DebugDrawings::init(mjvScene* s) noexcept { + ptrDebugDrawingsScene = s; +} + +void DebugDrawings::processDebugMessage(std::map data_map) { + std::string idLocal = data_map["idLocal"].as(); + std::string drawGeomTypeStr = data_map["drawGeomType"].as(); + + // Find debug figure + DebugDrawings::drawGeomType drawGeomType; + bool found = false; + for (int i = 0; i < static_cast(DebugDrawings::drawGeomType::COUNT); i++) { + if (DebugDrawings::toString(static_cast(i)) == drawGeomTypeStr) { + drawGeomType = static_cast(i); + found = true; + break; + } + } + + if (found == false) + throw std::runtime_error("Unknown drawGeomType: " + drawGeomTypeStr); + + if (drawGeomType == DebugDrawings::drawGeomType::Sphere) { + DebugDrawings::drawSphere(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Circle) { + DebugDrawings::drawCircle(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Cylinder) { + DebugDrawings::drawCylinder(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Arrow) { + DebugDrawings::drawArrow(data_map); + } else if (drawGeomType == DebugDrawings::drawGeomType::Line) { + DebugDrawings::drawLine(data_map); + } +} + +static void removeGeom(const std::string& idLocal) {} + +void DebugDrawings::drawDebugDrawings() { + if (!ptrDebugDrawingsScene) + return; + + for (const auto& pair : mapIdGeom) { + if (ptrDebugDrawingsScene->ngeom == ptrDebugDrawingsScene->maxgeom) + break; + + ptrDebugDrawingsScene->geoms[ptrDebugDrawingsScene->ngeom] = pair.second.geom; + ptrDebugDrawingsScene->ngeom += 1; + } +} + +void DebugDrawings::drawRegularGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, + const double size[3], const double pos[3], QColor color) { + mjvGeom geom; + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + + mjv_initGeom(&geom, mujocoType, size, pos, NULL, colorRGBA); + + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); + mapIdGeom[extendedId] = {.geom = geom, .customType = customType}; +} + +void DebugDrawings::drawRenderOnlyGeom(mjString idLocal, mjtGeom mujocoType, drawGeomType customType, + const double size[3], const double start[3], const double end[3], + const double width, QColor color) { + mjvGeom geom; + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + + mjv_initGeom(&geom, mujocoType, size, start, NULL, colorRGBA); + mjv_connector(&geom, mujocoType, width, start, end); + + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); + mapIdGeom[extendedId] = {.geom = geom, .customType = customType}; +} + +mjvGeom* DebugDrawings::isGeomExist(mjString idLocal, drawGeomType customType) { + mjString extendedId = idLocal + "_" + std::to_string(static_cast(customType)); + auto it = mapIdGeom.find(extendedId); + + if (it == mapIdGeom.end()) + return nullptr; + else if ((it->second).customType == customType) + return &((it->second).geom); + else + return nullptr; +} + +void DebugDrawings::moveGeom(mjvGeom* g, const double center[3], QColor color) { + g->pos[0] = center[0]; + g->pos[1] = center[1]; + g->pos[2] = center[2]; + + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + for (int i = 0; i < 4; ++i) { + g->rgba[i] = colorRGBA[i]; + } +} + +void DebugDrawings::moveGeom(mjvGeom* g, const double start[3], const double end[3], QColor color) { + const float colorRGBA[4] = {color.redF(), color.greenF(), color.blueF(), color.alphaF()}; + for (int i = 0; i < 4; ++i) { + g->rgba[i] = colorRGBA[i]; + } + + mjv_connector(g, g->type, g->size[0], start, end); +} + +void DebugDrawings::drawSphere(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + + mjtGeom geomType = mjGEOM_SPHERE; + drawGeomType customType = drawGeomType::Sphere; + + mjvGeom* foundGeom = isGeomExist(idLocal, customType); + + if (foundGeom != nullptr) { + moveGeom(foundGeom, center, color); + } else { + double size[3] = {radius, 0.0, 0.0}; + drawRegularGeom(idLocal, geomType, customType, size, center, color); + } +} + +void DebugDrawings::drawCircle(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + + mjtGeom geomType = mjGEOM_CYLINDER; + drawGeomType customType = drawGeomType::Circle; + + mjvGeom* foundGeom = isGeomExist(idLocal, customType); + + if (foundGeom != nullptr) { + moveGeom(foundGeom, center, color); + } else { + double size[3] = {radius, 0.0, 0.0}; + drawRegularGeom(idLocal, geomType, customType, size, center, color); + } +} + +void DebugDrawings::drawCylinder(const std::map& data_map) { + const auto centerArray = data_map.at("center").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double center[3] = {centerArray[0], centerArray[1], centerArray[2]}; + const double radius = data_map.at("radius").as(); + const double length = data_map.at("length").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + + mjtGeom geomType = mjGEOM_CYLINDER; + drawGeomType customType = drawGeomType::Cylinder; + + mjvGeom* foundGeom = isGeomExist(idLocal, customType); + + if (foundGeom != nullptr) { + moveGeom(foundGeom, center, color); + } else { + double size[3] = {radius, length, 0.0}; + drawRegularGeom(idLocal, geomType, customType, size, center, color); + } +} + +void DebugDrawings::drawArrow(const std::map& data_map) { + const auto startArray = data_map.at("start").as>(); + const auto endArray = data_map.at("end").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double start[3] = {startArray[0], startArray[1], startArray[2]}; + const double end[3] = {endArray[0], endArray[1], endArray[2]}; + const double thickness = data_map.at("thickness").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + + mjtGeom geomType = mjGEOM_ARROW; + drawGeomType customType = drawGeomType::Arrow; + + mjvGeom* foundGeom = isGeomExist(idLocal, customType); + + if (foundGeom != nullptr) { + moveGeom(foundGeom, start, end, color); + } else { + double size[3] = {1.0, 1.0, 1.0}; + double endDoubled[3] = {end[0] * 2, end[1], end[2]}; + drawRenderOnlyGeom(idLocal, geomType, customType, size, start, endDoubled, thickness / 100, color); + } +} + +void DebugDrawings::drawLine(const std::map& data_map) { + const auto startArray = data_map.at("start").as>(); + const auto endArray = data_map.at("end").as>(); + const auto colorArray = data_map.at("color").as>(); + + const auto idLocal = data_map.at("idLocal").as().c_str(); + const double start[3] = {startArray[0], startArray[1], startArray[2]}; + const double end[3] = {endArray[0], endArray[1], endArray[2]}; + const double thickness = data_map.at("thickness").as(); + QColor color = QColor::fromRgbF(colorArray[0], colorArray[1], colorArray[2], colorArray[3]); + + mjtGeom geomType = mjGEOM_LINE; + drawGeomType customType = drawGeomType::Line; + + mjvGeom* foundGeom = isGeomExist(idLocal, customType); + + if (foundGeom != nullptr) { + moveGeom(foundGeom, start, end, color); + } else { + drawRenderOnlyGeom(idLocal, geomType, customType, NULL, start, end, thickness, color); + } +} + +} // namespace spqr diff --git a/src/SimulationViewport.cpp b/src/SimulationViewport.cpp index a21867b..a8599c0 100644 --- a/src/SimulationViewport.cpp +++ b/src/SimulationViewport.cpp @@ -38,6 +38,9 @@ void SimulationViewport::paintGL() { mjrRect viewport = {0, 0, width, height}; mjr_setBuffer(mjFB_WINDOW, context); + + DebugDrawings::drawDebugDrawings(); + mjr_render(viewport, scene, context); mjv_updateScene(model, data, opt, nullptr, cam, mjCAT_ALL, scene); }