From 0ee9bc7c27b2db68629f97012c249b1b21c39c73 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Wed, 1 Jun 2022 17:13:04 +0300 Subject: [PATCH 1/3] Add custom reset function to all robots that needed --- src/python/robot.cpp | 10 +++++++++- src/robot_dart/robots/a1.cpp | 13 +++++++++++++ src/robot_dart/robots/a1.hpp | 2 ++ src/robot_dart/robots/hexapod.hpp | 6 ++++++ src/robot_dart/robots/icub.cpp | 7 +++++++ src/robot_dart/robots/icub.hpp | 2 ++ src/robot_dart/robots/talos.cpp | 11 +++++++++++ src/robot_dart/robots/talos.hpp | 2 ++ 8 files changed, 52 insertions(+), 1 deletion(-) diff --git a/src/python/robot.cpp b/src/python/robot.cpp index 451a23c5..a455014e 100644 --- a/src/python/robot.cpp +++ b/src/python/robot.cpp @@ -510,7 +510,8 @@ namespace robot_dart { py::class_>(m, "A1") .def(py::init>&>(), py::arg("urdf") = "unitree_a1/a1.urdf", - py::arg("packages") = std::vector>({{"a1_description", "unitree_a1/a1_description"}})); + py::arg("packages") = std::vector>({{"a1_description", "unitree_a1/a1_description"}})) + .def("reset", &A1::reset); py::class_>(m, "Arm") .def(py::init(), @@ -523,6 +524,11 @@ namespace robot_dart { py::arg("packages") = std::vector>({{"franka_description", "franka/franka_description"}})) .def("ft_wrist", &Franka::ft_wrist, py::return_value_policy::reference); + py::class_>(m, "Hexapod") + .def(py::init(), + py::arg("urdf") = "pexod.urdf") + .def("reset", &Hexapod::reset); + py::class_>(m, "Iiwa") .def(py::init>&>(), py::arg("frequency") = 1000, @@ -557,6 +563,7 @@ namespace robot_dart { py::arg("urdf") = "icub/icub.urdf", py::arg("packages") = std::vector>({{"icub_description", "icub/icub_description"}})) + .def("reset", &ICub::reset) .def("imu", &ICub::imu, py::return_value_policy::reference) .def("ft_foot_left", &ICub::ft_foot_left, py::return_value_policy::reference) .def("ft_foot_right", &ICub::ft_foot_right, py::return_value_policy::reference); @@ -567,6 +574,7 @@ namespace robot_dart { py::arg("urdf") = "talos/talos.urdf", py::arg("packages") = std::vector>({{"talos_description", "talos/talos_description"}})) + .def("reset", &Talos::reset) .def("imu", &Talos::imu, py::return_value_policy::reference) .def("ft_foot_left", &Talos::ft_foot_left, py::return_value_policy::reference) .def("ft_foot_right", &Talos::ft_foot_right, py::return_value_policy::reference) diff --git a/src/robot_dart/robots/a1.cpp b/src/robot_dart/robots/a1.cpp index bad17a8e..528e72d5 100644 --- a/src/robot_dart/robots/a1.cpp +++ b/src/robot_dart/robots/a1.cpp @@ -18,5 +18,18 @@ namespace robot_dart { names = std::vector(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(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 diff --git a/src/robot_dart/robots/a1.hpp b/src/robot_dart/robots/a1.hpp index 8a27938b..74cdd9f7 100644 --- a/src/robot_dart/robots/a1.hpp +++ b/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>& packages = {{"a1_description", "unitree_a1/a1_description"}}); + + void reset() override; }; } // namespace robots } // namespace robot_dart diff --git a/src/robot_dart/robots/hexapod.hpp b/src/robot_dart/robots/hexapod.hpp index 40ab8865..c85a04a1 100644 --- a/src/robot_dart/robots/hexapod.hpp +++ b/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 diff --git a/src/robot_dart/robots/icub.cpp b/src/robot_dart/robots/icub.cpp index fbfaa03d..ac2bfc5f 100644 --- a/src/robot_dart/robots/icub.cpp +++ b/src/robot_dart/robots/icub.cpp @@ -16,6 +16,13 @@ namespace robot_dart { skeleton()->setPosition(2, 1.57); } + void ICub::reset() + { + Robot::reset(); + skeleton()->setPosition(5, 0.46); + skeleton()->setPosition(2, 1.57); + } + void ICub::_post_addition(RobotDARTSimu* simu) { // We do not want to add sensors if we are a ghost robot diff --git a/src/robot_dart/robots/icub.hpp b/src/robot_dart/robots/icub.hpp index 84e757d0..dfe2d10d 100644 --- a/src/robot_dart/robots/icub.hpp +++ b/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>& 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; } diff --git a/src/robot_dart/robots/talos.cpp b/src/robot_dart/robots/talos.cpp index ce524dca..d722769c 100644 --- a/src/robot_dart/robots/talos.cpp +++ b/src/robot_dart/robots/talos.cpp @@ -22,6 +22,17 @@ namespace robot_dart { skeleton()->setPosition(2, 1.57); } + void Talos::reset() + { + Robot::reset(); + + // set a position abobe the floor + skeleton()->setPosition(5, 1.1); + + // rotate the robot + skeleton()->setPosition(2, 1.57); + } + void Talos::_post_addition(RobotDARTSimu* simu) { // We do not want to add sensors if we are a ghost robot diff --git a/src/robot_dart/robots/talos.hpp b/src/robot_dart/robots/talos.hpp index 73b5d21a..3c37515f 100644 --- a/src/robot_dart/robots/talos.hpp +++ b/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>& 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; } From 37b4e0ef9c6a953185b0de2f64c596a33a854585 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Thu, 2 Jun 2022 01:14:15 +0300 Subject: [PATCH 2/3] Better base positioning of robots (not affected by fix_to_world) --- src/robot_dart/robots/icub.cpp | 10 ++++++---- src/robot_dart/robots/talos.cpp | 14 ++++---------- src/robot_dart/robots/tiago.cpp | 10 ++++++---- 3 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/robot_dart/robots/icub.cpp b/src/robot_dart/robots/icub.cpp index ac2bfc5f..6d119fd7 100644 --- a/src/robot_dart/robots/icub.cpp +++ b/src/robot_dart/robots/icub.cpp @@ -12,15 +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(); - 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::_post_addition(RobotDARTSimu* simu) diff --git a/src/robot_dart/robots/talos.cpp b/src/robot_dart/robots/talos.cpp index d722769c..cfb94b02 100644 --- a/src/robot_dart/robots/talos.cpp +++ b/src/robot_dart/robots/talos.cpp @@ -15,22 +15,16 @@ namespace robot_dart { // use position/torque limits set_position_enforced(true); - // set a position abobe the floor - skeleton()->setPosition(5, 1.1); - - // 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::reset() { Robot::reset(); - // set a position abobe the floor - skeleton()->setPosition(5, 1.1); - - // 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) diff --git a/src/robot_dart/robots/tiago.cpp b/src/robot_dart/robots/tiago.cpp index bd81f4a4..4522a021 100644 --- a/src/robot_dart/robots/tiago.cpp +++ b/src/robot_dart/robots/tiago.cpp @@ -7,18 +7,20 @@ namespace robot_dart { : Robot(urdf, packages), _ft_wrist(std::make_shared(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& joint_names, bool override_mimic, bool override_base, bool override_caster) From 747ff009e8112d7deb1c0106a27c3991e302da7b Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Thu, 2 Jun 2022 11:06:15 +0300 Subject: [PATCH 3/3] No need to bind specific functions since reset is virtual now --- src/python/robot.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/python/robot.cpp b/src/python/robot.cpp index a455014e..546b34bd 100644 --- a/src/python/robot.cpp +++ b/src/python/robot.cpp @@ -510,8 +510,7 @@ namespace robot_dart { py::class_>(m, "A1") .def(py::init>&>(), py::arg("urdf") = "unitree_a1/a1.urdf", - py::arg("packages") = std::vector>({{"a1_description", "unitree_a1/a1_description"}})) - .def("reset", &A1::reset); + py::arg("packages") = std::vector>({{"a1_description", "unitree_a1/a1_description"}})); py::class_>(m, "Arm") .def(py::init(), @@ -526,8 +525,7 @@ namespace robot_dart { py::class_>(m, "Hexapod") .def(py::init(), - py::arg("urdf") = "pexod.urdf") - .def("reset", &Hexapod::reset); + py::arg("urdf") = "pexod.urdf"); py::class_>(m, "Iiwa") .def(py::init>&>(), @@ -541,7 +539,6 @@ namespace robot_dart { py::arg("frequency") = 1000, py::arg("urdf") = "tiago/tiago_steel.urdf", py::arg("packages") = std::vector>({{"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, @@ -563,7 +560,6 @@ namespace robot_dart { py::arg("urdf") = "icub/icub.urdf", py::arg("packages") = std::vector>({{"icub_description", "icub/icub_description"}})) - .def("reset", &ICub::reset) .def("imu", &ICub::imu, py::return_value_policy::reference) .def("ft_foot_left", &ICub::ft_foot_left, py::return_value_policy::reference) .def("ft_foot_right", &ICub::ft_foot_right, py::return_value_policy::reference); @@ -574,7 +570,6 @@ namespace robot_dart { py::arg("urdf") = "talos/talos.urdf", py::arg("packages") = std::vector>({{"talos_description", "talos/talos_description"}})) - .def("reset", &Talos::reset) .def("imu", &Talos::imu, py::return_value_policy::reference) .def("ft_foot_left", &Talos::ft_foot_left, py::return_value_policy::reference) .def("ft_foot_right", &Talos::ft_foot_right, py::return_value_policy::reference)