Merge pull request #197 from resibots/vx300

Add Interbotix Vx300 manipulator

src/examples/vx300.cpp

@@ -0,0 +1,39 @@
+#include <robot_dart/control/pd_control.hpp>
+#include <robot_dart/robot_dart_simu.hpp>
+#include <robot_dart/robots/vx300.hpp>
+
+#ifdef GRAPHIC
+#include <robot_dart/gui/magnum/graphics.hpp>
+#endif
+
+int main()
+{
+    auto robot = std::make_shared<robot_dart::robots::Vx300>();
+    robot->set_actuator_types("servo");
+
+    Eigen::VectorXd ctrl = robot_dart::make_vector({0.0, 1.0, -1.5, 1.0, 0.5, 0.});
+
+    auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl);
+    robot->add_controller(controller);
+
+    robot_dart::RobotDARTSimu simu;
+    simu.set_collision_detector("fcl");
+#ifdef GRAPHIC
+    simu.set_graphics(std::make_shared<robot_dart::gui::magnum::Graphics>());
+#endif
+    simu.add_robot(robot);
+    simu.add_checkerboard_floor();
+
+    for (auto& n : robot->dof_names()) {
+        std::cout << n << std::endl;
+    }
+
+    simu.run(2.5);
+
+    ctrl << 0.0, -0.5, 0.5, -0.5, 0., 1.;
+    controller->set_parameters(ctrl);
+    controller->set_pd(20., 0.);
+    simu.run(2.5);
+
+    return 0;
+}

src/python/robot.cpp

@@ -16,6 +16,7 @@
 #include <robot_dart/robots/talos.hpp>
 #include <robot_dart/robots/tiago.hpp>
 #include <robot_dart/robots/ur3e.hpp>
+#include <robot_dart/robots/vx300.hpp>
 
 #include <robot_dart/control/robot_control.hpp>
 
@@ -598,6 +599,11 @@ namespace robot_dart {
                     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"}}));
+
+            py::class_<Vx300, Robot, std::shared_ptr<Vx300>>(m, "Vx300")
+                .def(py::init<const std::string&, const std::vector<std::pair<std::string, std::string>>&>(),
+                    py::arg("urdf") = "vx300/vx300.urdf",
+                    py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"interbotix_xsarm_descriptions", "vx300"}}));
         }
     } // namespace python
 } // namespace robot_dart

src/robot_dart/robots/vx300.hpp

@@ -0,0 +1,19 @@
+#ifndef ROBOT_DART_ROBOTS_VX300_HPP
+#define ROBOT_DART_ROBOTS_VX300_HPP
+
+#include "robot_dart/robot.hpp"
+
+namespace robot_dart {
+    namespace robots {
+        class Vx300 : public Robot {
+        public:
+            Vx300(const std::string& urdf = "vx300/vx300.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"interbotix_xsarm_descriptions", "vx300"}}) : Robot(urdf, packages)
+            {
+                fix_to_world();
+                set_position_enforced(true);
+                set_color_mode("aspect");
+            }
+        };
+    } // namespace robots
+} // namespace robot_dart
+#endif