Create a robot pool

src/examples/robot_pool.cpp

@@ -0,0 +1,85 @@
+#include <algorithm>
+#include <cstdlib>
+#include <iostream>
+#include <thread>
+
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robot_pool.hpp>
+
+static constexpr int NUM_THREADS = 12;
+
+void simulate_robot(const std::shared_ptr<robot_dart::Robot>& robot)
+{
+    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<std::string> 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() <= 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.),
+                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();
+    }
+}
+
+namespace pool {
+    std::shared_ptr<robot_dart::Robot> robot_creator()
+    {
+        std::vector<std::pair<std::string, std::string>> packages = {{"talos_description", "talos/talos_description"}};
+        return std::make_shared<robot_dart::Robot>("talos/talos.urdf", packages);
+    }
+
+    robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS);
+} // namespace pool
+
+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 !
+    pool::robot_pool.free_robot(robot);
+}
+
+int main(int argc, char** argv)
+{
+    // for the example, we run NUM_THREADS threads of eval_robot()
+    std::vector<std::thread> 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;
+}

src/robot_dart/robot_pool.cpp

@@ -0,0 +1,61 @@
+#include <robot_dart/robot_pool.hpp>
+
+namespace robot_dart {
+    RobotPool::RobotPool(const std::function<std::shared_ptr<Robot>()>& 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<Robot> RobotPool::get_robot(const std::string& name)
+    {
+        std::lock_guard<std::mutex> lock(_skeleton_mutex);
+        while (true) {
+            for (size_t i = 0; i < _pool_size; i++)
+                if (_free[i]) {
+                    _free[i] = false;
+                    return std::make_shared<Robot>(_skeletons[i], name);
+                }
+        }
+    }
+
+    void RobotPool::free_robot(const std::shared_ptr<Robot>& robot)
+    {
+        std::lock_guard<std::mutex> 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();
+    }
+} // namespace robot_dart

src/robot_dart/robot_pool.hpp

@@ -0,0 +1,38 @@
+#ifndef _TALOS_CLONE_POOL
+#define _TALOS_CLONE_POOL
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <vector>
+
+#include <robot_dart/robot.hpp>
+
+namespace robot_dart {
+    class RobotPool {
+    public:
+        using robot_creator_t = std::function<std::shared_ptr<Robot>()>;
+
+        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;
+
+        virtual std::shared_ptr<Robot> get_robot(const std::string& name = "robot");
+        virtual void free_robot(const std::shared_ptr<Robot>& robot);
+
+    protected:
+        robot_creator_t _robot_creator;
+        size_t _pool_size;
+        bool _verbose;
+        std::vector<dart::dynamics::SkeletonPtr> _skeletons;
+        std::vector<bool> _free;
+        std::mutex _skeleton_mutex;
+        std::string _model_filename;
+
+        virtual void _set_init_pos(const dart::dynamics::SkeletonPtr& skel);
+    };
+} // namespace robot_dart
+
+#endif

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)