Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
tutorials:intermediate:collisions_and_constraints [2015/06/05 07:52] – Added basic structure and some content for collision checking. mpomarlantutorials:intermediate:collisions_and_constraints [2015/06/18 06:25] (current) – [Allowed Collision Matrix] mpomarlan
Line 63: Line 63:
  
 Let's try to check the other state: Let's try to check the other state:
 +
 +<code lisp>
 +(check-state-validity rs-cube "right_arm" (roslisp:make-msg "moveit_msgs/Constraints"))
 +</code>
 +
 +You should see as a response something like:
  
 <code lisp> <code lisp>
Line 97: Line 103:
  
 (Author's note: cost sources, defined and computed in the package FCL that MoveIt! calls for collision checks, try to approximate contact regions via axis aligned bounding boxes. It's not clear however which features are being used when creating the AABBs; the features are certainly not the complete mesh, and seem instead to center around face intersections.) (Author's note: cost sources, defined and computed in the package FCL that MoveIt! calls for collision checks, try to approximate contact regions via axis aligned bounding boxes. It's not clear however which features are being used when creating the AABBs; the features are certainly not the complete mesh, and seem instead to center around face intersections.)
 +
 +Collision and constraint checking are considered by MoveIt! while doing motion generation as well. For example, let's try this now:
 +
 +<code lisp>
 +(compute-cartesian-path "/odom_combined"
 +                        rs-mid
 +                        "right_arm" 
 +                        "r_wrist_roll_link" 
 +                        (list tuti:*pose-right-msg*)
 +                        0.1 
 +                        1.5 
 +                        t 
 +                        (roslisp:make-msg "moveit_msgs/Constraints"))
 +</code>
 +
 +We ask MoveIt! to create a simple, linear path for the right wrist roll link that will take it from one side of the cube to the other. But that means the motion must pass through the cube, so obviously it will fail; you can see in the RViz window that MoveIt created an incomplete path that stops just before hitting the cube, and you can see in the return values in the REPL window that the completion fraction is below 1.
 +
 +Sometimes however collisions between the robot and objects in the environment are ok, and even desired. To handle such cases, we will look at the Allowed Collision Matrix next.
  
 ==== Allowed Collision Matrix ==== ==== Allowed Collision Matrix ====
 +
 +MoveIt!'s Allowed Collision Matrix (or acm) is a datastructure containing pairs of object names associated to boolean values. If the value is true, then its associated object pair is allowed to collide without invalidating a state. Allowed collisions do not show up in responses from **check-state-validity**. If an object pair does not appear in the acm, it is assumed that collisions between those objects is not tolerated.
 +
 +The acm is useful when you want a more fine-tuned collision checking behavior. Perhaps your robot should touch certain objects (this is a certain requirement for manipulation, unless the PR2 is telekinetic), or perhaps you have some objects in the planning scene that are there just for visualization purposes. In any case, you can specify what collisions to ignore. 
 +
 +Let's remember first how we can retrieve the acm from the planning scene. In the REPL, run the following:
 +
 +<code lisp>
 +(defvar ps-acm nil)
 +(setf ps-acm (second (first (get-planning-scene-info :allowed-collision-matrix t))))
 +</code>
 +
 +Let's query the matrix first. Try:
 +
 +<code lisp>
 +(get-collision-matrix-entry ps-acm "CUBE" "r_forearm_link")
 +</code>
 +
 +(Note: cram-moveit stores objects with names in all-caps, and those are the names it sends to MoveIt! when adding/removing objects. Therefore, the object that MoveIt! knows about is "CUBE", not "cube". Also, cram-moveit cannot send names of objects for acm queries in all caps, because the entries of the acm are mostly populated by MoveIt!, which doesn't follow the all-caps names convention. As a result, you'll have to manually all-caps cram-moveit object names in acm queries. TODO: perhaps remove the cram-moveit all-caps convention?)
 +
 +The response you should get is nil, because this entry wasn't defined yet. Since we'll want to allow some links of the robot to pass through the cube, we'll need to define that entry (and a few others) so we run:
 +
 +<code lisp>
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_forearm_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_roll_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_flex_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_palm_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_r_finger_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_l_finger_link" T))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_motor_accelerometer_link" T))
 +</code>
 +
 +(Note that set-collision-matrix-entry returns a modified acm, but does not change the original acm however.)
 +
 +You can try get-collision-matrix-entry on one of those pairs to verify that its value is now T.
 +
 +The above calls only affected cram-moveit variables, so we now need to make the MoveIt! planning scene aware of the changes. Run the following in the REPL:
 +
 +<code lisp>
 +(set-planning-scene-collision-matrix ps-acm)
 +</code>
 +
 +and lets try the cartesian path request again:
 +
 +<code lisp>
 +(compute-cartesian-path "/odom_combined"
 +                        rs-mid
 +                        "right_arm" 
 +                        "r_wrist_roll_link" 
 +                        (list tuti:*pose-right-msg*)
 +                        0.1 
 +                        1.5 
 +                        t 
 +                        (roslisp:make-msg "moveit_msgs/Constraints"))
 +</code>
 +
 +As you can see, the robot arm now passes through the cube with no protest.
 +
 +Before we proceed, let's remove the new entries from the collision matrix because we will want the cube to function as an obstacle again.
 +
 +<code lisp>
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_forearm_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_roll_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_wrist_flex_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_palm_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_r_finger_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_l_finger_link" nil))
 +(setf ps-acm (set-collision-matrix-entry ps-acm "CUBE" "r_gripper_motor_accelerometer_link" nil))
 +(set-planning-scene-collision-matrix ps-acm)
 +</code>
 +
 +NOTE: MoveIt! will NOT remove entries in the allowed collision matrix when an object is removed from the planning scene. For clean removal, you should remember to do this yourself.
  
 ===== Constraint checking ===== ===== Constraint checking =====
 +
 +Sometimes motion must satisfy more requirements than just getting from A to B without bumping into anything. Suppose, for example, that the robot is carrying a glass full of water, in which case it shouldn't tilt the glass too hard or it will spill. MoveIt! allows you to impose constraints on motion planning, cartesian trajectory generation, and state validity checking. We'll first look at what kinds of constraints MoveIt! supports, and then show an example of how to use a constraint type for motion planning.
  
 ==== Primer to MoveIt! kinematic constraints ==== ==== Primer to MoveIt! kinematic constraints ====
Line 155: Line 253:
  
 If the target radius is on the order of machine epsilon or smaller, the constraint is considered perfectly satisfied. If the target radius is on the order of machine epsilon or smaller, the constraint is considered perfectly satisfied.
 +
 +==== Using kinematic constraints during planning ====
 +
 +From the previous part of the tutorial, we should have a running REPL with the cram moveit tutorial loaded and initialized, an RViz window showing a PR2 and an environment containing only a cube (which is now an obstacle: robot links are not allowed to collide with it), and MoveIt! running in the background.
 +
 +cram-moveit also allows you to define path constraints for **compute-cartesian-path**, **move-link-pose**, as well as imposing constraints for state validity checking, but for the example below we will issue a planning request with constraints.
 +
 +Let's first try a simple motion plan request, in which we ask the robot to move from one pose around the cube to another:
 +
 +<code lisp>
 +(plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right* :start-robot-state rs-mid)
 +</code>
 +
 +(Note the use of the &key parameter start-robot-state to plan a motion from a state that might not be the robot's current state. Also, we're not interested in executing the planned trajectory; we just want to see it in RViz. That's why we don't have the multiple value bind construct around this call, like we did in [[tutorials:intermediate:moveit|the previous tutorial]].)
 +
 +Run that plan request a few times and watch the path taken by the end effector in RViz. You'll notice that the planner has no objection to rotating the gripper in various ways; it hasn't been told not to do it.
 +
 +So let's do just that:
 +
 +<code lisp>
 +(defvar or-con nil)
 +(setf or-con 
 +      (roslisp:make-msg "moveit_msgs/Constraints"
 +                        :orientation_constraints (vector 
 +                                                   (roslisp:make-msg "moveit_msgs/OrientationConstraint"
 +                                                                     :header (roslisp:make-msg "std_msgs/Header"
 +                                                                                               :frame_id "/odom_combined")
 +                                                                     :orientation (roslisp:make-msg "geometry_msgs/Quaternion"
 +                                                                                                    :x 0.0
 +                                                                                                    :y 0.0
 +                                                                                                    :z 0.0
 +                                                                                                    :w 1.0)
 +                                                                     :link_name "r_wrist_roll_link"
 +                                                                     :absolute_x_axis_tolerance 0.01
 +                                                                     :absolute_y_axis_tolerance 0.01
 +                                                                     :absolute_z_axis_tolerance 0.01
 +                                                                     :weight 1.0))))
 +(plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right* :start-robot-state rs-mid 
 +                    :path-constraints-msg or-con)
 +</code>
 +
 +(We leave some tolerance in the axis to help the sampler; a too strict constraint may be impossible to satisfy anyway.)
 +
 +Unfortunately, if you try the code above, and assuming you've made //no changes// to the pr2_moveit_config package, you will see an error signal in the REPL window, and if you look in the RViz window you'll see that the planned path attempts to pass through the cube. What happened?
 +
 +There is a solution however-- look below.
 +
 +==== A note about MoveIt!'s collision checking ====
 +
 +MoveIt! does not do continuous collision checking, which means, instead, that it chooses some points along a robot trajectory segment and does collision checking on them. Usually this won't affect you, unless you work with very narrow objects and/or constraints. 
 +
 +Narrow objects are a problem because the poses MoveIt! chooses to check along a trajectory segment may just miss the object, even if, in fact, the trajectory passes through the object. Kinematic constraints seem to be a problem for a more complicated reason: there tends to be more space, and longer segments, between samples produced by constrained samplers. 
 +
 +There is a tradeoff between increasing the resolution of the collision checks along a trajectory segment and planning time. Sadly, MoveIt! doesn't yet have a way for you to set the resolution at run-time. 
 +
 +For the example above to work, you will have to close move_group, go to pr2_moveit_config/config and edit the ompl_planning.yaml file. Notice that each planning group has a variable longest_valid_segment_fraction. Set its value to 0.005 (rather than the original 0.05), then rerun the pr2_moveit_config/launch/demo.launch file.
 +
 +From the REPL, rerun the lisp code we tried above, for sending a constrained plan request. This time you should receive a multi-value consisting of a robot state and a robot trajectory as answer-- and the trajectory is a valid solution. You can also observe it in RViz (remember to enable Loop Animation from the Planned Path submenu on the left): notice how the planned path keeps the end effector in the same orientation while it moves around the cube. 
 +
 +== Next ==
 +
 +Coming soon!
 +