From 4d8b5472979b3b9896d189fcfebeba462d410003 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sat, 26 Sep 2020 22:12:11 +0200 Subject: [PATCH 1/6] First version of the generic robot pool --- src/examples/robot_pool.cpp | 76 +++++++++++++++++++++++++++++++++++ src/robot_dart/robot_pool.cpp | 51 +++++++++++++++++++++++ src/robot_dart/robot_pool.hpp | 39 ++++++++++++++++++ 3 files changed, 166 insertions(+) create mode 100644 src/examples/robot_pool.cpp create mode 100644 src/robot_dart/robot_pool.cpp create mode 100644 src/robot_dart/robot_pool.hpp diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp new file mode 100644 index 00000000..9eaeb6ce --- /dev/null +++ b/src/examples/robot_pool.cpp @@ -0,0 +1,76 @@ +#include +#include +#include +#include + +#include +#include + +static constexpr int NUM_THREADS = 12; + +void eval_robot(int i) +{ + // you can also use a standard function instead of a lambda + auto robot_creator = [] { + std::vector> packages = {{"talos_description", "talos/talos_description"}}; + return std::make_shared("talos/talos.urdf", packages); + }; // do not forget the ; for lambda! + // we want 12 robots in the pool(usually the number of threads or a bit more ) + auto robot + = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); + + std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; + + + /// --- some robot_dart code --- + + robot->set_position_enforced(true); + robot->skeleton()->setPosition(5, 1.1); + robot->skeleton()->setPosition(2, 1.57); + + // Set actuator types to VELOCITY (for speed) + robot->set_actuator_types("velocity"); + + double dt = 0.001; + robot_dart::RobotDARTSimu simu(dt); + simu.set_collision_detector("fcl"); + simu.add_checkerboard_floor(); + simu.add_robot(robot); + + simu.set_control_freq(100); + std::vector dofs = {"arm_left_1_joint", + "arm_left_2_joint", + "arm_right_1_joint", + "arm_right_2_joint", + "torso_1_joint"}; + + Eigen::VectorXd init_positions = robot->positions(dofs); + + while (simu.scheduler().next_time() < 5) {// 5 second of simu only + if (simu.schedule(simu.control_freq())) { + Eigen::VectorXd delta_pos(5); + delta_pos << sin(simu.scheduler().current_time() * 2.), + sin(simu.scheduler().current_time() * 2.), + sin(simu.scheduler().current_time() * 2.), + sin(simu.scheduler().current_time() * 2.), + sin(simu.scheduler().current_time() * 2.); + Eigen::VectorXd commands = (init_positions + delta_pos) - robot->positions(dofs); + robot->set_commands(commands, dofs); + } + simu.step_world(); + } + // --- do something with the result +} + +int main(int argc, char** argv) +{ + // for the example, we run NUM_THREADS threads of eval_robot() + std::vector threads(NUM_THREADS); + for (size_t i = 0; i < threads.size(); ++i) + threads[i] = std::thread(eval_robot, i); + + // wait for the threads to finish + for (size_t i = 0; i < threads.size(); ++i) + threads[i].join(); + return 0; +} diff --git a/src/robot_dart/robot_pool.cpp b/src/robot_dart/robot_pool.cpp new file mode 100644 index 00000000..bf16c830 --- /dev/null +++ b/src/robot_dart/robot_pool.cpp @@ -0,0 +1,51 @@ +#include + +namespace robot_dart { + RobotPool::RobotPool(const std::function()>& robot_creator, size_t pool_size) : _robot_creator(robot_creator), _pool_size(pool_size) + { + for (size_t i = 0; i < _pool_size; ++i) { + auto robot = robot_creator(); + _set_init_pos(robot->skeleton()); + _skeletons.push_back(robot->skeleton()); + } + for (size_t i = 0; i < _pool_size; i++) + _free.push_back(true); + } + + std::shared_ptr RobotPool::get_robot(const std::string& name) + { + std::lock_guard lock(_skeleton_mutex); + while (true) { + for (size_t i = 0; i < _pool_size; i++) + if (_free[i]) { + _free[i] = false; + return std::make_shared(_skeletons[i], name); + } + } + } + + void RobotPool::free_robot(const std::shared_ptr& robot) + { + std::lock_guard lock(_skeleton_mutex); + for (size_t i = 0; i < _pool_size; i++) { + if (_skeletons[i] == robot->skeleton()) { + _set_init_pos(_skeletons[i]); + _free[i] = true; + break; + } + } + } + + void RobotPool::_set_init_pos(const dart::dynamics::SkeletonPtr& skel) + { + skel->resetPositions(); + skel->resetVelocities(); + skel->resetAccelerations(); + skel->clearExternalForces(); + skel->clearInternalForces(); + skel->clearConstraintImpulses(); + skel->resetCommands(); + + // reset any altered properties (e.g., altered mass of a link) and set initial positions + } +} // namespace robot_dart diff --git a/src/robot_dart/robot_pool.hpp b/src/robot_dart/robot_pool.hpp new file mode 100644 index 00000000..94dd23af --- /dev/null +++ b/src/robot_dart/robot_pool.hpp @@ -0,0 +1,39 @@ +#ifndef _TALOS_CLONE_POOL +#define _TALOS_CLONE_POOL + +#include +#include +#include +#include + +#include + +namespace robot_dart { + class RobotPool { + public: + using robot_creator_t = std::function()>; + static RobotPool& instance(const robot_creator_t& robot_creator, size_t pool_size = 32) + { + static RobotPool gdata(robot_creator, pool_size); + return gdata; + } + RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32); + virtual ~RobotPool() {} + RobotPool(const RobotPool&) = delete; + void operator=(const RobotPool&) = delete; + + virtual std::shared_ptr get_robot(const std::string& name = "robot"); + virtual void free_robot(const std::shared_ptr& robot); + + protected: + virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel); + robot_creator_t _robot_creator; + size_t _pool_size; + std::vector _skeletons; + std::vector _free; + std::mutex _skeleton_mutex; + std::string _model_filename; + }; +} // namespace robot_dart + +#endif From a6dee48cc2ab3168b91ea42993b9a9ec6b9817d8 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Sat, 26 Sep 2020 22:15:45 +0200 Subject: [PATCH 2/6] free the robot --- src/examples/robot_pool.cpp | 26 ++++++++++++++++---------- src/examples/talos.cpp | 2 +- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp index 9eaeb6ce..88679aa6 100644 --- a/src/examples/robot_pool.cpp +++ b/src/examples/robot_pool.cpp @@ -20,8 +20,7 @@ void eval_robot(int i) = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; - - + /// --- some robot_dart code --- robot->set_position_enforced(true); @@ -46,7 +45,7 @@ void eval_robot(int i) Eigen::VectorXd init_positions = robot->positions(dofs); - while (simu.scheduler().next_time() < 5) {// 5 second of simu only + while (simu.scheduler().next_time() < 5) { // 5 second of simu only if (simu.schedule(simu.control_freq())) { Eigen::VectorXd delta_pos(5); delta_pos << sin(simu.scheduler().current_time() * 2.), @@ -60,17 +59,24 @@ void eval_robot(int i) simu.step_world(); } // --- do something with the result + + // CRITICAL : free your robot ! + robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).free_robot(robot); + } int main(int argc, char** argv) { - // for the example, we run NUM_THREADS threads of eval_robot() - std::vector threads(NUM_THREADS); - for (size_t i = 0; i < threads.size(); ++i) - threads[i] = std::thread(eval_robot, i); + for (int i = 0; i < 2; ++i) { // we do it twice to see reuse of the some robots + // for the example, we run NUM_THREADS threads of eval_robot() + std::vector threads(NUM_THREADS); + for (size_t i = 0; i < threads.size(); ++i) + threads[i] = std::thread(eval_robot, i); - // wait for the threads to finish - for (size_t i = 0; i < threads.size(); ++i) - threads[i].join(); + // wait for the threads to finish + for (size_t i = 0; i < threads.size(); ++i) + threads[i].join(); + std::cout << "first batch finished" << std::endl; + } return 0; } diff --git a/src/examples/talos.cpp b/src/examples/talos.cpp index 42ebbe94..9f7becaa 100644 --- a/src/examples/talos.cpp +++ b/src/examples/talos.cpp @@ -33,7 +33,7 @@ int main() auto graphics = std::make_shared(&simu); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); - graphics->record_video("talos_dancing.mp4"); + //graphics->record_video("talos_dancing.mp4"); #endif simu.add_checkerboard_floor(); simu.add_robot(robot); From 171c02e32c5a5b0f6c6a83c45b07e04d152a8947 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Mon, 28 Sep 2020 11:45:56 +0300 Subject: [PATCH 3/6] [pool]: formatting and linking flag fix --- src/examples/robot_pool.cpp | 18 ++++++++---------- src/examples/talos.cpp | 2 +- src/robot_dart/robot_pool.cpp | 2 -- src/robot_dart/robot_pool.hpp | 6 +++++- wscript | 4 ++-- 5 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp index 88679aa6..2276f6ee 100644 --- a/src/examples/robot_pool.cpp +++ b/src/examples/robot_pool.cpp @@ -16,13 +16,11 @@ void eval_robot(int i) return std::make_shared("talos/talos.urdf", packages); }; // do not forget the ; for lambda! // we want 12 robots in the pool(usually the number of threads or a bit more ) - auto robot - = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); + auto robot = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; /// --- some robot_dart code --- - robot->set_position_enforced(true); robot->skeleton()->setPosition(5, 1.1); robot->skeleton()->setPosition(2, 1.57); @@ -37,7 +35,8 @@ void eval_robot(int i) simu.add_robot(robot); simu.set_control_freq(100); - std::vector dofs = {"arm_left_1_joint", + std::vector dofs = { + "arm_left_1_joint", "arm_left_2_joint", "arm_right_1_joint", "arm_right_2_joint", @@ -62,7 +61,6 @@ void eval_robot(int i) // CRITICAL : free your robot ! robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).free_robot(robot); - } int main(int argc, char** argv) @@ -70,13 +68,13 @@ int main(int argc, char** argv) for (int i = 0; i < 2; ++i) { // we do it twice to see reuse of the some robots // for the example, we run NUM_THREADS threads of eval_robot() std::vector threads(NUM_THREADS); - for (size_t i = 0; i < threads.size(); ++i) - threads[i] = std::thread(eval_robot, i); + for (size_t j = 0; j < threads.size(); ++j) + threads[j] = std::thread(eval_robot, j); // wait for the threads to finish - for (size_t i = 0; i < threads.size(); ++i) - threads[i].join(); - std::cout << "first batch finished" << std::endl; + for (size_t j = 0; j < threads.size(); ++j) + threads[j].join(); + std::cout << "Batch " << i << " finished" << std::endl; } return 0; } diff --git a/src/examples/talos.cpp b/src/examples/talos.cpp index 9f7becaa..42ebbe94 100644 --- a/src/examples/talos.cpp +++ b/src/examples/talos.cpp @@ -33,7 +33,7 @@ int main() auto graphics = std::make_shared(&simu); simu.set_graphics(graphics); graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25}); - //graphics->record_video("talos_dancing.mp4"); + graphics->record_video("talos_dancing.mp4"); #endif simu.add_checkerboard_floor(); simu.add_robot(robot); diff --git a/src/robot_dart/robot_pool.cpp b/src/robot_dart/robot_pool.cpp index bf16c830..3015e34d 100644 --- a/src/robot_dart/robot_pool.cpp +++ b/src/robot_dart/robot_pool.cpp @@ -45,7 +45,5 @@ namespace robot_dart { skel->clearInternalForces(); skel->clearConstraintImpulses(); skel->resetCommands(); - - // reset any altered properties (e.g., altered mass of a link) and set initial positions } } // namespace robot_dart diff --git a/src/robot_dart/robot_pool.hpp b/src/robot_dart/robot_pool.hpp index 94dd23af..0c8096bc 100644 --- a/src/robot_dart/robot_pool.hpp +++ b/src/robot_dart/robot_pool.hpp @@ -12,13 +12,16 @@ namespace robot_dart { class RobotPool { public: using robot_creator_t = std::function()>; + static RobotPool& instance(const robot_creator_t& robot_creator, size_t pool_size = 32) { static RobotPool gdata(robot_creator, pool_size); return gdata; } + RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32); virtual ~RobotPool() {} + RobotPool(const RobotPool&) = delete; void operator=(const RobotPool&) = delete; @@ -26,13 +29,14 @@ namespace robot_dart { virtual void free_robot(const std::shared_ptr& robot); protected: - virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel); robot_creator_t _robot_creator; size_t _pool_size; std::vector _skeletons; std::vector _free; std::mutex _skeleton_mutex; std::string _model_filename; + + virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel); }; } // namespace robot_dart diff --git a/wscript b/wscript index 7ec9e8b5..10e436da 100644 --- a/wscript +++ b/wscript @@ -333,7 +333,7 @@ def build_examples(bld): # we first build the library build(bld) print("Bulding examples...") - libs = 'BOOST EIGEN DART' + libs = 'BOOST EIGEN DART PTHREAD' path = bld.path.abspath() + '/res' bld.env.LIB_PTHREAD = ['pthread'] @@ -365,7 +365,7 @@ def build_examples(bld): install_path = None, source = '/src/examples/' + filename, includes = './src', - uselib = 'PTHREAD ' + bld.env['magnum_libs'] + libs, + uselib = bld.env['magnum_libs'] + libs, use = 'RobotDARTSimu RobotDARTMagnum', defines = ['GRAPHIC'], target = basename) From 9e49dba6a9ae2fa1d7ae1cdecc46f72d61e112c1 Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Mon, 28 Sep 2020 13:24:57 +0200 Subject: [PATCH 4/6] remove the singleton from the robot pool --- src/examples/robot_pool.cpp | 56 ++++++++++++++++++----------------- src/robot_dart/robot_pool.cpp | 14 ++++++++- src/robot_dart/robot_pool.hpp | 8 ++--- 3 files changed, 44 insertions(+), 34 deletions(-) diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp index 88679aa6..4637150c 100644 --- a/src/examples/robot_pool.cpp +++ b/src/examples/robot_pool.cpp @@ -8,20 +8,8 @@ static constexpr int NUM_THREADS = 12; -void eval_robot(int i) +void simulate_robot(const std::shared_ptr& robot) { - // you can also use a standard function instead of a lambda - auto robot_creator = [] { - std::vector> packages = {{"talos_description", "talos/talos_description"}}; - return std::make_shared("talos/talos.urdf", packages); - }; // do not forget the ; for lambda! - // we want 12 robots in the pool(usually the number of threads or a bit more ) - auto robot - = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); - - std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; - - /// --- some robot_dart code --- robot->set_position_enforced(true); robot->skeleton()->setPosition(5, 1.1); @@ -45,7 +33,7 @@ void eval_robot(int i) Eigen::VectorXd init_positions = robot->positions(dofs); - while (simu.scheduler().next_time() < 5) { // 5 second of simu only + while (simu.scheduler().next_time() < 3) { // 3 second of simu only if (simu.schedule(simu.control_freq())) { Eigen::VectorXd delta_pos(5); delta_pos << sin(simu.scheduler().current_time() * 2.), @@ -58,25 +46,39 @@ void eval_robot(int i) } simu.step_world(); } +} + + +namespace pool { + std::shared_ptr robot_creator() { + std::vector> packages = {{"talos_description", "talos/talos_description"}}; + return std::make_shared("talos/talos.urdf", packages); + } + robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS); +} + +void eval_robot(int i) +{ + auto robot = pool::robot_pool.get_robot(); + std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; + + /// --- some robot_dart code --- + simulate_robot(robot); // --- do something with the result // CRITICAL : free your robot ! - robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).free_robot(robot); - + pool::robot_pool.free_robot(robot); } int main(int argc, char** argv) { - for (int i = 0; i < 2; ++i) { // we do it twice to see reuse of the some robots - // for the example, we run NUM_THREADS threads of eval_robot() - std::vector threads(NUM_THREADS); - for (size_t i = 0; i < threads.size(); ++i) - threads[i] = std::thread(eval_robot, i); - - // wait for the threads to finish - for (size_t i = 0; i < threads.size(); ++i) - threads[i].join(); - std::cout << "first batch finished" << std::endl; - } + // for the example, we run NUM_THREADS threads of eval_robot() + std::vector threads(NUM_THREADS * 2); // *2 to see some reuse + for (size_t i = 0; i < threads.size(); ++i) + threads[i] = std::thread(eval_robot, i); + + // wait for the threads to finish + for (size_t i = 0; i < threads.size(); ++i) + threads[i].join(); return 0; } diff --git a/src/robot_dart/robot_pool.cpp b/src/robot_dart/robot_pool.cpp index bf16c830..dde5fd59 100644 --- a/src/robot_dart/robot_pool.cpp +++ b/src/robot_dart/robot_pool.cpp @@ -1,15 +1,27 @@ #include namespace robot_dart { - RobotPool::RobotPool(const std::function()>& robot_creator, size_t pool_size) : _robot_creator(robot_creator), _pool_size(pool_size) + RobotPool::RobotPool(const std::function()>& robot_creator, size_t pool_size, bool verbose) : _robot_creator(robot_creator), _pool_size(pool_size), _verbose(true) { + if (_verbose) { + std::cout << "Creating a pool of " << pool_size << " robots: "; + std::cout.flush(); + } + for (size_t i = 0; i < _pool_size; ++i) { + if (_verbose) { + std::cout << "[" << i << "]"; + std::cout.flush(); + } auto robot = robot_creator(); _set_init_pos(robot->skeleton()); _skeletons.push_back(robot->skeleton()); } for (size_t i = 0; i < _pool_size; i++) _free.push_back(true); + + if (_verbose) + std::cout << std::endl; } std::shared_ptr RobotPool::get_robot(const std::string& name) diff --git a/src/robot_dart/robot_pool.hpp b/src/robot_dart/robot_pool.hpp index 94dd23af..0f9b2adf 100644 --- a/src/robot_dart/robot_pool.hpp +++ b/src/robot_dart/robot_pool.hpp @@ -12,12 +12,7 @@ namespace robot_dart { class RobotPool { public: using robot_creator_t = std::function()>; - static RobotPool& instance(const robot_creator_t& robot_creator, size_t pool_size = 32) - { - static RobotPool gdata(robot_creator, pool_size); - return gdata; - } - RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32); + RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32, bool verbose=true); virtual ~RobotPool() {} RobotPool(const RobotPool&) = delete; void operator=(const RobotPool&) = delete; @@ -29,6 +24,7 @@ namespace robot_dart { virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel); robot_creator_t _robot_creator; size_t _pool_size; + bool _verbose; std::vector _skeletons; std::vector _free; std::mutex _skeleton_mutex; From cb5add5c019315fa8f9d6ffbd8a967625473ff4f Mon Sep 17 00:00:00 2001 From: Jean-Baptiste Mouret Date: Mon, 28 Sep 2020 13:43:22 +0200 Subject: [PATCH 5/6] merge --- src/examples/robot_pool.cpp | 39 +++---------------------------------- 1 file changed, 3 insertions(+), 36 deletions(-) diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp index 8365a9fe..085d9a72 100644 --- a/src/examples/robot_pool.cpp +++ b/src/examples/robot_pool.cpp @@ -10,21 +10,6 @@ static constexpr int NUM_THREADS = 12; void simulate_robot(const std::shared_ptr& robot) { -<<<<<<< HEAD - -======= - // you can also use a standard function instead of a lambda - auto robot_creator = [] { - std::vector> packages = {{"talos_description", "talos/talos_description"}}; - return std::make_shared("talos/talos.urdf", packages); - }; // do not forget the ; for lambda! - // we want 12 robots in the pool(usually the number of threads or a bit more ) - auto robot = robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).get_robot(); - - std::cout << "Robot " << i << " created [" << robot->skeleton() << "]" << std::endl; - - /// --- some robot_dart code --- ->>>>>>> 171c02e32c5a5b0f6c6a83c45b07e04d152a8947 robot->set_position_enforced(true); robot->skeleton()->setPosition(5, 1.1); robot->skeleton()->setPosition(2, 1.57); @@ -63,14 +48,14 @@ void simulate_robot(const std::shared_ptr& robot) } } - namespace pool { - std::shared_ptr robot_creator() { + std::shared_ptr robot_creator() + { std::vector> packages = {{"talos_description", "talos/talos_description"}}; return std::make_shared("talos/talos.urdf", packages); } robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS); -} +} // namespace pool void eval_robot(int i) { @@ -82,16 +67,11 @@ void eval_robot(int i) // --- do something with the result // CRITICAL : free your robot ! -<<<<<<< HEAD pool::robot_pool.free_robot(robot); -======= - robot_dart::RobotPool::instance(robot_creator, NUM_THREADS).free_robot(robot); ->>>>>>> 171c02e32c5a5b0f6c6a83c45b07e04d152a8947 } int main(int argc, char** argv) { -<<<<<<< HEAD // for the example, we run NUM_THREADS threads of eval_robot() std::vector threads(NUM_THREADS * 2); // *2 to see some reuse for (size_t i = 0; i < threads.size(); ++i) @@ -100,18 +80,5 @@ int main(int argc, char** argv) // wait for the threads to finish for (size_t i = 0; i < threads.size(); ++i) threads[i].join(); -======= - for (int i = 0; i < 2; ++i) { // we do it twice to see reuse of the some robots - // for the example, we run NUM_THREADS threads of eval_robot() - std::vector threads(NUM_THREADS); - for (size_t j = 0; j < threads.size(); ++j) - threads[j] = std::thread(eval_robot, j); - - // wait for the threads to finish - for (size_t j = 0; j < threads.size(); ++j) - threads[j].join(); - std::cout << "Batch " << i << " finished" << std::endl; - } ->>>>>>> 171c02e32c5a5b0f6c6a83c45b07e04d152a8947 return 0; } From 01374fc33f8dc38a2ca6c59de01e9d6877d87844 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Mon, 28 Sep 2020 16:02:27 +0300 Subject: [PATCH 6/6] [pool]: formatting [ci skip] --- src/examples/robot_pool.cpp | 3 ++- src/robot_dart/robot_pool.hpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/examples/robot_pool.cpp b/src/examples/robot_pool.cpp index 085d9a72..22f1bea5 100644 --- a/src/examples/robot_pool.cpp +++ b/src/examples/robot_pool.cpp @@ -33,7 +33,7 @@ void simulate_robot(const std::shared_ptr& robot) Eigen::VectorXd init_positions = robot->positions(dofs); - while (simu.scheduler().next_time() < 3) { // 3 second of simu only + while (simu.scheduler().next_time() <= 3.) { // 3 second of simu only if (simu.schedule(simu.control_freq())) { Eigen::VectorXd delta_pos(5); delta_pos << sin(simu.scheduler().current_time() * 2.), @@ -54,6 +54,7 @@ namespace pool { std::vector> packages = {{"talos_description", "talos/talos_description"}}; return std::make_shared("talos/talos.urdf", packages); } + robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS); } // namespace pool diff --git a/src/robot_dart/robot_pool.hpp b/src/robot_dart/robot_pool.hpp index 2ca24f30..d04c1fba 100644 --- a/src/robot_dart/robot_pool.hpp +++ b/src/robot_dart/robot_pool.hpp @@ -12,7 +12,8 @@ namespace robot_dart { class RobotPool { public: using robot_creator_t = std::function()>; - RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32, bool verbose=true); + + RobotPool(const robot_creator_t& robot_creator, size_t pool_size = 32, bool verbose = true); virtual ~RobotPool() {} RobotPool(const RobotPool&) = delete;