API for generic Sensors

ci/install_dart.sh

@@ -19,7 +19,7 @@ else
         git checkout v6.9.2
         mkdir build_py
         cd build_py
-        cmake -DDART_BUILD_DARTPY=ON -DCMAKE_BUILD_TYPE=Release ..
+        cmake -DDART_BUILD_DARTPY=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr ..
         make -j4
         sudo make install
     fi

ci/install_dart_latest.sh

@@ -13,16 +13,16 @@ git clone git://github.com/dartsim/dart.git
 cd dart
 mkdir build
 cd build
-cmake -DCMAKE_BUILD_TYPE=Release ..
+if [ "$PYTHON_TESTS" = "ON" ]; then
+cmake -DDART_BUILD_DARTPY=ON -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr ..
+else
+cmake -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr ..
+fi
 make -j4
 sudo make install
 if [ "$PYTHON_TESTS" = "ON" ]; then
-    cd ..
-    mkdir build_py
-    cd build_py
-    cmake -DDART_BUILD_DARTPY=ON -DCMAKE_BUILD_TYPE=Release ..
-    make -j4
-    sudo make install
+    make dartpy -j4
+    sudo make install-dartpy
 fi
 sudo ldconfig
 cd $CI_HOME

src/examples/cameras.cpp

@@ -3,8 +3,8 @@
 #include <robot_dart/control/pd_control.hpp>
 #include <robot_dart/robot_dart_simu.hpp>
 
-#include <robot_dart/gui/magnum/camera_osr.hpp>
 #include <robot_dart/gui/magnum/graphics.hpp>
+#include <robot_dart/gui/magnum/sensor/camera.hpp>
 
 class MyApp : public robot_dart::gui::magnum::GlfwApplication {
 public:
@@ -54,7 +54,7 @@ int main()
     graphics->record_video("video-main.mp4", simu.graphics_freq());
 
     // Add camera
-    auto camera = std::make_shared<robot_dart::gui::magnum::CameraOSR>(&simu, graphics->magnum_app(), 256, 256);
+    auto camera = std::make_shared<robot_dart::sensor::Camera>(&simu, graphics->magnum_app(), 256, 256);
     camera->camera().set_far_plane(5.f);
     camera->camera().record(true, true); // cameras are recording color images by default, enable depth images as well for this example
     // cameras can also record video
@@ -65,15 +65,15 @@ int main()
     tf = Eigen::AngleAxisd(3.14, Eigen::Vector3d{1., 0., 0.});
     tf *= Eigen::AngleAxisd(1.57, Eigen::Vector3d{0., 0., 1.});
     tf.translation() = Eigen::Vector3d(0., 0., 0.1);
-    camera->attach_to("iiwa_link_ee", tf); // cameras are looking towards -z by default
+    camera->attach_to_body(global_robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default
 
     // simu.add_floor(5.);
     simu.add_checkerboard_floor(10.);
     simu.add_robot(global_robot);
     Eigen::Vector6d pose;
     pose << 0., 0., 0., 1.5, 0., 0.1;
     simu.add_robot(robot_dart::Robot::create_box({0.1, 0.1, 0.1}, pose, "free", 1., dart::Color::Red(1.), "box"));
-    simu.add_camera(camera);
+    simu.add_sensor(camera);
 
     simu.run(10.);
 

src/examples/icub.cpp

@@ -7,6 +7,9 @@
 #include <dart/collision/fcl/FCLCollisionDetector.hpp>
 #include <dart/constraint/ConstraintSolver.hpp>
 
+#include <robot_dart/sensor/force_torque.hpp>
+#include <robot_dart/sensor/imu.hpp>
+
 #ifdef GRAPHIC
 #include <robot_dart/gui/magnum/graphics.hpp>
 #endif
@@ -40,6 +43,19 @@ int main()
     simu.add_checkerboard_floor();
     simu.add_robot(robot);
 
+    // Add an IMU sensor to the "chest" body link
+    robot_dart::sensor::IMUConfig imu_config;
+    imu_config.body = robot->body_node("chest"); // choose which body the sensor is attached to
+    imu_config.frequency = 200; // update rate of the sensor
+    auto imu_sensor = simu.add_sensor<robot_dart::sensor::IMU>(&simu, imu_config);
+
+    // Add a force/torque sensor in "r_ankle_roll" joint
+    auto ft_sensor = simu.add_sensor<robot_dart::sensor::ForceTorque>(&simu, robot, "r_ankle_roll");
+
+    // Add some visualizations
+    robot->set_draw_axis(imu_config.body->getName());
+    robot->set_draw_axis("r_ankle_2");
+
     simu.set_control_freq(100); // 100 Hz
     std::vector<std::string> dofs = {
         "l_knee",
@@ -54,6 +70,10 @@ int main()
         "r_shoulder_roll",
     };
 
+    // Format Eigen to std::cout
+    Eigen::IOFormat fmt(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", "\n", "", "");
+    std::cout.precision(4);
+
     auto start = std::chrono::steady_clock::now();
     while (simu.scheduler().next_time() < 20 && !simu.graphics()->done()) {
         if (simu.schedule(simu.control_freq())) {
@@ -71,6 +91,20 @@ int main()
             robot->set_commands(commands, dofs);
         }
         simu.step_world();
+
+        // Print IMU measurements
+        if (simu.schedule(imu_sensor->frequency())) {
+            std::cout << "Angular    Velocity: " << imu_sensor->angular_velocity().transpose().format(fmt) << std::endl;
+            std::cout << "Linear Acceleration: " << imu_sensor->linear_acceleration().transpose().format(fmt) << std::endl;
+            std::cout << "=================================" << std::endl;
+        }
+
+        // Print FT measurements
+        if (simu.schedule(ft_sensor->frequency())) {
+            std::cout << "FT ( force): " << ft_sensor->force().transpose().format(fmt) << std::endl;
+            std::cout << "FT (torque): " << ft_sensor->torque().transpose().format(fmt) << std::endl;
+            std::cout << "=================================" << std::endl;
+        }
     }
     auto end = std::chrono::steady_clock::now();
     std::chrono::duration<double> elapsed_seconds = end - start;

src/python/example.py

@@ -60,12 +60,12 @@ def __call__(self):
 simu.add_robot(robot)
 simu.add_checkerboard_floor(10., 0.1, 1., np.zeros((6,1)), "floor")
 
-# Add a camera to the end-effector of the manipulator
-camera = rd.gui.CameraOSR(simu, graphics.magnum_app(), 256, 256)
+# Add a camera sensor to the end-effector of the manipulator
+camera = rd.sensor.Camera(simu, graphics.magnum_app(), 256, 256)
 rot = dartpy.math.AngleAxis(3.14, [1., 0., 0.]).to_rotation_matrix()
 rot = rot.dot(dartpy.math.AngleAxis(1.57, [0., 0., 1.]).to_rotation_matrix())
-camera.attach_to("arm_link_5", dartpy.math.Isometry3(rotation=rot, translation=[0., 0., 0.1]))
-simu.add_camera(camera)
+camera.attach_to_body(robot.body_node("arm_link_5"), dartpy.math.Isometry3(rotation=rot, translation=[0., 0., 0.1]))
+simu.add_sensor(camera)
 
 simu.run(5.)
 

src/python/gui.cpp

@@ -6,9 +6,11 @@
 
 #include <robot_dart/robot_dart_simu.hpp>
 
+#include <dart/dynamics/BodyNode.hpp>
+
 #ifdef GRAPHIC
-#include <robot_dart/gui/magnum/camera_osr.hpp>
 #include <robot_dart/gui/magnum/graphics.hpp>
+#include <robot_dart/gui/magnum/sensor/camera.hpp>
 #include <robot_dart/gui/magnum/windowless_graphics.hpp>
 #endif
 
@@ -19,6 +21,7 @@ namespace robot_dart {
         {
             auto sm = m.def_submodule("gui");
             auto gsmodule = sm.def_submodule("gs");
+            auto sensormodule = m.def_submodule("sensor");
 
             using namespace robot_dart;
 
@@ -203,45 +206,50 @@ namespace robot_dart {
                     gui::magnum::GlobalData::instance()->set_max_contexts(num_contexts);
                 });
 
-            // CameraOSR class
-            py::class_<gui::magnum::CameraOSR, gui::Base, std::shared_ptr<gui::magnum::CameraOSR>>(sm, "CameraOSR")
-                .def(py::init<RobotDARTSimu*, gui::magnum::BaseApplication*, size_t, size_t, bool>(),
+            // Camera sensor class
+            py::class_<gui::magnum::sensor::Camera, robot_dart::sensor::Sensor, std::shared_ptr<gui::magnum::sensor::Camera>>(sensormodule, "Camera")
+                .def(py::init<RobotDARTSimu*, gui::magnum::BaseApplication*, size_t, size_t, size_t, bool>(),
                     py::arg("simu"),
                     py::arg("app"),
                     py::arg("width"),
                     py::arg("height"),
+                    py::arg("freq") = 30,
                     py::arg("draw_ghost") = false)
 
-                .def("done", &gui::magnum::CameraOSR::done)
-                .def("refresh", &gui::magnum::CameraOSR::refresh)
-                .def("set_render_period", &gui::magnum::CameraOSR::set_render_period)
-                .def("set_enable", &gui::magnum::CameraOSR::set_enable)
+                .def("init", &gui::magnum::sensor::Camera::init)
+
+                .def("calculate", &gui::magnum::sensor::Camera::calculate)
+
+                .def("type", &gui::magnum::sensor::Camera::type)
+
+                .def("attach_to_body", &gui::magnum::sensor::Camera::attach_to_body,
+                    py::arg("body"),
+                    py::arg("tf") = Eigen::Isometry3d::Identity())
+
+                .def("attach_to_joint", &gui::magnum::sensor::Camera::attach_to_joint,
+                    py::arg("joint"),
+                    py::arg("tf") = Eigen::Isometry3d::Identity())
+
+                .def("camera", (Camera & (gui::magnum::sensor::Camera::*)()) & gui::magnum::sensor::Camera::camera, py::return_value_policy::reference)
+
+                .def("drawing_debug", &gui::magnum::sensor::Camera::drawing_debug)
+                .def("draw_debug", &gui::magnum::sensor::Camera::draw_debug,
+                    py::arg("draw") = true)
 
-                .def("look_at", &gui::magnum::CameraOSR::look_at,
+                .def("look_at", &gui::magnum::sensor::Camera::look_at,
                     py::arg("camera_pos"),
                     py::arg("look_at") = Eigen::Vector3d(0, 0, 0),
                     py::arg("up") = Eigen::Vector3d(0, 0, 1))
 
-                .def("render", &gui::magnum::CameraOSR::render)
-
-                .def("record_video", &gui::magnum::CameraOSR::record_video,
-                    py::arg("video_fname"),
-                    py::arg("fps") = -1)
+                .def("record_video", &gui::magnum::sensor::Camera::record_video,
+                    py::arg("video_fname"))
 
                 // Magnum::Image2D* magnum_image()
                 // Magnum::Image2D* magnum_depth_image()
 
-                .def("image", &gui::magnum::CameraOSR::image)
-                .def("depth_image", &gui::magnum::CameraOSR::depth_image)
-                .def("raw_depth_image", &gui::magnum::CameraOSR::raw_depth_image)
-
-                .def("attach_to", &gui::magnum::CameraOSR::attach_to)
-
-                .def("camera", (Camera & (gui::magnum::CameraOSR::*)()) & gui::magnum::CameraOSR::camera, py::return_value_policy::reference)
-
-                .def("drawing_debug", &gui::magnum::CameraOSR::drawing_debug)
-                .def("draw_debug", &gui::magnum::CameraOSR::draw_debug,
-                    py::arg("draw") = true);
+                .def("image", &gui::magnum::sensor::Camera::image)
+                .def("depth_image", &gui::magnum::sensor::Camera::depth_image)
+                .def("raw_depth_image", &gui::magnum::sensor::Camera::raw_depth_image);
 
             // Helper functions
             sm.def("save_png_image", (void (*)(const std::string&, const gui::Image&)) & gui::save_png_image);

src/python/robot.cpp

@@ -7,6 +7,9 @@
 
 #include <robot_dart/control/robot_control.hpp>
 
+#include <dart/dynamics/BodyNode.hpp>
+#include <dart/dynamics/Joint.hpp>
+
 namespace robot_dart {
     namespace python {
         void py_robot(py::module& m)
@@ -36,6 +39,18 @@ namespace robot_dart {
                     py::arg("ghost_color") = Eigen::Vector4d{0.3, 0.3, 0.3, 0.7})
                 .def("skeleton", &Robot::skeleton)
 
+                .def("body_node", (dart::dynamics::BodyNode * (Robot::*)(const std::string&)) & Robot::body_node, py::return_value_policy::reference,
+                    py::arg("body_name"))
+
+                .def("body_node", (dart::dynamics::BodyNode * (Robot::*)(size_t)) & Robot::body_node, py::return_value_policy::reference,
+                    py::arg("body_index"))
+
+                .def("joint", (dart::dynamics::Joint * (Robot::*)(const std::string&)) & Robot::joint, py::return_value_policy::reference,
+                    py::arg("joint_name"))
+
+                .def("joint", (dart::dynamics::Joint * (Robot::*)(size_t)) & Robot::joint, py::return_value_policy::reference,
+                    py::arg("joint_index"))
+
                 .def("name", &Robot::name)
                 .def("model_filename", &Robot::model_filename)
                 .def("model_packages", &Robot::model_packages)

src/python/robot_dart.cc

@@ -4,7 +4,7 @@ PYBIND11_MODULE(RobotDART, m)
 {
     using namespace robot_dart::python;
     // Load dartpy
-    // py::module::import("dartpy");
+    py::module::import("dartpy");
     // Load magnum math
     // py::module::import("magnum.math");
 
@@ -14,6 +14,7 @@ PYBIND11_MODULE(RobotDART, m)
     py_robot(m);
     py_control(m);
     py_utils(m);
+    py_sensors(m);
 
 #ifdef GRAPHIC
     py_gui(m);

src/python/robot_dart.hpp

@@ -8,6 +8,7 @@ namespace robot_dart {
         void py_simu(py::module& m);
         void py_control(py::module& m);
         void py_utils(py::module& m);
+        void py_sensors(py::module& m);
 
 #ifdef GRAPHIC
         void py_gui(py::module& m);