Fixed doc-comment for robot_state::computeAABB

[peci1]

Description

The docstring says the format of the vector is (minx, miny, minz, maxx, maxy, maxz), but according to both the method's implementation and use in moveit, the format is rather (minx, maxx, miny, maxy, minz, maxz).

Checklist

• Required: Code is auto formatted using clang-format
• Extended the tutorials / documentation, if necessary reference
• Include a screenshot if changing a GUI
• Optional: Created tests, which fail without this PR reference
• Optional: Decide if this should be cherry-picked to other current ROS branches (Indigo, Jade, Kinetic) -- definitely

Thank you!

[peci1]

Moreover, looking into updateAABB implementation, it really seems to be wrong.

Correct me if I'm missing a hidden assumption, but if I take the "maximal" corner of a box and transform it with an affine transform to a different frame, it no longer holds that this corner has to be the "maximal" in the new coordinate frame. I really think you should check all 8 vertices to be both larger than and lower than the current AABB.

[davetcoleman]

I have restarted the travis CI check - it seemed to be a failed internet connection.

both the method's implementation and use in moveit

Could you link to one of both of the implementation source code locations in your description?

I really think you should check all 8 vertices to be both larger than and lower than the current AABB.

I'm not sure the details of this function, but if you propose a change myself (and others) can look it over

[peci1]

Implementation: https://github.com/ros-planning/moveit/blob/0.9.7/moveit_core/robot_state/src/robot_state.cpp#L2030

if (aabb[0] > c1.x())
  aabb[0] = c1.x();
if (aabb[2] > c1.y())
  aabb[2] = c1.y();
if (aabb[4] > c1.z())
  aabb[4] = c1.z();
if (aabb[1] < c2.x())
  aabb[1] = c2.x();
if (aabb[3] < c2.y())
  aabb[3] = c2.y();
if (aabb[5] < c2.z())
  aabb[5] = c2.z();

Use 1: https://github.com/ros-planning/moveit/blob/0.9.7/moveit_ros/robot_interaction/src/robot_interaction.cpp#L224

v.size = std::max(std::max(aabb[1] - aabb[0], aabb[3] - aabb[2]), aabb[5] - aabb[4]);

Use 2: https://github.com/ros-planning/moveit/blob/0.9.7/moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp#L78

mk.pose.position.x = (aabb[0] + aabb[1]) / 2.0;
mk.pose.position.y = (aabb[2] + aabb[3]) / 2.0;
mk.pose.position.z = (aabb[4] + aabb[5]) / 2.0;

[peci1]

And this is my code that takes into account all vertices. I'm not sending it as a PR yet, because I just want to know from you if the idea looks correct to you. The piece of code was inspired by updateAABB, but was integrated elsewhere, so it is now using some member variables etc...

inline void update(const Eigen::Affine3d& transform, const Eigen::Vector3d& extents) {
    Eigen::Vector3d half = extents / 2.0;
    const Eigen::Vector3d max = transform * half;
    half = -half;
    const Eigen::Vector3d min = transform * half;

    // now we walk around all the box's 8 vertices
    Eigen::Vector3d corner = max;
    this->updateFromPoint(corner);
    corner[0] = min[0];
    this->updateFromPoint(corner);
    corner[1] = min[1];
    this->updateFromPoint(corner);
    corner[2] = min[2];  // corner == min
    this->updateFromPoint(corner);

    corner[1] = max[1];
    this->updateFromPoint(corner);
    corner[0] = max[0];
    this->updateFromPoint(corner);
    corner[1] = min[1];
    this->updateFromPoint(corner);
    corner[2] = max[2];
    this->updateFromPoint(corner);
}

inline void updateFromPoint(const Eigen::Vector3d& point) {
    if (this->min[0] > point[0])
        this->min[0] = point[0];
    if (this->min[1] > point[1])
        this->min[1] = point[1];
    if (this->min[2] > point[2])
        this->min[2] = point[2];

    if (this->max[0] < point[0])
        this->max[0] = point[0];
    if (this->max[1] < point[1])
        this->max[1] = point[1];
    if (this->max[2] < point[2])
        this->max[2] = point[2];
}

[davetcoleman]

your comment change certainly looks good, picked to J/K

While I understand your other proposed fix by description, I don't understand the code you've presented because I've never worked with that part of the code base. Perhaps you could a) propose the change in a clear PR to this current repo b) create unit tests to demonstrate the error c) use visualization to show a transformed object with a transformed AABB that is not correct or d) all of the above :-p

thanks for your efforts on this!!

[peci1]

I did a quick test, and this is the AABB of a PR2. There's definitely something wrong. It is too high and too narrow.

Or maybe I did the visualization it in a wrong way.

I compute the AABB using this code:

robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_));
robot_state::RobotState ks(kmodel);
ks.setToDefaultValues();

std::vector<double> aabb;
ks.computeAABB(aabb);

And the visualization is done using joint_state_publisher and robot_state_publisher using the "Center" values for all joints. Is that correct, or is the pose after calling setToDefaultValues different from the "all zeros" pose?