From 66780f2ba8ca5b6a1a59437a69f82e99afd96532 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 10 Oct 2020 14:36:16 +0300 Subject: [PATCH 1/5] [gui/sensor]: started clean-up to simplify API --- src/examples/arm.cpp | 2 +- src/examples/cameras.cpp | 4 ++-- src/examples/control_loop.cpp | 2 +- src/examples/dof_maps.cpp | 2 +- src/examples/hexapod.cpp | 2 +- src/examples/icub.cpp | 6 ++--- src/examples/iiwa.cpp | 2 +- src/examples/magnum_contexts.cpp | 2 +- src/examples/pendulum.cpp | 2 +- src/examples/talos.cpp | 2 +- src/examples/talos_fast.cpp | 2 +- src/examples/transparent.cpp | 2 +- src/examples/tutorial.cpp | 2 +- src/python/example.py | 4 ++-- src/python/example_parallel.py | 5 +++-- src/python/gui.cpp | 12 ++++++---- src/python/sensor.cpp | 15 ++++++------- src/robot_dart/gui/base.hpp | 8 +++++-- src/robot_dart/gui/magnum/base_graphics.hpp | 16 ++++++++------ src/robot_dart/gui/magnum/graphics.cpp | 3 ++- src/robot_dart/gui/magnum/graphics.hpp | 5 +++-- src/robot_dart/gui/magnum/sensor/camera.cpp | 7 +++--- src/robot_dart/gui/magnum/sensor/camera.hpp | 2 +- .../gui/magnum/windowless_graphics.cpp | 3 ++- .../gui/magnum/windowless_graphics.hpp | 5 +++-- src/robot_dart/robot_dart_simu.cpp | 4 +++- src/robot_dart/sensor/force_torque.cpp | 4 +--- src/robot_dart/sensor/force_torque.hpp | 4 ++-- src/robot_dart/sensor/imu.cpp | 5 +++-- src/robot_dart/sensor/imu.hpp | 2 +- src/robot_dart/sensor/sensor.cpp | 22 +++++++++++++------ src/robot_dart/sensor/sensor.hpp | 7 ++++-- 32 files changed, 95 insertions(+), 70 deletions(-) diff --git a/src/examples/arm.cpp b/src/examples/arm.cpp index c8e77cd7..31adeb98 100644 --- a/src/examples/arm.cpp +++ b/src/examples/arm.cpp @@ -42,7 +42,7 @@ int main() robot_dart::RobotDARTSimu simu; #ifdef GRAPHIC - simu.set_graphics(std::make_shared(&simu)); + simu.set_graphics(std::make_shared()); #endif simu.add_descriptor(std::make_shared(&simu)); simu.add_robot(g_robot); diff --git a/src/examples/cameras.cpp b/src/examples/cameras.cpp index 34cf2b76..0e59a883 100644 --- a/src/examples/cameras.cpp +++ b/src/examples/cameras.cpp @@ -51,7 +51,7 @@ int main() robot_dart::gui::magnum::GraphicsConfiguration configuration; configuration.width = 1024; configuration.height = 768; - auto graphics = std::make_shared>(&simu, configuration); + auto graphics = std::make_shared>(configuration); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); @@ -61,7 +61,7 @@ int main() graphics->record_video("video-main.mp4", simu.graphics_freq()); // Add camera - auto camera = std::make_shared(&simu, graphics->magnum_app(), 256, 256); + auto camera = std::make_shared(graphics->magnum_app(), 256, 256); camera->camera().set_far_plane(5.f); camera->camera().record(true, true); // cameras are recording color images by default, enable depth images as well for this example // cameras can also record video diff --git a/src/examples/control_loop.cpp b/src/examples/control_loop.cpp index 6af050e0..c6fba944 100644 --- a/src/examples/control_loop.cpp +++ b/src/examples/control_loop.cpp @@ -16,7 +16,7 @@ int main() robot_dart::RobotDARTSimu simu(1e-3); #ifdef GRAPHIC - simu.set_graphics(std::make_shared(&simu)); + simu.set_graphics(std::make_shared()); #endif simu.add_robot(robot); diff --git a/src/examples/dof_maps.cpp b/src/examples/dof_maps.cpp index 1b3e9fcc..4cb2f746 100644 --- a/src/examples/dof_maps.cpp +++ b/src/examples/dof_maps.cpp @@ -25,7 +25,7 @@ int main() float dt = 0.001; robot_dart::RobotDARTSimu simu(dt); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); #endif diff --git a/src/examples/hexapod.cpp b/src/examples/hexapod.cpp index abb6a970..f0a14a61 100644 --- a/src/examples/hexapod.cpp +++ b/src/examples/hexapod.cpp @@ -20,7 +20,7 @@ int main() robot_dart::RobotDARTSimu simu(0.001); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0.5, 3., 0.75}, {0.5, 0., 0.2}); #endif diff --git a/src/examples/icub.cpp b/src/examples/icub.cpp index 5d6567c6..95e0a88d 100644 --- a/src/examples/icub.cpp +++ b/src/examples/icub.cpp @@ -35,7 +35,7 @@ int main() robot_dart::RobotDARTSimu simu(0.001); simu.set_collision_detector("fcl"); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); // graphics->record_video("icub.mp4"); @@ -47,10 +47,10 @@ int main() robot_dart::sensor::IMUConfig imu_config; imu_config.body = robot->body_node("chest"); // choose which body the sensor is attached to imu_config.frequency = 200; // update rate of the sensor - auto imu_sensor = simu.add_sensor(&simu, imu_config); + auto imu_sensor = simu.add_sensor(imu_config); // Add a force/torque sensor in "r_ankle_roll" joint - auto ft_sensor = simu.add_sensor(&simu, robot, "r_ankle_roll"); + auto ft_sensor = simu.add_sensor(robot, "r_ankle_roll"); // Add some visualizations robot->set_draw_axis(imu_config.body->getName()); diff --git a/src/examples/iiwa.cpp b/src/examples/iiwa.cpp index d3aab239..d5d17ef3 100644 --- a/src/examples/iiwa.cpp +++ b/src/examples/iiwa.cpp @@ -31,7 +31,7 @@ int main() robot_dart::RobotDARTSimu simu(0.001); simu.set_collision_detector("fcl"); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); diff --git a/src/examples/magnum_contexts.cpp b/src/examples/magnum_contexts.cpp index b926463e..4a14b1bf 100644 --- a/src/examples/magnum_contexts.cpp +++ b/src/examples/magnum_contexts.cpp @@ -54,7 +54,7 @@ int main() robot_dart::gui::magnum::GraphicsConfiguration configuration; configuration.width = 1024; configuration.height = 768; - auto graphics = std::make_shared(&simu, configuration); + auto graphics = std::make_shared(configuration); simu.set_graphics(graphics); // Position the camera differently for each thread to visualize the difference graphics->look_at({0.4 * index, 3.5 - index * 0.1, 2.}, {0., 0., 0.25}); diff --git a/src/examples/pendulum.cpp b/src/examples/pendulum.cpp index a1e12905..054bc6e4 100644 --- a/src/examples/pendulum.cpp +++ b/src/examples/pendulum.cpp @@ -53,7 +53,7 @@ int main() robot_dart::RobotDARTSimu simu; #ifdef GRAPHIC - simu.set_graphics(std::make_shared(&simu)); + simu.set_graphics(std::make_shared()); #endif // (desc_period) simu.add_descriptor(2); diff --git a/src/examples/talos.cpp b/src/examples/talos.cpp index a345db52..8d2c199c 100644 --- a/src/examples/talos.cpp +++ b/src/examples/talos.cpp @@ -30,7 +30,7 @@ int main() robot_dart::RobotDARTSimu simu(dt); simu.set_collision_detector("fcl"); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); graphics->record_video("talos_dancing.mp4"); diff --git a/src/examples/talos_fast.cpp b/src/examples/talos_fast.cpp index 9225fb8b..172a9d1b 100644 --- a/src/examples/talos_fast.cpp +++ b/src/examples/talos_fast.cpp @@ -35,7 +35,7 @@ int main() // we use dart, but only the feet are used simu.set_collision_detector("dart"); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); graphics->record_video("talos.mp4"); diff --git a/src/examples/transparent.cpp b/src/examples/transparent.cpp index 6d6f8669..8790681a 100644 --- a/src/examples/transparent.cpp +++ b/src/examples/transparent.cpp @@ -38,7 +38,7 @@ int main() // choose time step of 0.001 seconds robot_dart::RobotDARTSimu simu(0.001); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); // set the camera at position (0, 3, 1) looking at the center (0, 0, 0) graphics->look_at({0., 3., 1.}, {0., 0., 0.}); diff --git a/src/examples/tutorial.cpp b/src/examples/tutorial.cpp index 91b46353..d130c7b3 100644 --- a/src/examples/tutorial.cpp +++ b/src/examples/tutorial.cpp @@ -35,7 +35,7 @@ int main() // choose time step of 0.001 seconds robot_dart::RobotDARTSimu simu(0.001); #ifdef GRAPHIC - auto graphics = std::make_shared(&simu); + auto graphics = std::make_shared(); simu.set_graphics(graphics); // set the camera at position (0, 3, 1) looking at the center (0, 0, 0) graphics->look_at({0., 3., 1.}, {0., 0., 0.}); diff --git a/src/python/example.py b/src/python/example.py index e1936af5..b6e1b418 100644 --- a/src/python/example.py +++ b/src/python/example.py @@ -49,12 +49,12 @@ def __call__(self): simu.add_descriptor(desc) # Create graphics -graphics = rd.gui.Graphics(simu, rd.gui.GraphicsConfiguration()) +graphics = rd.gui.Graphics() +simu.set_graphics(graphics) # graphics.clear_lights() # mat = rd.gui.Material(magnum.Color4(0, 0, 0, 1), magnum.Color4(1, 1, 1, 1), magnum.Color4(1, 1, 1, 1), 80.) # graphics.add_light(rd.gui.create_point_light(magnum.Vector3(-1., 1., 2.), mat, 2., magnum.Vector3(0., 0., 1.))) # graphics.add_light(rd.gui.create_point_light(magnum.Vector3(1., -1., 2.), mat, 2., magnum.Vector3(0., 0., 1.))) -simu.set_graphics(graphics) # Add robot and floor to the simulation simu.add_robot(robot) diff --git a/src/python/example_parallel.py b/src/python/example_parallel.py index cc4f404d..16338af1 100644 --- a/src/python/example_parallel.py +++ b/src/python/example_parallel.py @@ -26,10 +26,11 @@ def test(): simu = rd.RobotDARTSimu(0.001) # set the graphics - graphics = rd.gui.WindowlessGraphics(simu, rd.gui.GraphicsConfiguration()) - graphics.look_at([0.4 * ii, 3.5 - ii * 0.1, 2.], [0., 0., 0.25], [0., 0., 1.]) + graphics = rd.gui.WindowlessGraphics() simu.set_graphics(graphics) + graphics.look_at([0.4 * ii, 3.5 - ii * 0.1, 2.], [0., 0., 0.25], [0., 0., 1.]) + # add the robot and the floor simu.add_robot(grobot) simu.add_checkerboard_floor(10., 0.1, 1., np.zeros((6,1)), "floor") diff --git a/src/python/gui.cpp b/src/python/gui.cpp index a5de9e3b..7b95f269 100644 --- a/src/python/gui.cpp +++ b/src/python/gui.cpp @@ -115,13 +115,17 @@ namespace robot_dart { using namespace robot_dart::gui::magnum; // Graphics class py::class_>(sm, "Graphics") - .def(py::init()) + .def(py::init(), + py::arg("configuration") = GraphicsConfiguration()) .def("done", &Graphics::done) .def("refresh", &Graphics::refresh) .def("set_render_period", &Graphics::set_render_period) .def("set_enable", &Graphics::set_enable) + .def("set_simu", &Graphics::set_simu) + .def("simu", &Graphics::simu, py::return_value_policy::reference) + .def("look_at", &Graphics::look_at, py::arg("camera_pos"), py::arg("look_at") = Eigen::Vector3d(0, 0, 0), @@ -157,7 +161,8 @@ namespace robot_dart { // WindowlessGraphics class py::class_>(sm, "WindowlessGraphics") - .def(py::init()) + .def(py::init(), + py::arg("configuration") = GraphicsConfiguration()) .def("done", &WindowlessGraphics::done) .def("refresh", &WindowlessGraphics::refresh) @@ -214,8 +219,7 @@ namespace robot_dart { // Camera sensor class py::class_>(sensormodule, "Camera") - .def(py::init(), - py::arg("simu"), + .def(py::init(), py::arg("app"), py::arg("width"), py::arg("height"), diff --git a/src/python/sensor.cpp b/src/python/sensor.cpp index 39b5563d..10442efe 100644 --- a/src/python/sensor.cpp +++ b/src/python/sensor.cpp @@ -102,8 +102,7 @@ namespace robot_dart { }; py::class_>(sensormodule, "Sensor") - .def(py::init(), - py::arg("simu"), + .def(py::init(), py::arg("freq") = 40) .def_readwrite("_active", &PublicistSensor::_active) @@ -122,6 +121,9 @@ namespace robot_dart { py::arg("enable") = true) .def("active", &Sensor::active) + .def("set_simu", &Sensor::set_simu) + .def("simu", &Sensor::simu, py::return_value_policy::reference) + .def("frequency", &Sensor::frequency) .def("set_frequency", &Sensor::set_frequency, py::arg("freq")) @@ -166,13 +168,11 @@ namespace robot_dart { }; py::class_>(sensormodule, "ForceTorque") - .def(py::init(), - py::arg("simu"), + .def(py::init(), py::arg("joint"), py::arg("frequency") = 1000, py::arg("direction") = "child_to_parent") - .def(py::init&, const std::string&, size_t, const std::string&>(), - py::arg("simu"), + .def(py::init&, const std::string&, size_t, const std::string&>(), py::arg("robot"), py::arg("joint_name"), py::arg("frequency") = 1000, @@ -216,8 +216,7 @@ namespace robot_dart { }; py::class_>(sensormodule, "IMU") - .def(py::init(), - py::arg("simu"), + .def(py::init(), py::arg("config")) .def_readonly("_config", &PublicistIMUSensor::_config) diff --git a/src/robot_dart/gui/base.hpp b/src/robot_dart/gui/base.hpp index 299bc5ee..77fe23b4 100644 --- a/src/robot_dart/gui/base.hpp +++ b/src/robot_dart/gui/base.hpp @@ -11,10 +11,11 @@ namespace robot_dart { public: Base() {} - Base(RobotDARTSimu*) {} - virtual ~Base() {} + virtual void set_simu(RobotDARTSimu* simu) { _simu = simu; } + RobotDARTSimu* simu() { return _simu; } + virtual bool done() const { return false; } virtual void refresh() {} @@ -30,6 +31,9 @@ namespace robot_dart { virtual size_t width() const { return 0; } virtual size_t height() const { return 0; } + + protected: + RobotDARTSimu* _simu = nullptr; }; } // namespace gui } // namespace robot_dart diff --git a/src/robot_dart/gui/magnum/base_graphics.hpp b/src/robot_dart/gui/magnum/base_graphics.hpp index 5eb42bbf..4e2401c6 100644 --- a/src/robot_dart/gui/magnum/base_graphics.hpp +++ b/src/robot_dart/gui/magnum/base_graphics.hpp @@ -20,16 +20,21 @@ namespace robot_dart { template class BaseGraphics : public Base { public: - BaseGraphics(RobotDARTSimu* simu, const GraphicsConfiguration& configuration = GraphicsConfiguration()) - : _simu(simu), _world(simu->world()), _width(configuration.width), _height(configuration.height), _frame_counter(0), _enabled(true) + BaseGraphics(const GraphicsConfiguration& configuration = GraphicsConfiguration()) + : _configuration(configuration), _enabled(true) { Corrade::Utility::Debug magnum_silence_output{nullptr}; robot_dart_initialize_magnum_resources(); - _magnum_app.reset(make_application(simu, configuration)); } virtual ~BaseGraphics() {} + virtual void set_simu(RobotDARTSimu* simu) override + { + _simu = simu; + _magnum_app.reset(make_application(simu, _configuration)); + } + size_t width() const override { return _magnum_app->camera().width(); } size_t height() const override { return _magnum_app->camera().height(); } @@ -44,7 +49,6 @@ namespace robot_dart { return; _magnum_app->render(); - _frame_counter++; } void set_enable(bool enable) override @@ -123,9 +127,7 @@ namespace robot_dart { const BaseApplication* magnum_app() const { return &*_magnum_app; } protected: - RobotDARTSimu* _simu; - dart::simulation::WorldPtr _world; - size_t _width, _height, _frame_counter; + GraphicsConfiguration _configuration; bool _enabled; int _fps; std::unique_ptr _magnum_app; diff --git a/src/robot_dart/gui/magnum/graphics.cpp b/src/robot_dart/gui/magnum/graphics.cpp index e3ad64eb..fdd09afe 100644 --- a/src/robot_dart/gui/magnum/graphics.cpp +++ b/src/robot_dart/gui/magnum/graphics.cpp @@ -3,8 +3,9 @@ namespace robot_dart { namespace gui { namespace magnum { - Graphics::Graphics(RobotDARTSimu* simu, const GraphicsConfiguration& configuration) : BaseGraphics(simu, configuration) + void Graphics::set_simu(RobotDARTSimu* simu) { + BaseGraphics::set_simu(simu); // we synchronize by default if we have the graphics activated simu->scheduler().set_sync(true); // enable summary text when graphics activated diff --git a/src/robot_dart/gui/magnum/graphics.hpp b/src/robot_dart/gui/magnum/graphics.hpp index 83069da3..2c3f607a 100644 --- a/src/robot_dart/gui/magnum/graphics.hpp +++ b/src/robot_dart/gui/magnum/graphics.hpp @@ -8,9 +8,10 @@ namespace robot_dart { namespace magnum { class Graphics : public BaseGraphics { public: - Graphics(RobotDARTSimu* simu, const GraphicsConfiguration& configuration = GraphicsConfiguration()); - + Graphics(const GraphicsConfiguration& configuration = GraphicsConfiguration()) : BaseGraphics(configuration) {} ~Graphics() {} + + void set_simu(RobotDARTSimu* simu) override; }; } // namespace magnum } // namespace gui diff --git a/src/robot_dart/gui/magnum/sensor/camera.cpp b/src/robot_dart/gui/magnum/sensor/camera.cpp index 7b94d08d..686d6969 100644 --- a/src/robot_dart/gui/magnum/sensor/camera.cpp +++ b/src/robot_dart/gui/magnum/sensor/camera.cpp @@ -17,7 +17,7 @@ namespace robot_dart { namespace gui { namespace magnum { namespace sensor { - Camera::Camera(RobotDARTSimu* simu, BaseApplication* app, size_t width, size_t height, size_t freq, bool draw_debug) : robot_dart::sensor::Sensor(simu, freq), _magnum_app(app), _width(width), _height(height), _draw_debug(draw_debug) + Camera::Camera(BaseApplication* app, size_t width, size_t height, size_t freq, bool draw_debug) : robot_dart::sensor::Sensor(freq), _magnum_app(app), _width(width), _height(height), _draw_debug(draw_debug) { /* Camera setup */ _camera.reset( @@ -46,13 +46,12 @@ namespace robot_dart { Magnum::GL::Framebuffer::ColorAttachment(0), _color); _framebuffer.attachRenderbuffer( Magnum::GL::Framebuffer::BufferAttachment::Depth, _depth); - - _active = true; } void Camera::init() { - // Do nothing + if (_simu) + _active = true; // TO-DO: Maybe create a camera configuration structure that gets saved and re-initialized each time } diff --git a/src/robot_dart/gui/magnum/sensor/camera.hpp b/src/robot_dart/gui/magnum/sensor/camera.hpp index 3290a3f6..cc5059ba 100644 --- a/src/robot_dart/gui/magnum/sensor/camera.hpp +++ b/src/robot_dart/gui/magnum/sensor/camera.hpp @@ -15,7 +15,7 @@ namespace robot_dart { namespace sensor { class Camera : public robot_dart::sensor::Sensor { public: - Camera(RobotDARTSimu* simu, BaseApplication* app, size_t width, size_t height, size_t freq = 30, bool draw_debug = false); + Camera(BaseApplication* app, size_t width, size_t height, size_t freq = 30, bool draw_debug = false); ~Camera() {} void init() override; diff --git a/src/robot_dart/gui/magnum/windowless_graphics.cpp b/src/robot_dart/gui/magnum/windowless_graphics.cpp index 5cd0b412..e848e8c1 100644 --- a/src/robot_dart/gui/magnum/windowless_graphics.cpp +++ b/src/robot_dart/gui/magnum/windowless_graphics.cpp @@ -3,8 +3,9 @@ namespace robot_dart { namespace gui { namespace magnum { - WindowlessGraphics::WindowlessGraphics(RobotDARTSimu* simu, const GraphicsConfiguration& configuration) : BaseGraphics(simu, configuration) + void WindowlessGraphics::set_simu(RobotDARTSimu* simu) { + BaseGraphics::set_simu(simu); // we should not synchronize by default if we want windowless graphics (usually used only for sensors) simu->scheduler().set_sync(false); // disable summary text when windowless graphics activated diff --git a/src/robot_dart/gui/magnum/windowless_graphics.hpp b/src/robot_dart/gui/magnum/windowless_graphics.hpp index 11e3c6fe..60356ba5 100644 --- a/src/robot_dart/gui/magnum/windowless_graphics.hpp +++ b/src/robot_dart/gui/magnum/windowless_graphics.hpp @@ -9,9 +9,10 @@ namespace robot_dart { namespace magnum { class WindowlessGraphics : public BaseGraphics { public: - WindowlessGraphics(RobotDARTSimu* simu, const GraphicsConfiguration& configuration = GraphicsConfiguration()); - + WindowlessGraphics(const GraphicsConfiguration& configuration = GraphicsConfiguration()) : BaseGraphics(configuration) {} ~WindowlessGraphics() {} + + void set_simu(RobotDARTSimu* simu) override; }; } // namespace magnum } // namespace gui diff --git a/src/robot_dart/robot_dart_simu.cpp b/src/robot_dart/robot_dart_simu.cpp index 40e1cff1..91dfcf91 100644 --- a/src/robot_dart/robot_dart_simu.cpp +++ b/src/robot_dart/robot_dart_simu.cpp @@ -104,7 +104,7 @@ namespace robot_dart { _world->getConstraintSolver()->getCollisionOption().collisionFilter = std::make_shared(); _world->setTimeStep(timestep); _world->setTime(0.0); - _graphics = std::make_shared(this); + _graphics = std::make_shared(); _gui_data.reset(new simu::GUIData()); } @@ -199,6 +199,7 @@ namespace robot_dart { void RobotDARTSimu::set_graphics(const std::shared_ptr& graphics) { _graphics = graphics; + _graphics->set_simu(this); _graphics->set_fps(_graphics_freq); } @@ -226,6 +227,7 @@ namespace robot_dart { void RobotDARTSimu::add_sensor(const std::shared_ptr& sensor) { _sensors.push_back(sensor); + sensor->set_simu(this); sensor->init(); } diff --git a/src/robot_dart/sensor/force_torque.cpp b/src/robot_dart/sensor/force_torque.cpp index 0fd1e90f..8ddabbb0 100644 --- a/src/robot_dart/sensor/force_torque.cpp +++ b/src/robot_dart/sensor/force_torque.cpp @@ -1,13 +1,11 @@ #include "force_torque.hpp" -#include - #include #include namespace robot_dart { namespace sensor { - ForceTorque::ForceTorque(RobotDARTSimu* simu, dart::dynamics::Joint* joint, size_t frequency, const std::string& direction) : Sensor(simu, frequency), _direction(direction) + ForceTorque::ForceTorque(dart::dynamics::Joint* joint, size_t frequency, const std::string& direction) : Sensor(frequency), _direction(direction) { attach_to_joint(joint, Eigen::Isometry3d::Identity()); } diff --git a/src/robot_dart/sensor/force_torque.hpp b/src/robot_dart/sensor/force_torque.hpp index 60039c31..509f8b60 100644 --- a/src/robot_dart/sensor/force_torque.hpp +++ b/src/robot_dart/sensor/force_torque.hpp @@ -7,8 +7,8 @@ namespace robot_dart { namespace sensor { class ForceTorque : public Sensor { public: - ForceTorque(RobotDARTSimu* simu, dart::dynamics::Joint* joint, size_t frequency = 1000, const std::string& direction = "child_to_parent"); - ForceTorque(RobotDARTSimu* simu, const std::shared_ptr& robot, const std::string& joint_name, size_t frequency = 1000, const std::string& direction = "child_to_parent") : ForceTorque(simu, robot->joint(joint_name), frequency, direction) {} + ForceTorque(dart::dynamics::Joint* joint, size_t frequency = 1000, const std::string& direction = "child_to_parent"); + ForceTorque(const std::shared_ptr& robot, const std::string& joint_name, size_t frequency = 1000, const std::string& direction = "child_to_parent") : ForceTorque(robot->joint(joint_name), frequency, direction) {} void init() override; diff --git a/src/robot_dart/sensor/imu.cpp b/src/robot_dart/sensor/imu.cpp index 7721c9a7..b385315c 100644 --- a/src/robot_dart/sensor/imu.cpp +++ b/src/robot_dart/sensor/imu.cpp @@ -6,7 +6,7 @@ namespace robot_dart { namespace sensor { - IMU::IMU(RobotDARTSimu* simu, const IMUConfig& config) : Sensor(simu, config.frequency), _config(config) {} + IMU::IMU(const IMUConfig& config) : Sensor(config.frequency), _config(config) {} void IMU::init() { @@ -14,7 +14,8 @@ namespace robot_dart { _linear_accel.setZero(); attach_to_body(_config.body, Eigen::Isometry3d::Identity()); - _active = true; + if (_simu) + _active = true; } void IMU::calculate(double t) diff --git a/src/robot_dart/sensor/imu.hpp b/src/robot_dart/sensor/imu.hpp index 511a900d..24e7c28f 100644 --- a/src/robot_dart/sensor/imu.hpp +++ b/src/robot_dart/sensor/imu.hpp @@ -25,7 +25,7 @@ namespace robot_dart { class IMU : public Sensor { public: - IMU(RobotDARTSimu* simu, const IMUConfig& config); + IMU(const IMUConfig& config); void init() override; diff --git a/src/robot_dart/sensor/sensor.cpp b/src/robot_dart/sensor/sensor.cpp index 5d552ab9..7e6be0e4 100644 --- a/src/robot_dart/sensor/sensor.cpp +++ b/src/robot_dart/sensor/sensor.cpp @@ -7,13 +7,7 @@ namespace robot_dart { namespace sensor { - Sensor::Sensor(RobotDARTSimu* simu, size_t freq) : _simu(simu), _active(false), _frequency(freq), _world_pose(Eigen::Isometry3d::Identity()), _attaching_to_body(false), _attached_to_body(false), _attaching_to_joint(false), _attached_to_joint(false) - { - bool check = static_cast(_frequency) > simu->physics_freq(); - ROBOT_DART_WARNING(check, "Sensor frequency is bigger than simulation physics. Setting it to simulation rate!"); - if (check) - _frequency = simu->physics_freq(); - } + Sensor::Sensor(size_t freq) : _active(false), _frequency(freq), _world_pose(Eigen::Isometry3d::Identity()), _attaching_to_body(false), _attached_to_body(false), _attaching_to_joint(false), _attached_to_joint(false) {} void Sensor::activate(bool enable) { @@ -28,6 +22,20 @@ namespace robot_dart { return _active; } + void Sensor::set_simu(RobotDARTSimu* simu) + { + _simu = simu; + bool check = static_cast(_frequency) > simu->physics_freq(); + ROBOT_DART_WARNING(check, "Sensor frequency is bigger than simulation physics. Setting it to simulation rate!"); + if (check) + _frequency = simu->physics_freq(); + } + + RobotDARTSimu* Sensor::simu() + { + return _simu; + } + size_t Sensor::frequency() const { return _frequency; } void Sensor::set_frequency(size_t freq) { _frequency = freq; } diff --git a/src/robot_dart/sensor/sensor.hpp b/src/robot_dart/sensor/sensor.hpp index a0b4ec5e..2fd42652 100644 --- a/src/robot_dart/sensor/sensor.hpp +++ b/src/robot_dart/sensor/sensor.hpp @@ -23,12 +23,15 @@ namespace robot_dart { namespace sensor { class Sensor { public: - Sensor(RobotDARTSimu* simu, size_t freq = 40); + Sensor(size_t freq = 40); virtual ~Sensor() {} void activate(bool enable = true); bool active() const; + void set_simu(RobotDARTSimu* simu); + RobotDARTSimu* simu(); + size_t frequency() const; void set_frequency(size_t freq); @@ -50,7 +53,7 @@ namespace robot_dart { void attach_to_joint(const std::shared_ptr& robot, const std::string& joint_name, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()) { attach_to_joint(robot->joint(joint_name), tf); } protected: - RobotDARTSimu* _simu; + RobotDARTSimu* _simu = nullptr; bool _active; size_t _frequency; From cd88a10ca63171b3b3ae0c23cd4a05734127c885 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 10 Oct 2020 16:07:54 +0300 Subject: [PATCH 2/5] [tests]: fix sensor test --- src/tests/test_sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/tests/test_sensors.cpp b/src/tests/test_sensors.cpp index 6a7a9fa3..da199708 100644 --- a/src/tests/test_sensors.cpp +++ b/src/tests/test_sensors.cpp @@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(test_imu) robot_dart::sensor::IMUConfig imu_config; imu_config.body = robot->body_node("box"); imu_config.frequency = 1000; // big update rate to get result in one time-step - auto imu_sensor = simu.add_sensor(&simu, imu_config); + auto imu_sensor = simu.add_sensor(imu_config); // Test IMU zero linear acceleration on free-fall // Do one step @@ -134,7 +134,7 @@ BOOST_AUTO_TEST_CASE(test_force_torque) robot->set_positions(pos); // Add a force/torque sensor - auto ft_sensor = simu.add_sensor(&simu, robot, "joint_1"); + auto ft_sensor = simu.add_sensor(robot, "joint_1"); // Do a few steps to hit the limit for (int i = 0; i < 5000; i++) From f2647afeed0c070d3407e593d91b5ffb74763785 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 10 Oct 2020 16:30:44 +0300 Subject: [PATCH 3/5] [cmake]: adapt to new API --- cmake/example/example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/example/example.cpp b/cmake/example/example.cpp index c8e77cd7..31adeb98 100644 --- a/cmake/example/example.cpp +++ b/cmake/example/example.cpp @@ -42,7 +42,7 @@ int main() robot_dart::RobotDARTSimu simu; #ifdef GRAPHIC - simu.set_graphics(std::make_shared(&simu)); + simu.set_graphics(std::make_shared()); #endif simu.add_descriptor(std::make_shared(&simu)); simu.add_robot(g_robot); From 7147c766e6b1e0330463a6a327ac81ed968b8850 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 10 Oct 2020 16:44:34 +0300 Subject: [PATCH 4/5] [clean]: make descriptors not requiring simu as well --- cmake/example/example.cpp | 4 ++-- src/examples/arm.cpp | 4 ++-- src/examples/pendulum.cpp | 2 +- src/python/example.py | 8 ++++---- src/python/simu.cpp | 3 ++- src/robot_dart/descriptor/base_descriptor.cpp | 2 +- src/robot_dart/descriptor/base_descriptor.hpp | 3 ++- src/robot_dart/gui/base.hpp | 2 +- src/robot_dart/robot_dart_simu.cpp | 1 + src/robot_dart/robot_dart_simu.hpp | 2 +- src/robot_dart/sensor/sensor.cpp | 2 +- src/robot_dart/sensor/sensor.hpp | 2 +- 12 files changed, 19 insertions(+), 16 deletions(-) diff --git a/cmake/example/example.cpp b/cmake/example/example.cpp index 31adeb98..f26b0286 100644 --- a/cmake/example/example.cpp +++ b/cmake/example/example.cpp @@ -9,7 +9,7 @@ #endif struct StateDesc : public robot_dart::descriptor::BaseDescriptor { - StateDesc(robot_dart::RobotDARTSimu* simu, size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(simu, desc_dump) {} + StateDesc(size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(desc_dump) {} void operator()() { @@ -44,7 +44,7 @@ int main() #ifdef GRAPHIC simu.set_graphics(std::make_shared()); #endif - simu.add_descriptor(std::make_shared(&simu)); + simu.add_descriptor(std::make_shared()); simu.add_robot(g_robot); std::cout << (g_robot->body_pose("arm_link_5") * size).transpose() << std::endl; simu.run(2.5); diff --git a/src/examples/arm.cpp b/src/examples/arm.cpp index 31adeb98..f26b0286 100644 --- a/src/examples/arm.cpp +++ b/src/examples/arm.cpp @@ -9,7 +9,7 @@ #endif struct StateDesc : public robot_dart::descriptor::BaseDescriptor { - StateDesc(robot_dart::RobotDARTSimu* simu, size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(simu, desc_dump) {} + StateDesc(size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(desc_dump) {} void operator()() { @@ -44,7 +44,7 @@ int main() #ifdef GRAPHIC simu.set_graphics(std::make_shared()); #endif - simu.add_descriptor(std::make_shared(&simu)); + simu.add_descriptor(std::make_shared()); simu.add_robot(g_robot); std::cout << (g_robot->body_pose("arm_link_5") * size).transpose() << std::endl; simu.run(2.5); diff --git a/src/examples/pendulum.cpp b/src/examples/pendulum.cpp index 054bc6e4..0d78ad1b 100644 --- a/src/examples/pendulum.cpp +++ b/src/examples/pendulum.cpp @@ -8,7 +8,7 @@ #endif struct StateDesc : public robot_dart::descriptor::BaseDescriptor { - StateDesc(robot_dart::RobotDARTSimu* simu, size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(simu, desc_dump) {} + StateDesc(size_t desc_dump = 1) : robot_dart::descriptor::BaseDescriptor(desc_dump) {} void operator()() { diff --git a/src/python/example.py b/src/python/example.py index b6e1b418..a612a5b4 100644 --- a/src/python/example.py +++ b/src/python/example.py @@ -23,8 +23,8 @@ def clone(self): return MyController(self._ctrl, self._controllable_dofs) class MyDesc(rd.Descriptor): - def __init__(self, simu, desc): - rd.Descriptor.__init__(self, simu, desc) + def __init__(self, desc): + rd.Descriptor.__init__(self, desc) self._states = [] def __call__(self): @@ -45,7 +45,7 @@ def __call__(self): # Create simulator object simu = rd.RobotDARTSimu(0.001) -desc = MyDesc(simu, 10) +desc = MyDesc(10) simu.add_descriptor(desc) # Create graphics @@ -61,7 +61,7 @@ def __call__(self): simu.add_checkerboard_floor(10., 0.1, 1., np.zeros((6,1)), "floor") # Add a camera sensor to the end-effector of the manipulator -camera = rd.sensor.Camera(simu, graphics.magnum_app(), 256, 256) +camera = rd.sensor.Camera(graphics.magnum_app(), 256, 256) rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.]).to_rotation_matrix() rot = rot.dot(dartpy.math.AngleAxis(1.57, [0., 0., 1.]).to_rotation_matrix()) camera.attach_to_body(robot.body_node("arm_link_5"), dartpy.math.Isometry3(rotation=rot, translation=[0., 0., 0.1])) diff --git a/src/python/simu.cpp b/src/python/simu.cpp index b0bfd772..ee790734 100644 --- a/src/python/simu.cpp +++ b/src/python/simu.cpp @@ -38,7 +38,8 @@ namespace robot_dart { }; py::class_>(m, "Descriptor") - .def(py::init()) + .def(py::init(), + py::arg("desc_dump") = 1) .def_readwrite("_desc_period", &PublicistDescriptor::_desc_period) .def_readonly("_simu", &PublicistDescriptor::_simu) diff --git a/src/robot_dart/descriptor/base_descriptor.cpp b/src/robot_dart/descriptor/base_descriptor.cpp index 436d9d1c..a3168996 100644 --- a/src/robot_dart/descriptor/base_descriptor.cpp +++ b/src/robot_dart/descriptor/base_descriptor.cpp @@ -4,7 +4,7 @@ namespace robot_dart { namespace descriptor { - BaseDescriptor::BaseDescriptor(RobotDARTSimu* simu, size_t desc_dump) : _simu(simu), _desc_period(desc_dump) {} + BaseDescriptor::BaseDescriptor(size_t desc_dump) : _desc_period(desc_dump) {} size_t BaseDescriptor::desc_dump() const { diff --git a/src/robot_dart/descriptor/base_descriptor.hpp b/src/robot_dart/descriptor/base_descriptor.hpp index 99d53cf5..ffa9b8f1 100644 --- a/src/robot_dart/descriptor/base_descriptor.hpp +++ b/src/robot_dart/descriptor/base_descriptor.hpp @@ -11,7 +11,7 @@ namespace robot_dart { struct BaseDescriptor { public: - BaseDescriptor(RobotDARTSimu* simu, size_t desc_dump = 1); + BaseDescriptor(size_t desc_dump = 1); virtual ~BaseDescriptor() {} virtual void operator()() = 0; @@ -19,6 +19,7 @@ namespace robot_dart { size_t desc_dump() const; void set_desc_dump(size_t desc_dump); + void set_simu(RobotDARTSimu* simu) { _simu = simu; } const RobotDARTSimu* simu() const { return _simu; } protected: diff --git a/src/robot_dart/gui/base.hpp b/src/robot_dart/gui/base.hpp index 77fe23b4..6beadc94 100644 --- a/src/robot_dart/gui/base.hpp +++ b/src/robot_dart/gui/base.hpp @@ -14,7 +14,7 @@ namespace robot_dart { virtual ~Base() {} virtual void set_simu(RobotDARTSimu* simu) { _simu = simu; } - RobotDARTSimu* simu() { return _simu; } + const RobotDARTSimu* simu() const { return _simu; } virtual bool done() const { return false; } diff --git a/src/robot_dart/robot_dart_simu.cpp b/src/robot_dart/robot_dart_simu.cpp index 91dfcf91..d9d4815d 100644 --- a/src/robot_dart/robot_dart_simu.cpp +++ b/src/robot_dart/robot_dart_simu.cpp @@ -211,6 +211,7 @@ namespace robot_dart { void RobotDARTSimu::add_descriptor(const std::shared_ptr& desc) { _descriptors.push_back(desc); + desc->set_simu(this); } std::vector> RobotDARTSimu::descriptors() const diff --git a/src/robot_dart/robot_dart_simu.hpp b/src/robot_dart/robot_dart_simu.hpp index 375042ba..88613c9b 100644 --- a/src/robot_dart/robot_dart_simu.hpp +++ b/src/robot_dart/robot_dart_simu.hpp @@ -69,7 +69,7 @@ namespace robot_dart { template void add_descriptor(size_t desc_dump = 1) { - add_descriptor(std::make_shared(Descriptor{this, desc_dump})); + add_descriptor(std::make_shared(Descriptor{desc_dump})); } void add_descriptor(const std::shared_ptr& desc); diff --git a/src/robot_dart/sensor/sensor.cpp b/src/robot_dart/sensor/sensor.cpp index 7e6be0e4..40481f30 100644 --- a/src/robot_dart/sensor/sensor.cpp +++ b/src/robot_dart/sensor/sensor.cpp @@ -31,7 +31,7 @@ namespace robot_dart { _frequency = simu->physics_freq(); } - RobotDARTSimu* Sensor::simu() + const RobotDARTSimu* Sensor::simu() const { return _simu; } diff --git a/src/robot_dart/sensor/sensor.hpp b/src/robot_dart/sensor/sensor.hpp index 2fd42652..c0cfb8e0 100644 --- a/src/robot_dart/sensor/sensor.hpp +++ b/src/robot_dart/sensor/sensor.hpp @@ -30,7 +30,7 @@ namespace robot_dart { bool active() const; void set_simu(RobotDARTSimu* simu); - RobotDARTSimu* simu(); + const RobotDARTSimu* simu() const; size_t frequency() const; void set_frequency(size_t freq); From 59c4cf2da9c123b1fb38eea96971cd32fbcd4cb3 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 10 Oct 2020 23:47:27 +0300 Subject: [PATCH 5/5] [clean]: a bit more cleaning and exceptions/asserts --- src/robot_dart/gui/magnum/base_graphics.hpp | 85 +++++++++++++--- src/robot_dart/gui/magnum/sensor/camera.cpp | 1 + src/robot_dart/robot.cpp | 105 +++++++------------- src/robot_dart/sensor/imu.cpp | 1 + src/robot_dart/sensor/sensor.cpp | 1 + wscript | 2 +- 6 files changed, 112 insertions(+), 83 deletions(-) diff --git a/src/robot_dart/gui/magnum/base_graphics.hpp b/src/robot_dart/gui/magnum/base_graphics.hpp index 4e2401c6..ce5bf8ea 100644 --- a/src/robot_dart/gui/magnum/base_graphics.hpp +++ b/src/robot_dart/gui/magnum/base_graphics.hpp @@ -31,15 +31,26 @@ namespace robot_dart { virtual void set_simu(RobotDARTSimu* simu) override { + ROBOT_DART_EXCEPTION_ASSERT(simu, "Simulation pointer is null!"); _simu = simu; _magnum_app.reset(make_application(simu, _configuration)); } - size_t width() const override { return _magnum_app->camera().width(); } - size_t height() const override { return _magnum_app->camera().height(); } + size_t width() const override + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->camera().width(); + } + + size_t height() const override + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->camera().height(); + } bool done() const override { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); return _magnum_app->done(); } @@ -48,6 +59,7 @@ namespace robot_dart { if (!_enabled) return; + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); _magnum_app->render(); } @@ -62,48 +74,70 @@ namespace robot_dart { const Eigen::Vector3d& look_at = Eigen::Vector3d(0, 0, 0), const Eigen::Vector3d& up = Eigen::Vector3d(0, 0, 1)) { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); _magnum_app->look_at(camera_pos, look_at, up); } void clear_lights() { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); _magnum_app->clear_lights(); } void add_light(const magnum::gs::Light& light) { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); _magnum_app->add_light(light); } std::vector& lights() { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); return _magnum_app->lights(); } size_t num_lights() const { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); return _magnum_app->num_lights(); } magnum::gs::Light& light(size_t i) { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); return _magnum_app->light(i); } void record_video(const std::string& video_fname, int fps = -1) { int fps_computed = (fps == -1) ? _fps : fps; - ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(fps_computed != -1 && "Video FPS not set!"); + ROBOT_DART_EXCEPTION_ASSERT(fps_computed != -1, "Video FPS not set!"); + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); _magnum_app->record_video(video_fname, fps_computed); } - bool shadowed() const { return _magnum_app->shadowed(); } - bool transparent_shadows() const { return _magnum_app->transparent_shadows(); } - void enable_shadows(bool enable = true, bool transparent = true) { _magnum_app->enable_shadows(enable, transparent); } + bool shadowed() const + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->shadowed(); + } + + bool transparent_shadows() const + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->transparent_shadows(); + } + + void enable_shadows(bool enable = true, bool transparent = true) + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + _magnum_app->enable_shadows(enable, transparent); + } Magnum::Image2D* magnum_image() { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); if (_magnum_app->image()) return &(*_magnum_app->image()); return nullptr; @@ -117,14 +151,41 @@ namespace robot_dart { return Image(); } - GrayscaleImage depth_image() override { return _magnum_app->depth_image(); } - GrayscaleImage raw_depth_image() override { return _magnum_app->raw_depth_image(); } + GrayscaleImage depth_image() override + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->depth_image(); + } + + GrayscaleImage raw_depth_image() override + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->raw_depth_image(); + } + + gs::Camera& camera() + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->camera(); + } + + const gs::Camera& camera() const + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return _magnum_app->camera(); + } - gs::Camera& camera() { return _magnum_app->camera(); } - const gs::Camera& camera() const { return _magnum_app->camera(); } + BaseApplication* magnum_app() + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return &*_magnum_app; + } - BaseApplication* magnum_app() { return &*_magnum_app; } - const BaseApplication* magnum_app() const { return &*_magnum_app; } + const BaseApplication* magnum_app() const + { + ROBOT_DART_EXCEPTION_ASSERT(_magnum_app, "MagnumApp pointer is null!"); + return &*_magnum_app; + } protected: GraphicsConfiguration _configuration; diff --git a/src/robot_dart/gui/magnum/sensor/camera.cpp b/src/robot_dart/gui/magnum/sensor/camera.cpp index 686d6969..384c7af5 100644 --- a/src/robot_dart/gui/magnum/sensor/camera.cpp +++ b/src/robot_dart/gui/magnum/sensor/camera.cpp @@ -57,6 +57,7 @@ namespace robot_dart { void Camera::calculate(double) { + ROBOT_DART_EXCEPTION_ASSERT(_simu, "Simulation pointer is null!"); /* Update graphic meshes/materials and render */ _magnum_app->update_graphics(); /* Update lights transformations --- this also draws the shadows if enabled */ diff --git a/src/robot_dart/robot.cpp b/src/robot_dart/robot.cpp index 08497c66..82035d3d 100644 --- a/src/robot_dart/robot.cpp +++ b/src/robot_dart/robot.cpp @@ -84,8 +84,7 @@ namespace robot_dart { for (size_t i = 0; i < dof_names.size(); i++) { auto it = dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT(it != dof_map.end(), - "dof_data: " + dof_names[i] + " is not in dof_map", Eigen::VectorXd()); + ROBOT_DART_ASSERT(it != dof_map.end(), "dof_data: " + dof_names[i] + " is not in dof_map", Eigen::VectorXd()); auto dof = skeleton->getDof(it->second); if (content == 0) data(i) = dof->getPosition(); @@ -126,8 +125,7 @@ namespace robot_dart { { // Set all values if (dof_names.empty()) { - ROBOT_DART_ASSERT(static_cast(data.size()) == skeleton->getNumDofs(), - "set_dof_data: size of data is not the same as the DoFs", ); + ROBOT_DART_ASSERT(static_cast(data.size()) == skeleton->getNumDofs(), "set_dof_data: size of data is not the same as the DoFs", ); if (content == 0) return skeleton->setPositions(data); else if (content == 1) @@ -157,12 +155,10 @@ namespace robot_dart { ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } - ROBOT_DART_ASSERT(static_cast(data.size()) == dof_names.size(), - "set_dof_data: size of data is not the same as the dof_names size", ); + ROBOT_DART_ASSERT(static_cast(data.size()) == dof_names.size(), "set_dof_data: size of data is not the same as the dof_names size", ); for (size_t i = 0; i < dof_names.size(); i++) { auto it = dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT( - it != dof_map.end(), "dof_data: " + dof_names[i] + " is not in dof_map", ); + ROBOT_DART_ASSERT(it != dof_map.end(), "dof_data: " + dof_names[i] + " is not in dof_map", ); auto dof = skeleton->getDof(it->second); if (content == 0) dof->setPosition(data(i)); @@ -200,8 +196,7 @@ namespace robot_dart { { // Set all values if (dof_names.empty()) { - ROBOT_DART_ASSERT(static_cast(data.size()) == skeleton->getNumDofs(), - "set_dof_data: size of data is not the same as the DoFs", ); + ROBOT_DART_ASSERT(static_cast(data.size()) == skeleton->getNumDofs(), "set_dof_data: size of data is not the same as the DoFs", ); if (content == 0) return skeleton->setPositions(skeleton->getPositions() + data); else if (content == 1) @@ -231,12 +226,10 @@ namespace robot_dart { ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } - ROBOT_DART_ASSERT(static_cast(data.size()) == dof_names.size(), - "add_dof_data: size of data is not the same as the dof_names size", ); + ROBOT_DART_ASSERT(static_cast(data.size()) == dof_names.size(), "add_dof_data: size of data is not the same as the dof_names size", ); for (size_t i = 0; i < dof_names.size(); i++) { auto it = dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT( - it != dof_map.end(), "dof_data: " + dof_names[i] + " is not in dof_map", ); + ROBOT_DART_ASSERT(it != dof_map.end(), "dof_data: " + dof_names[i] + " is not in dof_map", ); auto dof = skeleton->getDof(it->second); if (content == 0) dof->setPosition(dof->getPosition() + data(i)); @@ -360,8 +353,7 @@ namespace robot_dart { dart::dynamics::BodyNode* Robot::body_node(size_t body_index) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", nullptr); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", nullptr); return _skeleton->getBodyNode(body_index); } @@ -369,8 +361,7 @@ namespace robot_dart { dart::dynamics::Joint* Robot::joint(size_t joint_index) { - ROBOT_DART_ASSERT( - joint_index < _skeleton->getNumJoints(), "Joint index out of bounds", nullptr); + ROBOT_DART_ASSERT(joint_index < _skeleton->getNumJoints(), "Joint index out of bounds", nullptr); return _skeleton->getJoint(joint_index); } @@ -489,16 +480,14 @@ namespace robot_dart { bool Robot::fixed() const { auto parent_jt = _skeleton->getRootBodyNode()->getParentJoint(); - ROBOT_DART_ASSERT( - parent_jt != nullptr, "RootBodyNode does not have a parent joint!", false); + ROBOT_DART_ASSERT(parent_jt != nullptr, "RootBodyNode does not have a parent joint!", false); return parent_jt->getType() == dart::dynamics::WeldJoint::getStaticType(); } bool Robot::free() const { auto parent_jt = _skeleton->getRootBodyNode()->getParentJoint(); - ROBOT_DART_ASSERT( - parent_jt != nullptr, "RootBodyNode does not have a parent joint!", false); + ROBOT_DART_ASSERT(parent_jt != nullptr, "RootBodyNode does not have a parent joint!", false); return parent_jt->getType() == dart::dynamics::FreeJoint::getStaticType(); } @@ -974,8 +963,7 @@ namespace robot_dart { void Robot::set_external_force(size_t body_index, const Eigen::Vector3d& force, const Eigen::Vector3d& offset, bool force_local, bool offset_local) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); auto bd = _skeleton->getBodyNode(body_index); bd->setExtForce(force, offset, force_local, offset_local); @@ -991,8 +979,7 @@ namespace robot_dart { void Robot::add_external_force(size_t body_index, const Eigen::Vector3d& force, const Eigen::Vector3d& offset, bool force_local, bool offset_local) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); auto bd = _skeleton->getBodyNode(body_index); bd->addExtForce(force, offset, force_local, offset_local); @@ -1008,8 +995,7 @@ namespace robot_dart { void Robot::set_external_torque(size_t body_index, const Eigen::Vector3d& torque, bool local) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); auto bd = _skeleton->getBodyNode(body_index); bd->setExtTorque(torque, local); @@ -1025,8 +1011,7 @@ namespace robot_dart { void Robot::add_external_torque(size_t body_index, const Eigen::Vector3d& torque, bool local) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); auto bd = _skeleton->getBodyNode(body_index); bd->addExtTorque(torque, local); @@ -1035,8 +1020,7 @@ namespace robot_dart { Eigen::Vector6d Robot::external_forces(const std::string& body_name) const { auto bd = _skeleton->getBodyNode(body_name); - ROBOT_DART_ASSERT( - bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); return bd->getExternalForceGlobal(); } @@ -1053,23 +1037,20 @@ namespace robot_dart { Eigen::Isometry3d Robot::body_pose(const std::string& body_name) const { auto bd = _skeleton->getBodyNode(body_name); - ROBOT_DART_ASSERT( - bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Isometry3d::Identity()); + ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Isometry3d::Identity()); return bd->getWorldTransform(); } Eigen::Isometry3d Robot::body_pose(size_t body_index) const { - ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", - Eigen::Isometry3d::Identity()); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", Eigen::Isometry3d::Identity()); return _skeleton->getBodyNode(body_index)->getWorldTransform(); } Eigen::Vector6d Robot::body_pose_vec(const std::string& body_name) const { auto bd = _skeleton->getBodyNode(body_name); - ROBOT_DART_ASSERT( - bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); Eigen::Isometry3d bd_trans = bd->getWorldTransform(); Eigen::Vector6d pose; @@ -1081,8 +1062,7 @@ namespace robot_dart { Eigen::Vector6d Robot::body_pose_vec(size_t body_index) const { - ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", - Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", Eigen::Vector6d::Zero()); Eigen::Isometry3d bd_trans = _skeleton->getBodyNode(body_index)->getWorldTransform(); @@ -1096,30 +1076,26 @@ namespace robot_dart { Eigen::Vector6d Robot::body_velocity(const std::string& body_name) const { auto bd = _skeleton->getBodyNode(body_name); - ROBOT_DART_ASSERT( - bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); return bd->getSpatialVelocity(dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); } Eigen::Vector6d Robot::body_velocity(size_t body_index) const { - ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", - Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", Eigen::Vector6d::Zero()); return _skeleton->getBodyNode(body_index)->getSpatialVelocity(dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); } Eigen::Vector6d Robot::body_acceleration(const std::string& body_name) const { auto bd = _skeleton->getBodyNode(body_name); - ROBOT_DART_ASSERT( - bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); return bd->getSpatialAcceleration(dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); } Eigen::Vector6d Robot::body_acceleration(size_t body_index) const { - ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", - Eigen::Vector6d::Zero()); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", Eigen::Vector6d::Zero()); return _skeleton->getBodyNode(body_index)->getSpatialAcceleration(dart::dynamics::Frame::World(), dart::dynamics::Frame::World()); } @@ -1133,15 +1109,13 @@ namespace robot_dart { std::string Robot::body_name(size_t body_index) const { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ""); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ""); return _skeleton->getBodyNode(body_index)->getName(); } void Robot::set_body_name(size_t body_index, const std::string& body_name) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); _skeleton->getBodyNode(body_index)->setName(body_name); } @@ -1154,8 +1128,7 @@ namespace robot_dart { double Robot::body_mass(size_t body_index) const { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", 0.); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", 0.); return _skeleton->getBodyNode(body_index)->getMass(); } @@ -1168,8 +1141,7 @@ namespace robot_dart { void Robot::set_body_mass(size_t body_index, double mass) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); _skeleton->getBodyNode(body_index)->setMass(mass); // TO-DO: Recompute inertia? } @@ -1182,8 +1154,7 @@ namespace robot_dart { void Robot::add_body_mass(size_t body_index, double mass) { - ROBOT_DART_ASSERT( - body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); + ROBOT_DART_ASSERT(body_index < _skeleton->getNumBodyNodes(), "BodyNode index out of bounds", ); auto bd = _skeleton->getBodyNode(body_index); bd->setMass(mass + bd->getMass()); // TO-DO: Recompute inertia? } @@ -1272,8 +1243,7 @@ namespace robot_dart { Eigen::VectorXd ret(dof_names.size()); for (size_t i = 0; i < dof_names.size(); i++) { auto it = _dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT( - it != _dof_map.end(), "vec_dof: " + dof_names[i] + " is not in dof_map", Eigen::VectorXd()); + ROBOT_DART_ASSERT(it != _dof_map.end(), "vec_dof: " + dof_names[i] + " is not in dof_map", Eigen::VectorXd()); ret(i) = vec[it->second]; } @@ -1370,8 +1340,7 @@ namespace robot_dart { ROBOT_DART_ASSERT(false, "dof_index : " + dof_name + " is not in the skeleton", 0); } auto it = _dof_map.find(dof_name); - ROBOT_DART_ASSERT( - it != _dof_map.end(), "dof_index : " + dof_name + " is not in DoF map", 0); + ROBOT_DART_ASSERT(it != _dof_map.end(), "dof_index : " + dof_name + " is not in DoF map", 0); return it->second; } @@ -1409,8 +1378,7 @@ namespace robot_dart { ROBOT_DART_ASSERT(false, "joint_index : " + joint_name + " is not in the skeleton", 0); } auto it = _joint_map.find(joint_name); - ROBOT_DART_ASSERT( - it != _joint_map.end(), "joint_index : " + joint_name + " is not in Joint map", 0); + ROBOT_DART_ASSERT(it != _joint_map.end(), "joint_index : " + joint_name + " is not in Joint map", 0); return it->second; } @@ -1685,8 +1653,7 @@ namespace robot_dart { for (size_t i = 0; i < dof_names.size(); i++) { auto it = _dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT( - it != _dof_map.end(), "_jacobian: " + dof_names[i] + " is not in dof_map", Eigen::MatrixXd()); + ROBOT_DART_ASSERT(it != _dof_map.end(), "_jacobian: " + dof_names[i] + " is not in dof_map", Eigen::MatrixXd()); jac_ret.col(i) = full_jacobian.col(it->second); } @@ -1703,15 +1670,13 @@ namespace robot_dart { for (size_t i = 0; i < dof_names.size(); i++) { auto it = _dof_map.find(dof_names[i]); - ROBOT_DART_ASSERT( - it != _dof_map.end(), "mass_matrix: " + dof_names[i] + " is not in dof_map", Eigen::MatrixXd()); + ROBOT_DART_ASSERT(it != _dof_map.end(), "mass_matrix: " + dof_names[i] + " is not in dof_map", Eigen::MatrixXd()); M_ret(i, i) = full_mass_matrix(it->second, it->second); for (size_t j = i + 1; j < dof_names.size(); j++) { auto it2 = _dof_map.find(dof_names[j]); - ROBOT_DART_ASSERT( - it2 != _dof_map.end(), "mass_matrix: " + dof_names[j] + " is not in dof_map", Eigen::MatrixXd()); + ROBOT_DART_ASSERT(it2 != _dof_map.end(), "mass_matrix: " + dof_names[j] + " is not in dof_map", Eigen::MatrixXd()); M_ret(i, j) = full_mass_matrix(it->second, it2->second); M_ret(j, i) = full_mass_matrix(it2->second, it->second); diff --git a/src/robot_dart/sensor/imu.cpp b/src/robot_dart/sensor/imu.cpp index b385315c..2d434361 100644 --- a/src/robot_dart/sensor/imu.cpp +++ b/src/robot_dart/sensor/imu.cpp @@ -22,6 +22,7 @@ namespace robot_dart { { if (!_attached_to_body) return; // cannot compute anything if not attached to a link + ROBOT_DART_EXCEPTION_ASSERT(_simu, "Simulation pointer is null!"); _angular_vel = _body_attached->getSpatialVelocity().head(3); // angular velocity with respect to the world, in local coordinates _linear_accel = _body_attached->getSpatialAcceleration().tail(3); // linear acceleration with respect to the world, in local coordinates diff --git a/src/robot_dart/sensor/sensor.cpp b/src/robot_dart/sensor/sensor.cpp index 40481f30..39f5b6fa 100644 --- a/src/robot_dart/sensor/sensor.cpp +++ b/src/robot_dart/sensor/sensor.cpp @@ -24,6 +24,7 @@ namespace robot_dart { void Sensor::set_simu(RobotDARTSimu* simu) { + ROBOT_DART_EXCEPTION_ASSERT(simu, "Simulation pointer is null!"); _simu = simu; bool check = static_cast(_frequency) > simu->physics_freq(); ROBOT_DART_WARNING(check, "Sensor frequency is bigger than simulation physics. Setting it to simulation rate!"); diff --git a/wscript b/wscript index 10e436da..13bff5b1 100644 --- a/wscript +++ b/wscript @@ -340,7 +340,7 @@ def build_examples(bld): # these examples should not be compiled without magnum magnum_only = ['magnum_contexts.cpp', 'cameras.cpp', 'transparent.cpp'] # these examples should be compiled only without grpahics - simu_only = ['scheduler.cpp'] + simu_only = ['scheduler.cpp', 'robot_pool.cpp'] # these examples have their own rules exclude = []