Talos with simple collision boxes

src/examples/talos_box.cpp

@@ -0,0 +1,79 @@
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/talos.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+// fast TalosFastCollision:
+// - use dart for collision detection (instead of FCL) [only handle boxes and spheres]
+// - the collisions are detected with boxes, you can have the boxes visual with talos_box.urdf
+// - the boxes are totally covering the meshes. There is one box per link
+// - the urdf does not have the mimic (used for grippers)
+int main()
+{
+    auto robot = std::make_shared<robot_dart::robots::TalosFastCollision>(1000, "talos/talos_box.urdf");
+    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
+
+    // Set actuator types to VELOCITY (for speed)
+    robot->set_actuator_types("velocity");
+
+    double dt = 0.001;
+    robot_dart::RobotDARTSimu simu(dt);
+    // we can use the DART (fast) collision detector
+    simu.set_collision_detector("dart");
+#ifdef GRAPHIC
+    auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
+    simu.set_graphics(graphics);
+    graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
+    graphics->record_video("talos_box.mp4");
+#endif
+    simu.add_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);
+    std::vector<int> undamaged_joints = {0, 1, 2, 3, 4};
+    auto start = std::chrono::steady_clock::now();
+
+    while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
+        if (simu.scheduler().next_time() > 2. && simu.scheduler().next_time() < 2.0 + dt) {
+            robot->set_actuator_type("passive", "arm_left_1_joint");
+            robot->set_actuator_type("passive", "arm_left_2_joint");
+            robot->set_actuator_type("passive", "arm_left_3_joint");
+            robot->set_actuator_type("passive", "arm_left_4_joint");
+            undamaged_joints = {2, 3, 4};
+        }
+
+        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);
+            Eigen::VectorXd commands_to_send(undamaged_joints.size());
+            std::vector<std::string> dofs_to_send;
+            for (int i = 0; i < commands_to_send.size(); i++) {
+                commands_to_send[i] = commands[undamaged_joints[i]];
+                dofs_to_send.push_back(dofs[undamaged_joints[i]]);
+            }
+            robot->set_commands(commands_to_send, dofs_to_send);
+        }
+        simu.step_world();
+    }
+
+    auto end = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end - start;
+    std::cout << "benchmark time: " << elapsed_seconds.count() << "s\n";
+
+    robot.reset();
+    return 0;
+}

src/examples/talos_fast.cpp

@@ -14,15 +14,9 @@
 // - the urdf does not have the mimic (used for grippers)
 int main()
 {
-    auto robot = std::make_shared<robot_dart::robots::Talos>(1000, "talos/talos_fast.urdf");
+    auto robot = std::make_shared<robot_dart::robots::TalosLight>();
     std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
 
-    robot->set_position_enforced(true);
-    auto positions = robot->positions();
-    positions[2] = M_PI / 2.;
-    positions[5] = 1.1;
-    robot->set_positions(positions);
-
     // Set actuator types to VELOCITY (for speed)
     robot->set_actuator_types("velocity");
 
@@ -34,7 +28,7 @@ int main()
     auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
     simu.set_graphics(graphics);
     graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
-    graphics->record_video("talos.mp4");
+    graphics->record_video("talos_light.mp4");
 #endif
     simu.add_floor();
     simu.add_robot(robot);

src/examples/talos_fast_collision.cpp

@@ -0,0 +1,79 @@
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/talos.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+// fast TalosFastCollision:
+// - use dart for collision detection (instead of FCL) [only handle boxes and spheres]
+// - the collisions are detected with boxes, you can have the boxes visual with talos_box.urdf
+// - the boxes are totally covering the meshes. There is one box per link
+// - the urdf does not have the mimic (used for grippers)
+int main()
+{
+    auto robot = std::make_shared<robot_dart::robots::TalosFastCollision>();
+    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
+
+    // Set actuator types to VELOCITY (for speed)
+    robot->set_actuator_types("velocity");
+
+    double dt = 0.001;
+    robot_dart::RobotDARTSimu simu(dt);
+    // we can use the DART (fast) collision detector
+    simu.set_collision_detector("dart");
+#ifdef GRAPHIC
+    auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
+    simu.set_graphics(graphics);
+    graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
+    graphics->record_video("talos_fast.mp4");
+#endif
+    simu.add_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);
+    std::vector<int> undamaged_joints = {0, 1, 2, 3, 4};
+    auto start = std::chrono::steady_clock::now();
+
+    while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
+        if (simu.scheduler().next_time() > 2. && simu.scheduler().next_time() < 2.0 + dt) {
+            robot->set_actuator_type("passive", "arm_left_1_joint");
+            robot->set_actuator_type("passive", "arm_left_2_joint");
+            robot->set_actuator_type("passive", "arm_left_3_joint");
+            robot->set_actuator_type("passive", "arm_left_4_joint");
+            undamaged_joints = {2, 3, 4};
+        }
+
+        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);
+            Eigen::VectorXd commands_to_send(undamaged_joints.size());
+            std::vector<std::string> dofs_to_send;
+            for (int i = 0; i < commands_to_send.size(); i++) {
+                commands_to_send[i] = commands[undamaged_joints[i]];
+                dofs_to_send.push_back(dofs[undamaged_joints[i]]);
+            }
+            robot->set_commands(commands_to_send, dofs_to_send);
+        }
+        simu.step_world();
+    }
+
+    auto end = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end - start;
+    std::cout << "benchmark time: " << elapsed_seconds.count() << "s\n";
+
+    robot.reset();
+    return 0;
+}

src/robot_dart/robots/talos.cpp

@@ -73,5 +73,37 @@ namespace robot_dart {
                 simu->remove_sensor(t.second);
             }
         }
+
+        void TalosFastCollision::_post_addition(RobotDARTSimu* simu)
+        {
+            Talos::_post_addition(simu);
+            auto vec = TalosFastCollision::collision_vec();
+            for (auto& t : vec) {
+                simu->set_collision_masks(simu->robots().size() - 1, std::get<0>(t), std::get<1>(t), std::get<2>(t));
+            }
+        }
+
+        std::vector<std::tuple<std::string, uint32_t, uint32_t>> TalosFastCollision::collision_vec()
+        {
+            std::vector<std::tuple<std::string, uint32_t, uint32_t>> vec;
+            vec.push_back(std::make_tuple("leg_right_6_link", 0x1, 0x1 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_4_link", 0x2, 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_6_link", 0x4, 0x1 | 0x2 | 0x4 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_4_link", 0x8, 0x1 | 0x2 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_1_link", 0x10, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_3_link", 0x20, 0x1 | 0x2 | 0x4 | 0x8 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_1_link", 0x40, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_3_link", 0x80, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x80 | 0x100 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_left_7_link", 0x100, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_left_5_link", 0x200, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x200 | 0x400 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_right_7_link", 0x400, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x400 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("arm_right_5_link", 0x800, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("torso_2_link", 0x1000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000));
+            vec.push_back(std::make_tuple("base_link", 0x2000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x2000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_right_2_link", 0x4000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("leg_left_2_link", 0x8000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x1000 | 0x4000 | 0x8000 | 0x10000));
+            vec.push_back(std::make_tuple("head_2_link", 0x10000, 0x1 | 0x2 | 0x4 | 0x8 | 0x10 | 0x20 | 0x40 | 0x80 | 0x100 | 0x200 | 0x800 | 0x2000 | 0x4000 | 0x8000 | 0x10000));
+            return vec;
+        }
     } // namespace robots
 } // namespace robot_dart

src/robot_dart/robots/talos.hpp

@@ -39,6 +39,16 @@ namespace robot_dart {
         public:
             TalosLight(size_t frequency = 1000, const std::string& urdf = "talos/talos_fast.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"talos_description", "talos/talos_description"}}) : Talos(frequency, urdf, packages) {}
         };
+
+        // for talos_fast_collision.urdf or talos_box.urdf which have simple box collision shapes
+        class TalosFastCollision : public Talos {
+        public:
+            TalosFastCollision(size_t frequency = 1000, const std::string& urdf = "talos/talos_fast_collision.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"talos_description", "talos/talos_description"}}) : Talos(frequency, urdf, packages) { set_self_collision(); }
+            static std::vector<std::tuple<std::string, uint32_t, uint32_t>> collision_vec();
+
+        protected:
+            void _post_addition(RobotDARTSimu* simu) override;
+        };
     } // namespace robots
 } // namespace robot_dart
 #endif