moveit_planning_scene_tutorial

moveit_planning_scene_tutorial

Part1 Add primitive shape into planning scene

​ The planning scene is a very useful tool when we want to create a collision-free trajectory for our robot. The first thing is that whatever our target is, the custom mesh object need to be added into the scene for realistic planning. We first introduce the tutorial on the moveit website.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
......
// Now let's define a collision object ROS message for the robot to avoid.
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group_interface.getPlanningFrame();

// The id of the object is used to identify it.
collision_object.id = "box1";

// Define a box to add to the world.
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.1;
primitive.dimensions[primitive.BOX_Y] = 1.5;
primitive.dimensions[primitive.BOX_Z] = 0.5;

// Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.5;
box_pose.position.y = 0.0;
box_pose.position.z = 0.25;

collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

// Now, let's add the collision object into the world
// (using a vector that could contain additional objects)
ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

// Show text in RViz of status and wait for MoveGroup to receive and process the collision object message
visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

// Now when we plan a trajectory it will avoid the obstacle
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
......

​ We also can attach objects to the robot, so that it moves with the robot geometry. This simulates picking up the object for the purpose of manipulating it. The motion planning should avoid collisions between the two objects as well.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
moveit_msgs::CollisionObject object_to_attach;
object_to_attach.id = "cylinder1";

shape_msgs::SolidPrimitive cylinder_primitive;
cylinder_primitive.type = primitive.CYLINDER;
cylinder_primitive.dimensions.resize(2);
cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20;
cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04;

// We define the frame/pose for this cylinder so that it appears in the gripper
object_to_attach.header.frame_id = move_group_interface.getEndEffectorLink();
geometry_msgs::Pose grab_pose;
grab_pose.orientation.w = 1.0;
grab_pose.position.z = 0.2;

// First, we add the object to the world (without using a vector)
object_to_attach.primitives.push_back(cylinder_primitive);
object_to_attach.primitive_poses.push_back(grab_pose);
object_to_attach.operation = object_to_attach.ADD;
planning_scene_interface.applyCollisionObject(object_to_attach);

// Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to.
// You could also use applyAttachedCollisionObject to attach an object to the robot directly.
ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group_interface.attachObject(object_to_attach.id, "panda_hand");

visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to receive and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot");

// Replan, but now with the object in hand.
move_group_interface.setStartStateToCurrentState();
success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED");
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete");

​ Now we can create the collision-free trajectory with our attached object. Since, we can not replace everything with basic primitive shape in this tutorial, because such a simplification will cause some difference and mismatch in our digital twin and the real environment setting.

Part2 Add custom object into planning scene

This tutorial demonstrates the way to add the custom object into the planning scene. To be more specific, we need a .stl or .dae model file. This difference between .obj, .stl and .dae is described in blog1 and blog2, from which we can know the interconversion between .obj and .stl file can be easily conducted.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
//Vector to scale 3D file units (to convert from mm to meters for example)
Vector3d vectorScale(0.001, 0.001, 0.001);
// Define a collision object ROS message.
moveit_msgs::CollisionObject collision_object;
// The id of the object is used to identify it.
collision_object.id = "custom_object";

//Path where the .dae or .stl object is located
shapes::Mesh* m = shapes::createMeshFromResource("<YOUR_OBJECT_PATH>", vectorScale);
ROS_INFO("Your mesh was loaded");

shape_msgs::Mesh mesh;
shapes::ShapeMsg mesh_msg;
shapes::constructMsgFromShape(m, mesh_msg);
mesh = boost::get<shape_msgs::Mesh>(mesh_msg);

//Define a pose for your mesh (specified relative to frame_id)
geometry_msgs::Pose obj_pose;
obj_pose.position.x = 0.1;
obj_pose.position.y = 0.1;
obj_pose.position.z = 1.0;

// Add the mesh to the Collision object message
collision_object.meshes.push_back(mesh);
collision_object.mesh_poses.push_back(obj_pose);
collision_object.operation = geometry_msgs::Pose::ADD;

// Create vector of collision objects to add
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

// Add the collision object into the world
moveit::planning_interface::MoveGroupInterface move_group(<YOUR_PLANNING_GROUP>);
planning_scene_interface.addCollisionObjects(collision_objects);

​ Adding static meshes in the Rviz will be omitted because we want the pipeline to be automatic.

​ to be continued

Author

Kami-code

Posted on

2021-12-08

Updated on

2021-12-09

Licensed under

Comments