Add custom reset function to all robots that need it

src/python/robot.cpp

@@ -523,6 +523,10 @@ namespace robot_dart {
                     py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"franka_description", "franka/franka_description"}}))
                 .def("ft_wrist", &Franka::ft_wrist, py::return_value_policy::reference);
 
+            py::class_<Hexapod, Robot, std::shared_ptr<Hexapod>>(m, "Hexapod")
+                .def(py::init<const std::string&>(),
+                    py::arg("urdf") = "pexod.urdf");
+
             py::class_<Iiwa, Robot, std::shared_ptr<Iiwa>>(m, "Iiwa")
                 .def(py::init<size_t, const std::string&, const std::vector<std::pair<std::string, std::string>>&>(),
                     py::arg("frequency") = 1000,
@@ -535,7 +539,6 @@ namespace robot_dart {
                     py::arg("frequency") = 1000,
                     py::arg("urdf") = "tiago/tiago_steel.urdf",
                     py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"tiago_description", "tiago/tiago_description"}}))
-                .def("reset", &Tiago::reset)
                 .def("ft_wrist", &Tiago::ft_wrist, py::return_value_policy::reference)
                 .def("caster_joints", &Tiago::caster_joints)
                 .def("set_actuator_types", &Tiago::set_actuator_types,

src/robot_dart/robots/a1.cpp

@@ -18,5 +18,18 @@ namespace robot_dart {
             names = std::vector<std::string>(names.begin() + 6, names.end());
             set_positions(robot_dart::make_vector({0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3}), names);
         }
+
+        void A1::reset()
+        {
+            Robot::reset();
+
+            // put above ground
+            set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.5}));
+
+            // standing pose
+            auto names = dof_names(true, true, true);
+            names = std::vector<std::string>(names.begin() + 6, names.end());
+            set_positions(robot_dart::make_vector({0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3}), names);
+        }
     } // namespace robots
 } // namespace robot_dart

src/robot_dart/robots/a1.hpp

@@ -8,6 +8,8 @@ namespace robot_dart {
         class A1 : public Robot {
         public:
             A1(const std::string& urdf = "unitree_a1/a1.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"a1_description", "unitree_a1/a1_description"}});
+
+            void reset() override;
         };
     } // namespace robots
 } // namespace robot_dart

src/robot_dart/robots/hexapod.hpp

@@ -13,6 +13,12 @@ namespace robot_dart {
                 skeleton()->enableSelfCollisionCheck();
                 set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.2}));
             }
+
+            void reset() override
+            {
+                Robot::reset();
+                set_base_pose(robot_dart::make_vector({0., 0., 0., 0., 0., 0.2}));
+            }
         };
     } // namespace robots
 } // namespace robot_dart

src/robot_dart/robots/icub.cpp

@@ -12,8 +12,17 @@ namespace robot_dart {
             set_color_mode("material");
 
             set_position_enforced(true);
-            skeleton()->setPosition(5, 0.46);
-            skeleton()->setPosition(2, 1.57);
+
+            // position iCub
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 0.46}));
+        }
+
+        void ICub::reset()
+        {
+            Robot::reset();
+
+            // position iCub
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 0.46}));
         }
 
         void ICub::_post_addition(RobotDARTSimu* simu)

src/robot_dart/robots/icub.hpp

@@ -11,6 +11,8 @@ namespace robot_dart {
         public:
             ICub(size_t frequency = 1000, const std::string& urdf = "icub/icub.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"icub_description", "icub/icub_description"}});
 
+            void reset() override;
+
             const sensor::IMU& imu() const { return *_imu; }
             const sensor::ForceTorque& ft_foot_left() const { return *_ft_foot_left; }
             const sensor::ForceTorque& ft_foot_right() const { return *_ft_foot_right; }

src/robot_dart/robots/talos.cpp

@@ -15,11 +15,16 @@ namespace robot_dart {
             // use position/torque limits
             set_position_enforced(true);
 
-            // set a position abobe the floor
-            skeleton()->setPosition(5, 1.1);
+            // position Talos
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 1.1}));
+        }
+
+        void Talos::reset()
+        {
+            Robot::reset();
 
-            // rotate the robot
-            skeleton()->setPosition(2, 1.57);
+            // position Talos
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 1.1}));
         }
 
         void Talos::_post_addition(RobotDARTSimu* simu)

src/robot_dart/robots/talos.hpp

@@ -13,6 +13,8 @@ namespace robot_dart {
         public:
             Talos(size_t frequency = 1000, const std::string& urdf = "talos/talos.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"talos_description", "talos/talos_description"}});
 
+            void reset() override;
+
             const sensor::IMU& imu() const { return *_imu; }
             const sensor::ForceTorque& ft_foot_left() const { return *_ft_foot_left; }
             const sensor::ForceTorque& ft_foot_right() const { return *_ft_foot_right; }

src/robot_dart/robots/tiago.cpp

@@ -7,18 +7,20 @@ namespace robot_dart {
             : 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.);
             set_position_enforced(true);
             // We use servo actuators, but not for the caster joints
             set_actuator_types("servo");
+
+            // position Tiago
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 0.}));
         }
 
         void Tiago::reset()
         {
             Robot::reset();
-            skeleton()->setPosition(2, M_PI / 2.);
-            skeleton()->setPosition(5, 0.);
+
+            // position Tiago
+            set_base_pose(robot_dart::make_vector({0., 0., M_PI / 2., 0., 0., 0.}));
         }
 
         void Tiago::set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names, bool override_mimic, bool override_base, bool override_caster)