Merge pull request #165 from resibots/support_ur3e

[robots]: add UR3E with Schunk hand

Author: costashatz

src/examples/ur3e.cpp

@@ -0,0 +1,54 @@
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/ur3e.hpp>
+
+#include <robot_dart/control/pd_control.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+int main(int argc, char** argv)
+{
+    bool hand = (argc > 1 && std::string(argv[1]) == "--hand") ? true : false;
+    auto robot = hand ? std::make_shared<robot_dart::robots::Ur3eHand>() : std::make_shared<robot_dart::robots::Ur3e>();
+
+    robot->set_actuator_types("torque");
+
+    std::vector<std::string> dofs = {"shoulder_pan_joint",
+        "shoulder_lift_joint",
+        "elbow_joint",
+        "wrist_1_joint",
+        "wrist_2_joint",
+        "wrist_3_joint"};
+
+    auto up = robot->position_upper_limits(dofs);
+    auto low = robot->position_lower_limits(dofs);
+    for (size_t i = 0; i < dofs.size(); ++i)
+        std::cout << "[" << i << "] " << dofs[i]
+                  << " -> [" << low[i] << ", " << up[i] << "]" << std::endl;
+
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0., -M_PI / 4., M_PI / 2., -M_PI / 4., M_PI / 2., 0.});
+    // add the controller to the robot
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl, dofs);
+    robot->add_controller(controller);
+
+    controller->set_pd(5000., 50.);
+
+    // choose time step of 0.001 seconds
+    robot_dart::RobotDARTSimu simu(0.001);
+    simu.set_collision_detector("fcl");
+    // simu.enable_status_bar(true, 20); // change the font size
+
+#ifdef GRAPHIC
+    auto graphics = std::make_shared<robot_dart::gui::magnum::Graphics>();
+    simu.set_graphics(graphics);
+#endif
+
+    simu.add_checkerboard_floor();
+    simu.add_robot(robot);
+
+    simu.run(10.);
+    std::cout << "robot->pos: " << robot->positions().transpose() << std::endl;
+
+    return 0;
+}

src/python/robot.cpp

@@ -15,6 +15,7 @@
 #include <robot_dart/robots/pendulum.hpp>
 #include <robot_dart/robots/talos.hpp>
 #include <robot_dart/robots/tiago.hpp>
+#include <robot_dart/robots/ur3e.hpp>
 
 #include <robot_dart/control/robot_control.hpp>
 
@@ -565,6 +566,19 @@ namespace robot_dart {
                     py::arg("frequency") = 1000,
                     py::arg("urdf") = "talos/talos_fast.urdf",
                     py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"talos_description", "talos/talos_description"}}));
+
+            py::class_<Ur3e, Robot, std::shared_ptr<Ur3e>>(m, "Ur3e")
+                .def(py::init<size_t, const std::string&, const std::vector<std::pair<std::string, std::string>>&>(),
+                    py::arg("frequency") = 1000,
+                    py::arg("urdf") = "ur3e/ur3e.urdf",
+                    py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"ur3e_description", "ur3e/ur3e_description"}}))
+                .def("ft_wrist", &Ur3e::ft_wrist, py::return_value_policy::reference);
+
+            py::class_<Ur3eHand, Ur3e, std::shared_ptr<Ur3eHand>>(m, "Ur3eHand")
+                .def(py::init<size_t, const std::string&, const std::vector<std::pair<std::string, std::string>>&>(),
+                    py::arg("frequency") = 1000,
+                    py::arg("urdf") = "ur3e/ur3e_with_schunk_hand.urdf",
+                    py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"ur3e_description", "ur3e/ur3e_description"}}));
         }
     } // namespace python
 } // namespace robot_dart

src/robot_dart/robots/ur3e.cpp

@@ -0,0 +1,29 @@
+
+#include "robot_dart/robots/ur3e.hpp"
+#include "robot_dart/robot_dart_simu.hpp"
+
+namespace robot_dart {
+    namespace robots {
+        Ur3e::Ur3e(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("wrist_3-flange"), frequency))
+        {
+            fix_to_world();
+            set_position_enforced(true);
+            set_color_mode("material");
+        }
+
+        void Ur3e::_post_addition(RobotDARTSimu* simu)
+        {
+            // We do not want to add sensors if we are a ghost robot
+            if (ghost())
+                return;
+            simu->add_sensor(_ft_wrist);
+        }
+
+        void Ur3e::_post_removal(RobotDARTSimu* simu)
+        {
+            simu->remove_sensor(_ft_wrist);
+        }
+    } // namespace robots
+} // namespace robot_dart

src/robot_dart/robots/ur3e.hpp

@@ -0,0 +1,27 @@
+#ifndef ROBOT_DART_ROBOTS_UR3E_HPP
+#define ROBOT_DART_ROBOTS_UR3E_HPP
+
+#include "robot_dart/robot.hpp"
+#include "robot_dart/sensor/force_torque.hpp"
+
+namespace robot_dart {
+    namespace robots {
+        class Ur3e : public Robot {
+        public:
+            Ur3e(size_t frequency = 1000, const std::string& urdf = "ur3e/ur3e.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"ur3e_description", "ur3e/ur3e_description"}});
+
+            const sensor::ForceTorque& ft_wrist() const { return *_ft_wrist; }
+
+        protected:
+            std::shared_ptr<sensor::ForceTorque> _ft_wrist;
+            void _post_addition(RobotDARTSimu* simu) override;
+            void _post_removal(RobotDARTSimu* simu) override;
+        };
+
+        class Ur3eHand : public Ur3e {
+        public:
+            Ur3eHand(size_t frequency = 1000, const std::string& urdf = "ur3e/ur3e_with_schunk_hand.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"ur3e_description", "ur3e/ur3e_description"}}) : Ur3e(frequency, urdf, packages) {}
+        };
+    } // namespace robots
+} // namespace robot_dart
+#endif