Fixes for tiago

NOSALRO · Dec 8, 2021 · c5ab591 · c5ab591
1 parent 8b65d12
commit c5ab591
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 11 deletions.
diff --git a/src/examples/tiago.cpp b/src/examples/tiago.cpp
@@ -1,6 +1,7 @@
 #include <algorithm>
 #include <cstdlib>
 #include <iostream>
+
 #include <robot_dart/control/pd_control.hpp>
 #include <robot_dart/robot_dart_simu.hpp>
 #include <robot_dart/robots/tiago.hpp>
@@ -14,15 +15,14 @@
 
 int main()
 {
-    std::srand(std::time(NULL));
-
-    auto robot = std::make_shared<robot_dart::robots::Tiago>();
-    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
-
     double dt = 0.01;
     robot_dart::RobotDARTSimu simu(dt);
     simu.set_collision_detector("fcl");
 
+    size_t freq = 1. / dt;
+    auto robot = std::make_shared<robot_dart::robots::Tiago>(freq);
+    std::cout << "The model used is: [" << robot->model_filename() << "]" << std::endl;
+
 #ifdef GRAPHIC
     robot_dart::gui::magnum::GraphicsConfiguration configuration;
     configuration.width = 1280;
@@ -31,7 +31,7 @@ int main()
     auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>(configuration);
     simu.set_graphics(graphics);
     graphics->look_at({0., 3.5, 2.}, {0., 0., 0.25});
-    graphics->record_video("talos_dancing.mp4");
+    graphics->record_video("tiago_dancing.mp4");
 #endif
     simu.add_checkerboard_floor();
     simu.add_robot(robot);
@@ -50,7 +50,7 @@ int main()
     while (simu.scheduler().next_time() < 20. && !simu.graphics()->done()) {
         if (simu.schedule(simu.control_freq())) {
             Eigen::VectorXd delta_pos(6);
-            delta_pos << sin(simu.scheduler().current_time() * 2.)+ M_PI_2,
+            delta_pos << sin(simu.scheduler().current_time() * 2.) + M_PI_2,
                 sin(simu.scheduler().current_time() * 2.),
                 sin(simu.scheduler().current_time() * 2.),
                 sin(simu.scheduler().current_time() * 2.),

diff --git a/src/robot_dart/robots/tiago.cpp b/src/robot_dart/robots/tiago.cpp
@@ -4,10 +4,8 @@
 
 namespace robot_dart {
     namespace robots {
-        Tiago::Tiago(size_t frequency,
-            const std::string& urdf, const std::vector<std::pair<std::string, std::string>>& packages)
-            : Robot(urdf, packages),
-              _ft_wrist(std::make_shared<sensor::ForceTorque>(joint("gripper_tool_joint"), frequency))
+        Tiago::Tiago(size_t frequency, const std::string& urdf, const std::vector<std::pair<std::string, std::string>>& packages)
+            : Robot(urdf, packages), _ft_wrist(std::make_shared<sensor::ForceTorque>(joint("gripper_tool_joint"), frequency))
         {
             skeleton()->setPosition(2, M_PI / 2.);
             skeleton()->setPosition(5, 0.001);

diff --git a/src/robot_dart/robots/tiago.hpp b/src/robot_dart/robots/tiago.hpp
@@ -14,6 +14,7 @@ namespace robot_dart {
             Tiago(size_t frequency = 1000, const std::string& urdf = "tiago/tiago_steel.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"tiago_description", "tiago/tiago_description"}});
 
             const sensor::ForceTorque& ft_wrist() const { return *_ft_wrist; }
+
         protected:
             std::shared_ptr<sensor::ForceTorque> _ft_wrist;
             void _post_addition(RobotDARTSimu* simu) override;