Skip to content

Commit

Permalink
Merge pull request #131 from resibots/fix_pose_tf
Browse files Browse the repository at this point in the history 
Make tf/6d pose consistent
  • Loading branch information
@costashatz
costashatz authored Mar 12, 2021
2 parents 9ecbd34 + 0c6996e commit 8c1905e
Show file tree
Hide file tree
Showing 7 changed files with 58 additions and 44 deletions.
2 changes: 1 addition & 1 deletion src/python/example_parallel.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.)
Expand Down
19 changes: 17 additions & 2 deletions src/python/robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::shared_ptr<Robot> (*)(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<std::shared_ptr<Robot> (*)(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<std::shared_ptr<Robot> (*)(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<std::shared_ptr<Robot> (*)(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
Expand Down
9 changes: 5 additions & 4 deletions src/python/simu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
33 changes: 14 additions & 19 deletions src/robot_dart/robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -2077,6 +2069,11 @@ namespace robot_dart {
return M_ret;
}

std::shared_ptr<Robot> 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> 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);
Expand Down Expand Up @@ -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> 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> 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> 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);
Expand Down
11 changes: 10 additions & 1 deletion src/robot_dart/robot.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -271,12 +271,21 @@ namespace robot_dart {
const std::vector<std::pair<dart::dynamics::BodyNode*, double>>& drawing_axes() const;

// helper functions
// pose: Orientation-Position
static std::shared_ptr<Robot> 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<Robot> 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<Robot> 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<Robot> 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),
Expand Down
24 changes: 9 additions & 15 deletions src/robot_dart/robot_dart_simu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Robot> RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Vector6d& pose, const std::string& floor_name)
std::shared_ptr<Robot> 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)
Expand All @@ -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<Robot>(floor_skel, floor_name);
add_robot(floor_robot);
return floor_robot;
}

std::shared_ptr<Robot> 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<Robot> 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)
Expand All @@ -520,12 +517,9 @@ namespace robot_dart {
main_body->createShapeNodeWith<dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(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);
Expand Down Expand Up @@ -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++;
Expand Down
4 changes: 2 additions & 2 deletions src/robot_dart/robot_dart_simu.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,8 +128,8 @@ namespace robot_dart {

std::shared_ptr<simu::TextData> 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<Robot> 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<Robot> 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<Robot> 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<Robot> 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;
Expand Down

0 comments on commit 8c1905e

Please sign in to comment.