Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make tf/6d pose consistent #131

Merged
merged 1 commit into from
Mar 12, 2021
Merged

Make tf/6d pose consistent #131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/python/example_parallel.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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