Planning movement with constraints

Constraints in movement of the robot come from various sources. The movement of the robot can be constrained e.g. by their own geometry (self-collisions), by contraints set to the joints, by contraints from the surrounding physical space ...

In the first example it will be presented how collision object in the planning space are handled with MoveIt.

Avoiding collision with an object

Create a new file named avoid_collision.cpp in the fanuc_manipulation package.

Use the following code:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_group_interface_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // Initialising and defining the planning group for move_base
    static const std::string PLANNING_GROUP = "fanuc_arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    // Raw pointers are frequently used to refer to the planning group for improved performance.
    const robot_state::JointModelGroup *joint_model_group =
        move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    // Initialising the PlanninSceneInterface that enables taking
    // collision objects into account during planning
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // Initialisation of Rviz Visual Tools
    moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
    // Clear existing markers
    visual_tools.deleteAllMarkers();
    // Pre-load remote control
    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation().z() = 1;
    visual_tools.publishText(text_pose, "Avoid Box Tutorial", rviz_visual_tools::YELLOW, rviz_visual_tools::XLARGE);
    visual_tools.trigger();

    // Define a collision object ROS message.
    moveit_msgs::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.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[0] = 0.2;
    primitive.dimensions[1] = 0.1;
    primitive.dimensions[2] = 0.3;

    // 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.25;
    box_pose.position.y = -0.15;
    box_pose.position.z = 0.3;

    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
    ROS_INFO_NAMED("tutorial", "Add an object into the world");
    planning_scene_interface.addCollisionObjects(collision_objects);

    // Show text in RViz of status
    text_pose.translation().z() = 0.9;
    visual_tools.publishText(text_pose, "Box added ...", rviz_visual_tools::CYAN, rviz_visual_tools::XLARGE);
    visual_tools.trigger();

    // Wait for MoveGroup to receive and process the collision object message
    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
    move_group.setStartState(*move_group.getCurrentState());
    geometry_msgs::Pose another_pose;
    another_pose.orientation.y = sqrt(2) / 2;
    another_pose.orientation.w = sqrt(2) / 2;
    another_pose.position.x = 0.25;
    another_pose.position.y = 0.0;
    another_pose.position.z = 0.3;
    move_group.setPoseTarget(another_pose);

    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan %s", success ? "" : "FAILED");

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue");

    // Executing the movement
    move_group.move();


    move_group.setStartState(*move_group.getCurrentState());
    another_pose.position.x = 0.0;
    another_pose.position.y = -0.25;
    move_group.setPoseTarget(another_pose);

    success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "Visualizing plan (pose goal move around cuboid) %s", success ? "" : "FAILED");

    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue");

    // Executing the movement
    move_group.move();

    ros::shutdown();
    return 0;
}

Task

Modify the above code in a new file avoid_two.cpp accordingly:

• add another object (cuboid) to planning scene
• the dimensions of the new object are 1, 1, 0.01
• the position of the new object is 0, 0, 0.8
• send the robot again to the first pose