Merge pull request #118 from resibots/cleanup

Cleanup

src/examples/arm.cpp

@@ -14,7 +14,7 @@ struct StateDesc : public robot_dart::descriptor::BaseDescriptor {
     void operator()()
     {
         if (_simu->robots().size() > 0) {
-            states.push_back(_simu->robots()[0]->skeleton()->getPositions());
+            states.push_back(_simu->robots()[0]->positions());
         }
     }
 
@@ -24,40 +24,38 @@ struct StateDesc : public robot_dart::descriptor::BaseDescriptor {
 int main()
 {
     std::srand(std::time(NULL));
-    auto global_robot = std::make_shared<robot_dart::Robot>("arm.urdf");
+    auto robot = std::make_shared<robot_dart::Robot>("arm.urdf");
 
-    global_robot->fix_to_world();
-    global_robot->set_position_enforced(true);
-    // g_robot->skeleton()->setPosition(1, M_PI / 2.0);
-    Eigen::Vector3d size(0, 0, 0);
+    robot->fix_to_world();
+    robot->set_position_enforced(true);
 
-    global_robot->set_actuator_types("velocity");
+    robot->set_actuator_types("velocity");
 
-    Eigen::VectorXd ctrl(4);
-    ctrl << 0.0, 1.0, -1.5, 1.0;
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0.0, 1.0, -1.5, 1.0});
 
-    global_robot->add_controller(std::make_shared<robot_dart::control::PDControl>(ctrl));
-
-    auto g_robot = global_robot->clone();
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl);
+    robot->add_controller(controller);
 
     robot_dart::RobotDARTSimu simu;
 #ifdef GRAPHIC
     simu.set_graphics(std::make_shared<robot_dart::gui::magnum::Graphics>());
 #endif
-    simu.add_descriptor(std::make_shared<StateDesc>());
-    simu.add_robot(g_robot);
-    std::cout << (g_robot->body_pose("arm_link_5") * size).transpose() << std::endl;
+    auto state_desc = std::make_shared<StateDesc>();
+    simu.add_descriptor(state_desc);
+    simu.add_robot(robot);
+    std::cout << robot->body_pose("arm_link_5").translation().transpose() << std::endl;
+
     simu.run(2.5);
-    std::cout << (g_robot->body_pose("arm_link_5") * size).transpose() << std::endl;
+    std::cout << robot->body_pose("arm_link_5").translation().transpose() << std::endl;
+
     ctrl << 0.0, -1.0, 1.5, -1.0;
-    g_robot->controllers()[0]->set_parameters(ctrl);
-    std::static_pointer_cast<robot_dart::control::PDControl>(g_robot->controllers()[0])->set_pd(20., 0.);
+    controller->set_parameters(ctrl);
+    controller->set_pd(20., 0.);
     simu.run(2.5);
-    std::cout << (g_robot->body_pose("arm_link_5") * size).transpose() << std::endl;
+    std::cout << robot->body_pose("arm_link_5").translation().transpose() << std::endl;
 
-    std::cout << std::static_pointer_cast<StateDesc>(simu.descriptor(0))->states.size() << std::endl;
+    std::cout << state_desc->states.size() << std::endl;
 
-    g_robot.reset();
-    global_robot.reset();
+    robot.reset();
     return 0;
 }

src/examples/cameras.cpp

@@ -34,16 +34,16 @@ int main()
     std::srand(std::time(NULL));
 
     std::vector<std::pair<std::string, std::string>> packages = {{"iiwa_description", "iiwa/iiwa_description"}};
-    auto global_robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
+    auto robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
 
-    global_robot->fix_to_world();
-    global_robot->set_position_enforced(true);
+    robot->fix_to_world();
+    robot->set_position_enforced(true);
 
-    Eigen::VectorXd ctrl(7);
-    ctrl << 0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.;
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.});
 
-    global_robot->add_controller(std::make_shared<robot_dart::control::PDControl>(ctrl));
-    std::static_pointer_cast<robot_dart::control::PDControl>(global_robot->controllers()[0])->set_pd(500., 50.);
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl);
+    robot->add_controller(controller);
+    controller->set_pd(500., 50.);
 
     robot_dart::RobotDARTSimu simu(0.001);
 
@@ -72,11 +72,11 @@ int main()
     tf = Eigen::AngleAxisd(3.14, Eigen::Vector3d{1., 0., 0.});
     tf *= Eigen::AngleAxisd(1.57, Eigen::Vector3d{0., 0., 1.});
     tf.translation() = Eigen::Vector3d(0., 0., 0.1);
-    camera->attach_to_body(global_robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default
+    camera->attach_to_body(robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default
 
     // simu.add_floor(5.);
     simu.add_checkerboard_floor(10.);
-    simu.add_robot(global_robot);
+    simu.add_robot(robot);
     Eigen::Vector6d pose;
     pose << 0., 0., 0., 1.5, 0., 0.1;
     simu.add_robot(robot_dart::Robot::create_box({0.1, 0.1, 0.1}, pose, "free", 1., dart::Color::Red(1.), "box"));
@@ -102,6 +102,6 @@ int main()
     // and the raw values that can be used along with the camera parameters to transform the image to point-cloud
     robot_dart::gui::save_png_image("camera-depth-raw.png", camera->raw_depth_image());
 
-    global_robot.reset();
+    robot.reset();
     return 0;
 }

src/examples/control_loop.cpp

@@ -8,19 +8,20 @@
 
 int main()
 {
+    std::srand(std::time(NULL));
+
     auto robot = std::make_shared<robot_dart::Robot>("pendulum.urdf");
     robot->fix_to_world();
     robot->set_position_enforced(false);
-    robot->skeleton()->setPosition(0, M_PI);
-    Eigen::Vector3d size(0.0402, 0.05, 1);
 
-    robot_dart::RobotDARTSimu simu(1e-3);
+    robot_dart::RobotDARTSimu simu(0.001);
 #ifdef GRAPHIC
     simu.set_graphics(std::make_shared<robot_dart::gui::magnum::Graphics>());
 #endif
 
     simu.add_robot(robot);
     simu.set_control_freq(100); // 100 Hz
+
     std::vector<std::string> dofs = {"pendulum_joint_1"};
     while (simu.scheduler().next_time() < 10 && !simu.graphics()->done()) { // simulate 10 seconds
         if (simu.schedule(simu.control_freq())) {

src/examples/dof_maps.cpp

@@ -14,13 +14,13 @@ int main()
     std::srand(std::time(NULL));
 
     std::vector<std::pair<std::string, std::string>> packages = {{"iiwa_description", "iiwa/iiwa_description"}};
-    auto global_robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
+    auto robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
 
-    global_robot->fix_to_world();
-    global_robot->set_position_enforced(true);
+    robot->fix_to_world();
+    robot->set_position_enforced(true);
 
     // Set actuator types to SERVO motors so that they stay in position without any controller
-    global_robot->set_actuator_types("servo");
+    robot->set_actuator_types("servo");
 
     float dt = 0.001;
     robot_dart::RobotDARTSimu simu(dt);
@@ -33,19 +33,18 @@ int main()
     /**************** Use dofs names to control the robot *********************************************/
 
     //Set Initial Position for the robot
-    Eigen::VectorXd q0(7);
-    q0 << 0.0, M_PI / 3., 0.0, -M_PI / 4.0, 0.0, 0.0, 0.0;
-    global_robot->set_positions(q0);
+    Eigen::VectorXd q0 = robot_dart::make_vector({0.0, M_PI / 3., 0.0, -M_PI / 4.0, 0.0, 0.0, 0.0});
+    robot->set_positions(q0);
 
     //Get dofs names
-    auto dofs = global_robot->dof_names();
+    auto dofs = robot->dof_names();
     std::cout << "Dofs : " << std::endl;
     for (auto& d : dofs) {
         std::cout << d << std::endl;
     }
 
     simu.add_checkerboard_floor();
-    simu.add_robot(global_robot);
+    simu.add_robot(robot);
 
     //Create a custom update loop using step_world instead of run
     int sim_time = 10 / dt;
@@ -54,33 +53,34 @@ int main()
     std::vector<std::string> dof_to_control;
     dof_to_control.push_back("iiwa_joint_1");
     dof_to_control.push_back("iiwa_joint_4");
-    Eigen::VectorXd cmd(2), pos(2), full_pos(q0.size());
-    cmd(0) = 0.1;
-    cmd(1) = -0.1;
+    Eigen::VectorXd cmd = robot_dart::make_vector({0.1, -0.1}),
+                    pos(2),
+                    full_pos(q0.size());
 
     for (int i = 0; i < sim_time; i++) {
-        global_robot->set_commands(cmd, dof_to_control);
-        pos = global_robot->positions(dof_to_control); //get dof_to_control positions only
-        full_pos = global_robot->positions(); //get all dofs positions
-        simu.step_world(); // do not update controllers (it will override set_commands)
+        robot->set_commands(cmd, dof_to_control);
+        pos = robot->positions(dof_to_control); //get dof_to_control positions only
+        full_pos = robot->positions(); //get all dofs positions
+        if (simu.step_world()) // do not update controllers (it will override set_commands)
+            break;
     }
 
     // If you wanted to control every dofs you could have used :
     // Eigen::VectorXd cmd_full = Eigen::VectorXd::Constant(q0.size(), 0.1);
     // for (int i = 0; i < sim_time; i++) {
-    //     global_robot->set_commands(cmd_full);
+    //     robot->set_commands(cmd_full);
     //     simu.step_world(); // do not update controllers (it will override set_commands)
     // }
 
     /**************** Mimic / Passive / Locked joint handling ********************************************************/
     bool filter_mimics = true, filter_locked = true, filter_passive = true;
 
-    global_robot->set_mimic("iiwa_joint_5", "iiwa_joint_1");
-    global_robot->set_actuator_type("locked", "iiwa_joint_6");
-    global_robot->set_actuator_type("passive", "iiwa_joint_7");
+    robot->set_mimic("iiwa_joint_5", "iiwa_joint_1");
+    robot->set_actuator_type("locked", "iiwa_joint_6");
+    robot->set_actuator_type("passive", "iiwa_joint_7");
 
     //Get the controllable joints (You cannot send a command to a mimic or passive or locked joint)
-    auto controllable_dofs = global_robot->dof_names(filter_mimics, filter_locked, filter_passive);
+    auto controllable_dofs = robot->dof_names(filter_mimics, filter_locked, filter_passive);
 
     std::cout << "Filtered dofs : " << std::endl;
     for (auto& cd : controllable_dofs) {
@@ -91,11 +91,12 @@ int main()
     cmd.setConstant(-0.1);
 
     for (int i = 0; i < sim_time; i++) {
-        global_robot->set_commands(cmd, controllable_dofs);
-        // global_robot->set_commands(Eigen::VectorXd::Constant(q0.size(), -0.1)); // This also works, but you will be getting a lot of warnings from DART; the comands to the mimic/passive/locked joints are ignored!
-        simu.step_world(); // do not update controllers (it will override set_commands)
+        robot->set_commands(cmd, controllable_dofs);
+        // robot->set_commands(Eigen::VectorXd::Constant(q0.size(), -0.1)); // This also works, but you will be getting a lot of warnings from DART; the comands to the mimic/passive/locked joints are ignored!
+        if (simu.step_world()) // do not update controllers (it will override set_commands)
+            break;
     }
 
-    global_robot.reset();
+    robot.reset();
     return 0;
 }

src/examples/franka.cpp

@@ -12,23 +12,23 @@ int main()
     std::srand(std::time(NULL));
 
     std::vector<std::pair<std::string, std::string>> packages = {{"franka_description", "franka/franka_description"}};
-    auto global_robot = std::make_shared<robot_dart::Robot>("franka/franka.urdf", packages);
-    global_robot->set_color_mode("material");
+    auto robot = std::make_shared<robot_dart::Robot>("franka/franka.urdf", packages);
+    robot->set_color_mode("material");
 
     // pin the arm to world
-    global_robot->fix_to_world();
-    global_robot->set_position_enforced(true);
+    robot->fix_to_world();
+    robot->set_position_enforced(true);
 
-    global_robot->set_actuator_types("torque");
+    robot->set_actuator_types("torque");
 
     // add a PD-controller to the arm
     // set desired positions
-    Eigen::VectorXd ctrl(8);
-    ctrl << 0., M_PI / 4., 0., -M_PI / 4, 0., M_PI / 2., 0., 0.;
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 4., 0., -M_PI / 4, 0., M_PI / 2., 0., 0.});
 
     // add the controller to the robot
-    global_robot->add_controller(std::make_shared<robot_dart::control::PDControl>(ctrl));
-    std::static_pointer_cast<robot_dart::control::PDControl>(global_robot->controllers()[0])->set_pd(300., 50.);
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl);
+    robot->add_controller(controller);
+    controller->set_pd(300., 50.);
 
     // choose time step of 0.001 seconds
     robot_dart::RobotDARTSimu simu(0.001);
@@ -42,9 +42,9 @@ int main()
 #endif
 
     simu.add_checkerboard_floor();
-    simu.add_robot(global_robot);
+    simu.add_robot(robot);
 
     simu.run(30.);
-    global_robot.reset();
+    robot.reset();
     return 0;
 }

src/examples/hexapod.cpp

@@ -8,15 +8,14 @@
 int main()
 {
     std::srand(std::time(NULL));
-    auto global_robot = std::make_shared<robot_dart::Robot>("pexod.urdf");
+    auto robot = std::make_shared<robot_dart::Robot>("pexod.urdf");
 
-    global_robot->set_position_enforced(true);
+    robot->set_position_enforced(true);
 
-    global_robot->set_actuator_types("servo");
-    global_robot->skeleton()->enableSelfCollisionCheck();
+    robot->set_actuator_types("servo");
+    robot->skeleton()->enableSelfCollisionCheck();
 
-    auto g_robot = global_robot->clone();
-    g_robot->skeleton()->setPosition(5, 0.2);
+    robot->set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.2}));
 
     robot_dart::RobotDARTSimu simu(0.001);
 #ifdef GRAPHIC
@@ -25,12 +24,11 @@ int main()
     graphics->look_at({0.5, 3., 0.75}, {0.5, 0., 0.2});
 #endif
     simu.add_floor();
-    simu.add_robot(g_robot);
+    simu.add_robot(robot);
     simu.run(10.);
 
-    std::cout << g_robot->skeleton()->getPositions().head(6).tail(3).transpose() << std::endl;
+    std::cout << robot->base_pose_vec().tail(3).transpose() << std::endl;
 
-    g_robot.reset();
-    global_robot.reset();
+    robot.reset();
     return 0;
 }

src/examples/icub.cpp

@@ -26,8 +26,7 @@ int main()
     robot->set_color_mode("aspect", "left_foot");
 
     robot->set_position_enforced(true);
-    robot->skeleton()->setPosition(5, 0.5);
-    robot->skeleton()->setPosition(2, 1.57);
+    robot->set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 0.5}));
 
     // Set actuator types to VELOCITY motors so that they stay in position without any controller
     robot->set_actuator_types("velocity");

src/examples/iiwa.cpp

@@ -14,19 +14,19 @@ int main()
     std::srand(std::time(NULL));
 
     std::vector<std::pair<std::string, std::string>> packages = {{"iiwa_description", "iiwa/iiwa_description"}};
-    auto global_robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
+    auto robot = std::make_shared<robot_dart::Robot>("iiwa/iiwa.urdf", packages);
 
-    global_robot->fix_to_world();
-    global_robot->set_position_enforced(true);
+    robot->fix_to_world();
+    robot->set_position_enforced(true);
 
-    Eigen::VectorXd ctrl(7);
-    ctrl << 0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.;
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.});
 
-    global_robot->add_controller(std::make_shared<robot_dart::control::PDControl>(ctrl));
-    std::static_pointer_cast<robot_dart::control::PDControl>(global_robot->controllers()[0])->set_pd(300., 50.);
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl);
+    robot->add_controller(controller);
+    controller->set_pd(300., 50.);
 
     // Add a ghost robot; only visuals, no dynamics, no collision
-    auto ghost = global_robot->clone_ghost();
+    auto ghost = robot->clone_ghost();
 
     robot_dart::RobotDARTSimu simu(0.001);
     simu.set_collision_detector("fcl");
@@ -37,11 +37,11 @@ int main()
     graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
 #endif
     simu.add_checkerboard_floor();
-    simu.add_robot(global_robot);
+    simu.add_robot(robot);
     simu.add_robot(ghost);
     simu.set_text_panel("IIWA simulation");
     simu.run(20.);
 
-    global_robot.reset();
+    robot.reset();
     return 0;
 }