Include a clone pool

[jbmouret]

We use this pattern often (to avoid creating a new robot for each evaluation). We could have a polished implementation in robot_dart.

[costashatz]

I recently made something for a project that I am developing.. Is this a good basis? Or you think it's better to be handling skeletons?

using RobotPtr = std::shared_ptr<robot_dart::Robot>;

class ClonePool {
public:
    ClonePool() { _init_robots(); }

    RobotPtr get_robot()
    {
        std::lock_guard<std::mutex> lock(_robot_mutex);
        for (size_t i = 0; i < _num_robots; i++) {
            if (_free[i]) {
                _free[i] = false;
                return _robots[i];
            }
        }

        return nullptr;
    }

    void free_robot(const RobotPtr& robot)
    {
        std::lock_guard<std::mutex> lock(_robot_mutex);
        for (size_t i = 0; i < _num_robots; i++) {
            if (_robots[i] == robot) {
                _set_init_pos(robot);
                _free[i] = true;
                break;
            }
        }
    }

protected:
    // Global variables
    std::vector<RobotPtr> _robots;
    std::vector<bool> _free;
    size_t _num_robots = 3;
    std::mutex _robot_mutex;

    // Functions for handling skeletons
    void _set_init_pos(const RobotPtr& robot)  // TO-DO: Make this function virtual
    {
        robot->reset();
        robot->remove_all_drawing_axis();
        robot->clear_controllers();

        Eigen::VectorXd init_positions(robot->num_dofs());

        robot->set_positions(init_positions);

        // re-set correct masses, etc...
    }

    void _init_robots() // Load and clone robots (e.g., Talos)
    {
        std::vector<std::pair<std::string, std::string>> packages = {{"talos_description", "talos/talos_description"}};

        for (size_t i = 0; i < _num_robots; i++) {
            auto robot = std::make_shared<robot_dart::Robot>("talos/talos_fast.urdf", packages);

            std::vector<std::string> jt_disable = {"gripper_left_joint", "gripper_left_inner_double_joint", "gripper_left_fingertip_1_joint", "gripper_left_fingertip_2_joint", "gripper_left_motor_single_joint", "gripper_left_inner_single_joint", "gripper_left_fingertip_3_joint", "gripper_right_joint", "gripper_right_inner_double_joint", "gripper_right_fingertip_1_joint", "gripper_right_fingertip_2_joint", "gripper_right_motor_single_joint", "gripper_right_inner_single_joint", "gripper_right_fingertip_3_joint"};

            for (auto& joint : jt_disable) {
                auto jt = robot->skeleton()->getJoint(joint);
                auto bd = jt->getChildBodyNode();

                Eigen::Isometry3d tf = jt->getTransformFromParentBodyNode();
                dart::dynamics::WeldJoint::Properties properties;
                properties.mName = jt->getName();
                bd->changeParentJointType<dart::dynamics::WeldJoint>(properties);
                bd->getParentJoint()->setTransformFromParentBodyNode(tf);
            }

            robot->update_joint_dof_maps();

            _set_init_pos(robot);

            _robots.push_back(robot);
            _free.push_back(true);
        }
    }
};

[jbmouret]

Yes, this is what I had in mind. We can take a lambda function for the robot creator or define a virtual function (old school ;) ).

[costashatz]

Linked to #59 ...