Skip to content

Commit dd731c0

Browse files
costashatzcostashatz
authored
Merge pull request #131 from resibots/fix_pose_tf
Make tf/6d pose consistent
2 parents 66a56eb + c566ec7 commit dd731c0

File tree

7 files changed

+58
-44
lines changed

7 files changed

+58
-44
lines changed

‎src/python/example_parallel.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ def test():
3333

3434
# add the robot and the floor
3535
simu.add_robot(grobot)
36-
simu.add_checkerboard_floor(10., 0.1, 1., np.zeros((6,1)), "floor")
36+
simu.add_checkerboard_floor()
3737

3838
# run
3939
simu.run(20.)

‎src/python/robot.cpp

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -435,19 +435,34 @@ namespace robot_dart {
435435

436436
// .def("drawing_axes", &Robot::drawing_axes)
437437

438-
.def_static("create_box", &Robot::create_box,
438+
.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),
439439
py::arg("dims"),
440440
py::arg("pose") = Eigen::Vector6d::Zero(),
441441
py::arg("type") = "free",
442442
py::arg("mass") = 1.,
443443
py::arg("color") = dart::Color::Red(1.0),
444444
py::arg("box_name") = "box")
445-
.def_static("create_ellipsoid", &Robot::create_ellipsoid,
445+
.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),
446+
py::arg("dims"),
447+
py::arg("tf") = Eigen::Isometry3d::Identity(),
448+
py::arg("type") = "free",
449+
py::arg("mass") = 1.,
450+
py::arg("color") = dart::Color::Red(1.0),
451+
py::arg("box_name") = "box")
452+
453+
.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),
446454
py::arg("dims"),
447455
py::arg("pose") = Eigen::Vector6d::Zero(),
448456
py::arg("type") = "free",
449457
py::arg("mass") = 1.,
450458
py::arg("color") = dart::Color::Red(1.0),
459+
py::arg("ellipsoid_name") = "ellipsoid")
460+
.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),
461+
py::arg("dims"),
462+
py::arg("tf") = Eigen::Isometry3d::Identity(),
463+
py::arg("type") = "free",
464+
py::arg("mass") = 1.,
465+
py::arg("color") = dart::Color::Red(1.0),
451466
py::arg("ellipsoid_name") = "ellipsoid");
452467
}
453468
} // namespace python

‎src/python/simu.cpp

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -164,16 +164,17 @@ namespace robot_dart {
164164
.def("add_floor", &RobotDARTSimu::add_floor,
165165
py::arg("floor_width") = 10.,
166166
py::arg("floor_height") = 0.1,
167-
py::arg("pose") = Eigen::Vector6d::Zero(),
167+
py::arg("tf") = Eigen::Isometry3d::Identity(),
168168
py::arg("floor_name") = "floor")
169+
169170
.def("add_checkerboard_floor", &RobotDARTSimu::add_checkerboard_floor,
170171
py::arg("floor_width") = 10.,
171172
py::arg("floor_height") = 0.1,
172173
py::arg("size") = 1.,
174+
py::arg("tf") = Eigen::Isometry3d::Identity(),
175+
py::arg("floor_name") = "checkerboard_floor",
173176
py::arg("first_color") = dart::Color::White(1.),
174-
py::arg("second_color") = dart::Color::Gray(1.),
175-
py::arg("pose") = Eigen::Vector6d::Zero(),
176-
py::arg("floor_name") = "checkerboard_floor")
177+
py::arg("second_color") = dart::Color::Gray(1.))
177178

178179
.def("set_collision_detector", &RobotDARTSimu::set_collision_detector)
179180
.def("collision_detector", &RobotDARTSimu::collision_detector)

‎src/robot_dart/robot.cpp

Lines changed: 14 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1437,11 +1437,7 @@ namespace robot_dart {
14371437
ROBOT_DART_ASSERT(bd != nullptr, "BodyNode does not exist in skeleton!", Eigen::Vector6d::Zero());
14381438
Eigen::Isometry3d bd_trans = bd->getWorldTransform();
14391439

1440-
Eigen::Vector6d pose;
1441-
pose.head(3) = dart::math::logMap(bd_trans.linear().matrix());
1442-
pose.tail(3) = bd_trans.translation();
1443-
1444-
return pose;
1440+
return dart::math::logMap(bd->getWorldTransform());
14451441
}
14461442

14471443
Eigen::Vector6d Robot::body_pose_vec(size_t body_index) const
@@ -1450,11 +1446,7 @@ namespace robot_dart {
14501446

14511447
Eigen::Isometry3d bd_trans = _skeleton->getBodyNode(body_index)->getWorldTransform();
14521448

1453-
Eigen::Vector6d pose;
1454-
pose.head(3) = dart::math::logMap(bd_trans.linear().matrix());
1455-
pose.tail(3) = bd_trans.translation();
1456-
1457-
return pose;
1449+
return dart::math::logMap(bd_trans);
14581450
}
14591451

14601452
Eigen::Vector6d Robot::body_velocity(const std::string& body_name) const
@@ -2077,6 +2069,11 @@ namespace robot_dart {
20772069
return M_ret;
20782070
}
20792071

2072+
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)
2073+
{
2074+
return create_box(dims, dart::math::logMap(tf), type, mass, color, box_name);
2075+
}
2076+
20802077
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)
20812078
{
20822079
dart::dynamics::SkeletonPtr box_skel = dart::dynamics::Skeleton::create(box_name);
@@ -2106,19 +2103,17 @@ namespace robot_dart {
21062103
if (type == "free") // free floating
21072104
robot->set_positions(pose);
21082105
else // fixed
2109-
{
2110-
Eigen::Isometry3d T;
2111-
T.linear() = dart::math::eulerXYZToMatrix(pose.head(3));
2112-
T.translation() = pose.tail(3);
2113-
body->getParentJoint()->setTransformFromParentBodyNode(T);
2114-
}
2106+
body->getParentJoint()->setTransformFromParentBodyNode(dart::math::expMap(pose));
21152107

21162108
return robot;
21172109
}
21182110

2119-
std::shared_ptr<Robot> Robot::create_ellipsoid(const Eigen::Vector3d& dims,
2120-
const Eigen::Vector6d& pose, const std::string& type, double mass,
2121-
const Eigen::Vector4d& color, const std::string& ellipsoid_name)
2111+
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)
2112+
{
2113+
return create_ellipsoid(dims, dart::math::logMap(tf), type, mass, color, ellipsoid_name);
2114+
}
2115+
2116+
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)
21222117
{
21232118
dart::dynamics::SkeletonPtr ellipsoid_skel
21242119
= dart::dynamics::Skeleton::create(ellipsoid_name);

‎src/robot_dart/robot.hpp

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -271,12 +271,21 @@ namespace robot_dart {
271271
const std::vector<std::pair<dart::dynamics::BodyNode*, double>>& drawing_axes() const;
272272

273273
// helper functions
274-
// pose: Orientation-Position
274+
static std::shared_ptr<Robot> create_box(const Eigen::Vector3d& dims,
275+
const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& type = "free",
276+
double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0),
277+
const std::string& box_name = "box");
278+
// pose: 6D log_map
275279
static std::shared_ptr<Robot> create_box(const Eigen::Vector3d& dims,
276280
const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& type = "free",
277281
double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0),
278282
const std::string& box_name = "box");
279283

284+
static std::shared_ptr<Robot> create_ellipsoid(const Eigen::Vector3d& dims,
285+
const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity(), const std::string& type = "free",
286+
double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0),
287+
const std::string& ellipsoid_name = "ellipsoid");
288+
// pose: 6D log_map
280289
static std::shared_ptr<Robot> create_ellipsoid(const Eigen::Vector3d& dims,
281290
const Eigen::Vector6d& pose = Eigen::Vector6d::Zero(), const std::string& type = "free",
282291
double mass = 1.0, const Eigen::Vector4d& color = dart::Color::Red(1.0),

‎src/robot_dart/robot_dart_simu.cpp

Lines changed: 9 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -473,7 +473,7 @@ namespace robot_dart {
473473
return _gui_data->add_text(text, tf, color, alignment, draw_bg, bg_color, font_size);
474474
}
475475

476-
std::shared_ptr<Robot> RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Vector6d& pose, const std::string& floor_name)
476+
std::shared_ptr<Robot> RobotDARTSimu::add_floor(double floor_width, double floor_height, const Eigen::Isometry3d& tf, const std::string& floor_name)
477477
{
478478
// We do not want 2 floors with the same name!
479479
if (_world->getSkeleton(floor_name) != nullptr)
@@ -490,19 +490,16 @@ namespace robot_dart {
490490
box_node->getVisualAspect()->setColor(dart::Color::Gray());
491491

492492
// Put the body into position
493-
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
494-
// tf.translation() = Eigen::Vector3d(x, y, -floor_height / 2.0);
495-
tf.linear() = dart::math::expMapRot(pose.head(3));
496-
tf.translation() = pose.tail(3);
497-
tf.translation()[2] -= floor_height / 2.0;
498-
body->getParentJoint()->setTransformFromParentBodyNode(tf);
493+
Eigen::Isometry3d new_tf = tf;
494+
new_tf.translate(Eigen::Vector3d(0., 0., -floor_height / 2.0));
495+
body->getParentJoint()->setTransformFromParentBodyNode(new_tf);
499496

500497
auto floor_robot = std::make_shared<Robot>(floor_skel, floor_name);
501498
add_robot(floor_robot);
502499
return floor_robot;
503500
}
504501

505-
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)
502+
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)
506503
{
507504
// We do not want 2 floors with the same name!
508505
if (_world->getSkeleton(floor_name) != nullptr)
@@ -520,12 +517,9 @@ namespace robot_dart {
520517
main_body->createShapeNodeWith<dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(box);
521518

522519
// Put the body into position
523-
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
524-
// tf.translation() = Eigen::Vector3d(x, y, -floor_height / 2.0);
525-
tf.linear() = dart::math::expMapRot(pose.head(3));
526-
tf.translation() = pose.tail(3);
527-
tf.translation()[2] -= floor_height / 2.0;
528-
main_body->getParentJoint()->setTransformFromParentBodyNode(tf);
520+
Eigen::Isometry3d new_tf = tf;
521+
new_tf.translate(Eigen::Vector3d(0., 0., -floor_height / 2.0));
522+
main_body->getParentJoint()->setTransformFromParentBodyNode(new_tf);
529523

530524
// Add visual bodies just for visualization
531525
int step = std::ceil(floor_width / size);
@@ -553,7 +547,7 @@ namespace robot_dart {
553547

554548
// Put the body into position
555549
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
556-
tf.translation() = pose.tail(3) + init_pose;
550+
tf.translation() = init_pose;
557551
body->getParentJoint()->setTransformFromParentBodyNode(tf);
558552

559553
c++;

‎src/robot_dart/robot_dart_simu.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -128,8 +128,8 @@ namespace robot_dart {
128128

129129
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);
130130

131-
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");
132-
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");
131+
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");
132+
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.));
133133

134134
void set_collision_detector(const std::string& collision_detector); // collision_detector can be "DART", "FCL", "Ode" or "Bullet" (case does not matter)
135135
const std::string& collision_detector() const;

0 commit comments

Comments
 (0)