Icub collisions

etc/icub/humanoid_pos_tracker.yaml

@@ -16,8 +16,8 @@ CONTROLLER:
   closed_loop: false
   verbose: true
   mimic_dof_names : []
-  check_model_collisions: false
-  collision_path: ""
+  check_model_collisions: true
+  collision_path: "icub_collision_spheres.yaml"
   stabilizer:
     activated: true
     params_ss: stab_single_support.yaml

etc/icub/icub_collision_spheres.yaml

@@ -0,0 +1,18 @@
+members:
+    leg_left:
+      l_upper_leg: [[0.0,-0.1,0.0,0.14], [0.0,-0.15,0.0,0.14], [0.0,-0.2,0.0,0.14]]
+      l_lower_leg: [[0.0,0.0,0.0,0.13], [-0.05,0.0,0.0,0.13], [-0.1,0.0,0.0,0.13]]
+    leg_right:
+      r_upper_leg: [[0.0,-0.1,0.0,0.14], [0.0,-0.15,0.0,0.14],[0.0,-0.2,0.0,0.14]]
+      r_lower_leg: [[0.0,0.0,0.0,0.13], [-0.05,0.0,0.0,0.13], [-0.1,0.0,0.0,0.13]]
+    arm_left:
+      l_upper_arm: [[0.0, 0.0, 0.11, 0.1], [0.0, 0.0, 0.15, 0.1]]
+      l_forearm: [[0.0, 0.05, 0.0, 0.1], [0.0, 0.1, 0.0, 0.1]]
+      l_hand: [[0.05, 0.0, 0.0, 0.1]]
+    arm_right:
+      r_upper_arm: [[0.0, 0.0, 0.11, 0.1], [0.0, 0.0, 0.15, 0.1]]
+      r_forearm: [[0.0, -0.05, 0.0, 0.1], [0.0, -0.1, 0.0, 0.1]]
+      r_hand: [[0.05, 0.0, 0.0, 0.1]]
+    torso:
+      waist: [[0.0, 0.0, 0.0, 0.1]]
+      chest: [[-0.025, 0.0, 0.0, 0.21], [-0.025, 0.1, 0.0, 0.21], [-0.025, 0.15, 0.0, 0.21]]

src/robot_dart/icub.cpp

@@ -62,6 +62,7 @@ int main(int argc, char* argv[])
         ("closed_loop", "Close the loop with floating base position and joint positions; required for torque control [default: from YAML file]")
         ("help,h", "produce help message")
         ("height", po::value<bool>()->default_value(false), "print total feet force data to adjust height in config")
+        ("model_collisions",po::value<bool>()->default_value(false), "display pinocchio qp model collision spheres")
         ("mp4,m", po::value<std::string>(), "save the display to a mp4 video [filename]")
         ("push,p", po::value<std::vector<float>>(), "push the robot at t=x1 0.25 s")
         ("norm_force,n", po::value<float>()->default_value(-15) , "push norm force value")
@@ -222,6 +223,12 @@ int main(int argc, char* argv[])
 
         //////////////////// START SIMULATION //////////////////////////////////////
         simu.set_control_freq(control_freq); // default = 1000 Hz
+
+        // self-collision spheres
+        bool init_model_sphere_collisions = false;
+        std::vector<std::shared_ptr<robot_dart::Robot>> spheres;
+        bool is_colliding = false;
+
 
         std::shared_ptr<robot_dart::Robot> ghost;
         if (vm.count("ghost") || vm.count("collisions")) {
@@ -293,6 +300,23 @@ int main(int argc, char* argv[])
                     ghost->set_positions(controller->filter_cmd(q).tail(ncontrollable), controllable_dofs);
                     ghost->set_positions(q.head(6) + translate_ghost, floating_base);
                 }
+
+                // self-collision spheres
+                is_colliding = controller->is_model_colliding();
+                if (vm["model_collisions"].as<bool>()) {
+                    auto spherical_members = controller->collision_check().spherical_members();
+                    auto sphere_color = dart::Color::Green(0.5);
+                    Eigen::VectorXd translate_ghost = Eigen::VectorXd::Zero(6);
+                    translate_ghost(0) -= 1;
+
+                    if (init_model_sphere_collisions == false) {
+                        spheres = inria_wbc::robot_dart::create_spherical_members(spherical_members, simu, sphere_color);
+                        init_model_sphere_collisions = true;
+                    }
+                    else {
+                        inria_wbc::robot_dart::update_spherical_members(spherical_members, spheres, sphere_color, is_colliding, controller->collision_check().collision_index(), translate_ghost.head(3));
+                    }
+                }
             }
 
             if (simu.schedule(simu.graphics_freq()) && vm.count("collisions")) {