Moveit2 - 5. Scenario Planning

Scenario Planning

PlanningScene Class provides the main interface for collision detection and constraint detection . In this tutorial , Will explore this class C++ Interface .

Complete code

The complete code of this tutorial can be found in Here MoveIt GitHub project see .

Set up

have access to RobotModel or URDF and SRDF To easily set up and configure PlanningScene class . however , This is not instantiation PlanningScene The recommended method .PlanningSceneMonitor（ It will be introduced in detail in the next tutorial ） It is a recommended method to use robot joints and sensor data on the robot to create and maintain the current planning scene . In this tutorial , Will directly instantiate a PlanningScene class , But this instantiation method is only for demonstration purposes .

robot_model_loader::RobotModelLoader robot_model_loader(planning_scene_tutorial_node, "robot_description");
const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
planning_scene::PlanningScene planning_scene(kinematic_model);

Self collision detection

The first thing we need to do is to detect whether the robot in the current state is in a self collision state , That is, whether the current configuration of the robot will cause the collision between various parts of the robot . So , Will construct a CollisionRequest Object and a CollisionResult Object and pass them to the collision detection function . Please note that , Whether the robot self collides will be included in the detection result . Self collision detection uses unfilled （unpadded） Version of robot , I.e. direct use URDF Collision mesh provided in without adding additional padding .

collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 1: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

Change state

Now? , Let's change the current state of the robot . The planning scenario will maintain the current state of the robot internally . We can get a reference to the current state and change it , Then collision detection is carried out for the new configuration of the robot . Please pay special attention , Clear before sending a new collision detection request collision_result The object is worth .

moveit::core::RobotState& current_state = planning_scene.getCurrentStateNonConst();
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 2: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

Perform collision detection on the planning group

Now? , We will only deal with Panda Robot hands for collision detection , It will check whether there is any collision between the robot's hand and other parts of the robot's body . We can add the planning group name to the collision request “hand” To specifically request this .

collision_request.group_name = "hand";
current_state.setToRandomPositions();
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 3: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

Get contact information

First , Manual will Panda The arm of the robot （arm） Set it to a position inside that knows that self collision will indeed occur . Please note that , This state is now actually beyond Panda Robot joint limitations , This can also be checked directly .

std::vector<double> joint_values = {
     0.0, 0.0, 0.0, -2.9, 0.0, 1.4, 0.0 };
const moveit::core::JointModelGroup* joint_model_group =     current_state.getJointModelGroup("panda_arm");
current_state.setJointGroupPositions(joint_model_group, joint_values);
RCLCPP_INFO_STREAM(LOGGER, "Test 4: Current state is "
                           << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid"));

Now? , Can be obtained in Panda Contact information of any collision that may occur under the given configuration of the robot arm . Contact information can be requested by filling in the appropriate fields in the collision request and specifying the maximum number of contacts to return .

collision_request.contacts = true;
collision_request.max_contacts = 1000;

collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result);
RCLCPP_INFO_STREAM(LOGGER, "Test 5: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");
collision_detection::CollisionResult::ContactMap::const_iterator it;
for (it = collision_result.contacts.begin(); it != collision_result.contacts.end(); ++it)
{
    
  RCLCPP_INFO(LOGGER, "Contact between: %s and %s", it->first.first.c_str(), it-    >first.second.c_str());
}

Modify the allowable collision matrix

AllowedCollisionMatrix （ACM） Provides a mechanism to tell the collision world to ignore collisions between specific objects ： Specific objects can be components of robots and various objects in the robot world . You can tell the collision detector to ignore all collisions between links reported above , in other words , Even if these links are actually in a collision state , The collision detector will also ignore these collisions , And return the non collision state for this specific state of the robot . In addition, we need to pay attention to how we copy the allowable collision matrix and current state in this example and pass them to the collision detection function .

collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix();
moveit::core::RobotState copied_state = planning_scene.getCurrentState();

collision_detection::CollisionResult::ContactMap::const_iterator it2;
for (it2 = collision_result.contacts.begin(); it2 != collision_result.contacts.end(); ++it2)
{
    
  acm.setEntry(it2->first.first, it2->first.second, true);
}
collision_result.clear();
planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "Test 6: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

Complete collision detection

Although self collision detection has been carried out in the past , But you can use checkCollision Function for self collision detection and environment （ Currently empty ） Collision detection based on . This is the most commonly used set of collision detection functions in planners . Please note that , Collision detection with the environment will use the filled version of the robot . Filling helps keep robots away from obstacles in the environment .

collision_result.clear();
planning_scene.checkCollision(collision_request, collision_result, copied_state, acm);
RCLCPP_INFO_STREAM(LOGGER, "Test 7: Current state is " << (collision_result.collision ? "in" : "not in") << " self collision");

Constraint detection

PlanningScene Class also includes easy-to-use constraint detection function calls . There are two types of constraints ：(a) from KinematicConstraint Set the selected constraints , namely JointConstraint、PositionConstraint、OrientationConstraint and VisibilityConstraint as well as (b) User defined constraint specified by callback function . Let's first look at a simple KinematicConstraint An example of .

Detect kinematic constraints

First of all, we will talk about Panda Robotic panda_arm Define a simple position and orientation constraint on the end effector of the planning group . Note the convenience function for filling constraints （ These functions can be found in moveit_core Package's kinematic_constraints In the catalog utils.h Found in file ）.

std::string end_effector_name = joint_model_group->getLinkModelNames().back();

geometry_msgs::msg::PoseStamped desired_pose;
desired_pose.pose.orientation.w = 1.0;
desired_pose.pose.position.x = 0.3;
desired_pose.pose.position.y = -0.185;
desired_pose.pose.position.z = 0.5;
desired_pose.header.frame_id = "panda_link0";
moveit_msgs::msg::Constraints goal_constraint =
kinematic_constraints::constructGoalConstraints(end_effector_name, desired_pose);

Now? , have access to PlanningScene Class isStateConstrained Function to detect the state of this constraint .

copied_state.setToRandomPositions();
copied_state.update();
bool constrained = planning_scene.isStateConstrained(copied_state, goal_constraint);
RCLCPP_INFO_STREAM(LOGGER, "Test 8: Random state is " << (constrained ? "constrained" : "not constrained"));

There is a more effective constraint detection method （ For example, when you need to detect the same constraint again and again in a planner ）. First build a KinematicConstraintSet, It will be right ROS Constrain the message to preprocess and set it up for fast processing .

kinematic_constraints::KinematicConstraintSet kinematic_constraint_set(kinematic_model);
kinematic_constraint_set.add(goal_constraint, planning_scene.getTransforms());
bool constrained_2 = planning_scene.isStateConstrained(copied_state, kinematic_constraint_set);
RCLCPP_INFO_STREAM(LOGGER, "Test 9: Random state is " << (constrained_2 ? "constrained" : "not constrained"));

There is a use KinematicConstraintSet Direct methods of classes can do this ：

kinematic_constraints::ConstraintEvaluationResult constraint_eval_result =         kinematic_constraint_set.decide(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "Test 10: Random state is "
<< (constraint_eval_result.satisfied ? "constrained" : "not constrained"));

User defined constraints

User defined constraints can also be assigned to PlanningScene class . This is by using setStateFeasibilityPredicate Function specifies a callback function to complete . The following is a simple example of a user-defined callback function , This callback function is used to detect Panda Robotic “panda_joint1” Is it at a positive angle or a negative angle ：

bool stateFeasibilityTestExample(const moveit::core::RobotState& kinematic_state, bool /*verbose*/)
{
    
  const double* joint_values = kinematic_state.getJointPositions("panda_joint1");
  return (joint_values[0] > 0.0);
}

Now? , Whenever you call isStateFeasible function , Will call this user-defined callback function .

planning_scene.setStateFeasibilityPredicate(stateFeasibilityTestExample);
bool state_feasible = planning_scene.isStateFeasible(copied_state);
RCLCPP_INFO_STREAM(LOGGER, "Test 11: Random state is " << (state_feasible ? "feasible" : "not feasible"));

Whenever you call isStateValid when , Three tests will be carried out ：(a) collision detection ;(b) Constraint detection ; Use the user-defined callback function for feasibility detection .

bool state_valid = planning_scene.isStateValid(copied_state, kinematic_constraint_set, "panda_arm");
RCLCPP_INFO_STREAM(LOGGER, "Test 12: Random state is " << (state_valid ? "valid" : "not valid"));

Please note that ,MoveIt2 and OMPL All available planners in currently perform collision detection 、 Constraint detection and feasibility detection using user-defined callback functions .

launch file

The complete startup file is in GitHub On the website here . All the code in this tutorial can be downloaded from moveit_tutorials Compile and run in the software package .

Run code

have access to ros2 launch Command directly from moveit_tutorials The startup file in the software package runs the code ：

ros2 launch moveit2_tutorials planning_scene_tutorial.launch.py

Expected output

The output should look like this , But because we use random joint values , So the output may be different .

moveit2_tutorials: Test 1: Current state is in self collision
moveit2_tutorials: Test 2: Current state is not in self collision
moveit2_tutorials: Test 3: Current state is not in self collision
moveit2_tutorials: Test 4: Current state is valid
moveit2_tutorials: Test 5: Current state is in self collision
moveit2_tutorials: Contact between: panda_leftfinger and panda_link1
moveit2_tutorials: Contact between: panda_link1 and panda_rightfinger
moveit2_tutorials: Test 6: Current state is not in self collision
moveit2_tutorials: Test 7: Current state is not in self collision
moveit2_tutorials: Test 8: Random state is not constrained
moveit2_tutorials: Test 9: Random state is not constrained
moveit2_tutorials: Test 10: Random state is not constrained
moveit2_tutorials: Test 11: Random state is feasible
moveit2_tutorials: Test 12: Random state is not valid