From c566ec758659c98cf05e134495d32ad60e11e459 Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Fri, 12 Mar 2021 13:21:09 +0200 Subject: [PATCH] [simu/robot]: make tf/6d pose consistent --- src/python/example_parallel.py | 2 +- src/python/robot.cpp | 19 +++++++++++++++-- src/python/simu.cpp | 9 ++++---- src/robot_dart/robot.cpp | 33 +++++++++++++----------------- src/robot_dart/robot.hpp | 11 +++++++++- src/robot_dart/robot_dart_simu.cpp | 24 ++++++++-------------- src/robot_dart/robot_dart_simu.hpp | 4 ++-- 7 files changed, 58 insertions(+), 44 deletions(-) diff --git a/src/python/example_parallel.py b/src/python/example_parallel.py index 16338af1..5f678896 100644 --- a/src/python/example_parallel.py +++ b/src/python/example_parallel.py @@ -33,7 +33,7 @@ def test(): # add the robot and the floor simu.add_robot(grobot) - simu.add_checkerboard_floor(10., 0.1, 1., np.zeros((6,1)), "floor") + simu.add_checkerboard_floor() # run simu.run(20.) diff --git a/src/python/robot.cpp b/src/python/robot.cpp index 72432a43..9e9ef010 100644 --- a/src/python/robot.cpp +++ b/src/python/robot.cpp @@ -435,19 +435,34 @@ namespace robot_dart { // .def("drawing_axes", &Robot::drawing_axes) - .def_static("create_box", &Robot::create_box, + .def_static("create_box", static_cast (*)(const Eigen::Vector3d&, const Eigen::Vector6d&, const std::string&, double, const Eigen::Vector4d&, const std::string&)>(&Robot::create_box), py::arg("dims"), py::arg("pose") = Eigen::Vector6d::Zero(), py::arg("type") = "free", py::arg("mass") = 1., py::arg("color") = dart::Color::Red(1.0), py::arg("box_name") = "box") - .def_static("create_ellipsoid", &Robot::create_ellipsoid, + .def_static("create_box", static_cast (*)(const Eigen::Vector3d&, const Eigen::Isometry3d&, const std::string&, double, const Eigen::Vector4d&, const std::string&)>(&Robot::create_box), + py::arg("dims"), + py::arg("tf") = Eigen::Isometry3d::Identity(), + py::arg("type") = "free", + py::arg("mass") = 1., + py::arg("color") = dart::Color::Red(1.0), + py::arg("box_name") = "box") + + .def_static("create_ellipsoid", static_cast (*)(const Eigen::Vector3d&, const Eigen::Vector6d&, const std::string&, double, const Eigen::Vector4d&, const std::string&)>(&Robot::create_ellipsoid), py::arg("dims"), py::arg("pose") = Eigen::Vector6d::Zero(), py::arg("type") = "free", py::arg("mass") = 1., py::arg("color") = dart::Color::Red(1.0), + py::arg("ellipsoid_name") = "ellipsoid") + .def_static("create_ellipsoid", static_cast (*)(const Eigen::Vector3d&, const Eigen::Isometry3d&, const std::string&, double, const Eigen::Vector4d&, const std::string&)>(&Robot::create_ellipsoid), + py::arg("dims"), + py::arg("tf") = Eigen::Isometry3d::Identity(), + py::arg("type") = "free", + py::arg("mass") = 1., + py::arg("color") = dart::Color::Red(1.0), py::arg("ellipsoid_name") = "ellipsoid"); } } // namespace python diff --git a/src/python/simu.cpp b/src/python/simu.cpp index 1aa2ca6a..5a459055 100644 --- a/src/python/simu.cpp +++ b/src/python/simu.cpp @@ -164,16 +164,17 @@ namespace robot_dart { .def("add_floor", &RobotDARTSimu::add_floor, py::arg("floor_width") = 10., py::arg("floor_height") = 0.1, - py::arg("pose") = Eigen::Vector6d::Zero(), + py::arg("tf") = Eigen::Isometry3d::Identity(), py::arg("floor_name") = "floor") + .def("add_checkerboard_floor", &RobotDARTSimu::add_checkerboard_floor, py::arg("floor_width") = 10., py::arg("floor_height") = 0.1, py::arg("size") = 1., + py::arg("tf") = Eigen::Isometry3d::Identity(), + py::arg("floor_name") = "checkerboard_floor", py::arg("first_color") = dart::Color::White(1.), - py::arg("second_color") = dart::Color::Gray(1.), - py::arg("pose") = Eigen::Vector6d::Zero(), - py::arg("floor_name") = "checkerboard_floor") + py::arg("second_color") = dart::Color::Gray(1.)) .def("set_collision_detector", &RobotDARTSimu::set_collision_detector) .def("collision_detector", &RobotDARTSimu::collision_detector) diff --git a/src/robot_dart/robot.cpp b/src/robot_dart/robot.cpp index c4cc428b..82de72db 100644 --- a/src/robot_dart/robot.cpp +++ b/src/robot_dart/robot.cpp @@ -1437,11 +1437,7 @@ namespace robot_dart { ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero()); Eigen::Isometry3d bd_trans = bd->getWorldTransform(); - Eigen::Vector6d pose; - pose.head(3) = dart::math::logMap(bd_trans.linear().matrix()); - pose.tail(3) = bd_trans.translation(); - - return pose; + return dart::math::logMap(bd->getWorldTransform()); } Eigen::Vector6d Robot::body_pose_vec(size_t body_index) const @@ -1450,11 +1446,7 @@ namespace robot_dart { Eigen::Isometry3d bd_trans = _skeleton->getBodyNode(body_index)->getWorldTransform(); - Eigen::Vector6d pose; - pose.head(3) = dart::math::logMap(bd_trans.linear().matrix()); - pose.tail(3) = bd_trans.translation(); - - return pose; + return dart::math::logMap(bd_trans); } Eigen::Vector6d Robot::body_velocity(const std::string& body_name) const @@ -2077,6 +2069,11 @@ namespace robot_dart { return M_ret; } + std::shared_ptr Robot::create_box(const Eigen::Vector3d& dims, const Eigen::Isometry3d& tf, const std::string& type, double mass, const Eigen::Vector4d& color, const std::string& box_name) + { + return create_box(dims, dart::math::logMap(tf), type, mass, color, box_name); + } + std::shared_ptr Robot::create_box(const Eigen::Vector3d& dims, const Eigen::Vector6d& pose, const std::string& type, double mass, const Eigen::Vector4d& color, const std::string& box_name) { dart::dynamics::SkeletonPtr box_skel = dart::dynamics::Skeleton::create(box_name); @@ -2106,19 +2103,17 @@ namespace robot_dart { if (type == "free") // free floating robot->set_positions(pose); else // fixed - { - Eigen::Isometry3d T; - T.linear() = dart::math::eulerXYZToMatrix(pose.head(3)); - T.translation() = pose.tail(3); - body->getParentJoint()->setTransformFromParentBodyNode(T); - } + body->getParentJoint()->setTransformFromParentBodyNode(dart::math::expMap(pose)); return robot; } - std::shared_ptr Robot::create_ellipsoid(const Eigen::Vector3d& dims, - const Eigen::Vector6d& pose, const std::string& type, double mass, - const Eigen::Vector4d& color, const std::string& ellipsoid_name) + std::shared_ptr Robot::create_ellipsoid(const Eigen::Vector3d& dims, const Eigen::Isometry3d& tf, const std::string& type, double mass, const Eigen::Vector4d& color, const std::string& ellipsoid_name) + { + return create_ellipsoid(dims, dart::math::logMap(tf), type, mass, color, ellipsoid_name); + } + + std::shared_ptr Robot::create_ellipsoid(const Eigen::Vector3d& dims, const Eigen::Vector6d& pose, const std::string& type, double mass, const Eigen::Vector4d& color, const std::string& ellipsoid_name) { dart::dynamics::SkeletonPtr ellipsoid_skel = dart::dynamics::Skeleton::create(ellipsoid_name); diff --git a/src/robot_dart/robot.hpp b/src/robot_dart/robot.hpp index e07e9ad3..177e0749 100644 --- a/src/robot_dart/robot.hpp +++ b/src/robot_dart/robot.hpp @@ -271,12 +271,21 @@ namespace robot_dart { const std::vector>& drawing_axes() const; // helper functions - // pose: Orientation-Position + static std::shared_ptr create_box(const Eigen::Vector3d& dims, + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& type = "free", + double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0), + const std::string& box_name = "box"); + // pose: 6D log_map static std::shared_ptr create_box(const Eigen::Vector3d& dims, const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& type = "free", double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0), const std::string& box_name = "box"); + static std::shared_ptr create_ellipsoid(const Eigen::Vector3d& dims, + const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& type = "free", + double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0), + const std::string& ellipsoid_name = "ellipsoid"); + // pose: 6D log_map static std::shared_ptr create_ellipsoid(const Eigen::Vector3d& dims, const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& type = "free", double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0), diff --git a/src/robot_dart/robot_dart_simu.cpp b/src/robot_dart/robot_dart_simu.cpp index 9c971e87..7d0863f4 100644 --- a/src/robot_dart/robot_dart_simu.cpp +++ b/src/robot_dart/robot_dart_simu.cpp @@ -473,7 +473,7 @@ namespace robot_dart { return _gui_data->add_text(text, tf, color, alignment, draw_bg, bg_color, font_size); } - std::shared_ptr RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Vector6d& pose, const std::string& floor_name) + std::shared_ptr RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Isometry3d& tf, const std::string& floor_name) { // We do not want 2 floors with the same name! if (_world->getSkeleton(floor_name) != nullptr) @@ -490,19 +490,16 @@ namespace robot_dart { box_node->getVisualAspect()->setColor(dart::Color::Gray()); // Put the body into position - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - // tf.translation() = Eigen::Vector3d(x, y, -floor_height / 2.0); - tf.linear() = dart::math::expMapRot(pose.head(3)); - tf.translation() = pose.tail(3); - tf.translation()[2] -= floor_height / 2.0; - body->getParentJoint()->setTransformFromParentBodyNode(tf); + Eigen::Isometry3d new_tf = tf; + new_tf.translate(Eigen::Vector3d(0., 0., -floor_height / 2.0)); + body->getParentJoint()->setTransformFromParentBodyNode(new_tf); auto floor_robot = std::make_shared(floor_skel, floor_name); add_robot(floor_robot); return floor_robot; } - std::shared_ptr RobotDARTSimu::add_checkerboard_floor(double floor_width, double floor_height, double size, const Eigen::Vector4d& first_color, const Eigen::Vector4d& second_color, const Eigen::Vector6d& pose, const std::string& floor_name) + std::shared_ptr RobotDARTSimu::add_checkerboard_floor(double floor_width, double floor_height, double size, const Eigen::Isometry3d& tf, const std::string& floor_name, const Eigen::Vector4d& first_color, const Eigen::Vector4d& second_color) { // We do not want 2 floors with the same name! if (_world->getSkeleton(floor_name) != nullptr) @@ -520,12 +517,9 @@ namespace robot_dart { main_body->createShapeNodeWith(box); // Put the body into position - Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - // tf.translation() = Eigen::Vector3d(x, y, -floor_height / 2.0); - tf.linear() = dart::math::expMapRot(pose.head(3)); - tf.translation() = pose.tail(3); - tf.translation()[2] -= floor_height / 2.0; - main_body->getParentJoint()->setTransformFromParentBodyNode(tf); + Eigen::Isometry3d new_tf = tf; + new_tf.translate(Eigen::Vector3d(0., 0., -floor_height / 2.0)); + main_body->getParentJoint()->setTransformFromParentBodyNode(new_tf); // Add visual bodies just for visualization int step = std::ceil(floor_width / size); @@ -553,7 +547,7 @@ namespace robot_dart { // Put the body into position Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); - tf.translation() = pose.tail(3) + init_pose; + tf.translation() = init_pose; body->getParentJoint()->setTransformFromParentBodyNode(tf); c++; diff --git a/src/robot_dart/robot_dart_simu.hpp b/src/robot_dart/robot_dart_simu.hpp index edc1c7cb..e156fd8c 100644 --- a/src/robot_dart/robot_dart_simu.hpp +++ b/src/robot_dart/robot_dart_simu.hpp @@ -128,8 +128,8 @@ namespace robot_dart { std::shared_ptr add_text(const std::string& text, const Eigen::Affine2d& tf = Eigen::Affine2d::Identity(), Eigen::Vector4d color = Eigen::Vector4d(1, 1, 1, 1), std::uint8_t alignment = (1 | 3 << 3), bool draw_bg = false, Eigen::Vector4d bg_color = Eigen::Vector4d(0, 0, 0, 0.75), double font_size = 28); - std::shared_ptr add_floor(double floor_width = 10.0, double floor_height = 0.1, const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& floor_name = "floor"); - std::shared_ptr add_checkerboard_floor(double floor_width = 10.0, double floor_height = 0.1, double size = 1., const Eigen::Vector4d& first_color = dart::Color::White(1.), const Eigen::Vector4d& second_color = dart::Color::Gray(1.), const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& floor_name = "checkerboard_floor"); + std::shared_ptr add_floor(double floor_width = 10.0, double floor_height = 0.1, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& floor_name = "floor"); + std::shared_ptr add_checkerboard_floor(double floor_width = 10.0, double floor_height = 0.1, double size = 1., const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& floor_name = "checkerboard_floor", const Eigen::Vector4d& first_color = dart::Color::White(1.), const Eigen::Vector4d& second_color = dart::Color::Gray(1.)); void set_collision_detector(const std::string& collision_detector); // collision_detector can be "DART", "FCL", "Ode" or "Bullet" (case does not matter) const std::string& collision_detector() const;