Question

I want to add an XY plane to the PlanningScene so the arm of my robot doesn't hit the ground. Unfortunately I can't work out if I'm doing it right. I don't see anything in RViz. Also, I'm not sure how to instantiate Plane coefficients correctly. Please have a look on this code:

void insertGroundPlane()
{
  moveit_msgs::CollisionObject object;
  object.header.frame_id = "base_footprint";

  shape_msgs::Plane plane;
  plane.coef = {{0, 0, 1, 0}};

  geometry_msgs::Pose pose;
  pose.orientation.w = 1.0;

  object.planes.push_back(plane);
  object.plane_poses.push_back(pose);

  moveit_msgs::PlanningScene planning_scene;
  planning_scene.world.collision_objects.push_back(object);
  planning_scene.is_diff = true;

  moveit_msgs::ApplyPlanningScene srv;
  srv.request.scene = planning_scene;
  planning_scene_diff_client.call(srv);
}

Asked by lubiluk on 2020-01-18 16:51:11 UTC

Answer 1

The easiest way would be to add a box underneath the robot in the URDF that your moveit config is based on, as described in this tutorial. You could add something like this to your URDF:

<link name="ground_plane_box">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <box size="1.0 1.0 0.5"/> 
        </geometry>
        <material name="Grey">
            <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
            <box size="1.0 1.0 0.5"/> 
        </geometry>
    </collision>
</link>

<joint name="ground_plane_box" type="fixed">
    <parent link="world" />
    <child link="ground_plane_box" />
    <origin xyz="0 0 -0.25" rpy="0 0 0" />
</joint>

edit:

I didn't finish reading your post. You want to specifically add the plane, not a box as a workaround (although an advantage of using a box as a robot base is that it will be there immediately when you start the robot, and does not have to be initialized in code).

Anyway, your code looks good to me. Have you tried running it? There may be no visualization of the plane in Rviz, but if you drag a robot via an interactive marker into the ground, you could see if collisions are detected.

Asked by fvd on 2020-02-28 02:38:13 UTC

Comments

Thank you, but you described how to add a box in Gazebo simulator. My question was about MoveIt planning scene and about plane specifically as its API is undocumented.

Asked by lubiluk on 2020-03-01 14:45:05 UTC

My bad for reading too quickly. I edited the answer. For completeness: This is just a box in URDF, which defines the MoveIt robot model, not only objects in Gazebo.

Asked by fvd on 2020-03-01 17:03:56 UTC

How to add a ground plane to MoveIt scene?

Comments

Answers

Comments