Differences
This shows you the differences between two versions of the page.
Next revision | Previous revisionNext revisionBoth sides next revision | ||
tutorials:intermediate:collisions_and_constraints [2015/06/04 14:39] – Created and added some notes about MoveIt! constraints. mpomarlan | tutorials:intermediate:collisions_and_constraints [2015/06/05 13:20] – [Using kinematic constraints during planning] mpomarlan | ||
---|---|---|---|
Line 1: | Line 1: | ||
- | (This page under construction, currently | + | ====== Collisions and Constraints in MoveIt! ====== |
+ | |||
+ | **Description: | ||
+ | |||
+ | **Previous Tutorial:** [[tutorials: | ||
+ | |||
+ | **Next Tutorial:** (coming soon) | ||
+ | |||
+ | ===== Collision checking ===== | ||
+ | |||
+ | Let's restart the tutorial. If you have the moveit pr2_config demo running, close it and rerun it. Once the RViz window appears, make sure you have the "Loop animation" | ||
+ | |||
+ | Ensure you have tf running. You don't need to restart it, but if it is not running then open a terminal tab and run | ||
+ | |||
+ | < | ||
+ | rosrun tf2_ros buffer_server | ||
+ | </ | ||
+ | |||
+ | If you have a REPL window running, close it, restart the REPL, then run | ||
+ | |||
+ | <code lisp> | ||
+ | (swank: | ||
+ | (in-package :moveit) | ||
+ | (tuti: | ||
+ | </ | ||
+ | |||
+ | You should now see only the PR2, in the default configuration, | ||
+ | |||
+ | < | ||
+ | (register-collision-object " | ||
+ | |||
+ | (add-collision-object " | ||
+ | </ | ||
+ | |||
+ | We'll now create two robot state messages, representing joint states at two poses. One pose puts the right end effector to the left of the cube, and the other puts the end effector inside it. | ||
+ | |||
+ | <code lisp> | ||
+ | (defvar rs-mid nil) | ||
+ | (defvar rs-cube nil) | ||
+ | |||
+ | (setf rs-mid (compute-ik " | ||
+ | (setf rs-cube (compute-ik " | ||
+ | </ | ||
+ | |||
+ | Note that for this example we run compute-ik with state validation disabled, but even so it is in general good practice to validate robot states when you need to move to them, even if they were good at generation time. The environment may have changed since then. | ||
+ | |||
+ | To check whether a state is valid, we use the cram-moveit: | ||
+ | |||
+ | <code lisp> | ||
+ | (check-state-validity rs-mid " | ||
+ | </ | ||
+ | |||
+ | First, let's look t the parameters. The first is the robot state we wish to check, and the second names the planning group we specifically want to verify. For speed reasons, state checking can be restricted to a subset of the robot' | ||
+ | |||
+ | We'll look at constraints later. For now let's just see what the response from **check-state-validity** was, for a simple collision check. The response should look something like this: | ||
+ | |||
+ | <code lisp> | ||
+ | ((" | ||
+ | | ||
+ | </ | ||
+ | |||
+ | As you can see, it is a list of named pairs, and you can rely on the pairs being in the same order always (the names are just there for user readability). First pair contains a boolean value indicating whether the state respects the defined constraints AND does not collide with any obstacles. In the simplest use cases, that's all you need. The other named pairs contain vectors of ROS messages that describe what, if any, violations occur, whether these are contacts/ | ||
+ | |||
+ | Let's try to check the other state: | ||
+ | |||
+ | <code lisp> | ||
+ | (check-state-validity rs-cube " | ||
+ | </ | ||
+ | |||
+ | You should see as a response something like: | ||
+ | |||
+ | <code lisp> | ||
+ | ((" | ||
+ | | ||
+ | # | ||
+ | | ||
+ | | ||
+ | (:FRAME_ID . "/ | ||
+ | | ||
+ | | ||
+ | (:Y . -0.19797068080424754d0) | ||
+ | (:Z . 0.9014074405165766d0)) | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | | ||
+ | " | ||
+ | | ||
+ | 1 | ||
+ | | ||
+ | " | ||
+ | | ||
+ | | ||
+ | | ||
+ | #(<a long vector of messages> | ||
+ | | ||
+ | </ | ||
+ | |||
+ | Notice that this state is invalid, as expected. Also, notice that the contacts vector tells you what objects collide-- in this case, a link on the robot and the cube-- as well as information about the contact: depth of interpenetration, | ||
+ | |||
+ | MoveIt! should return all contacts it finds between links in the planning group and other objects in the environment (including other links that are not adjacent to each other in the robot' | ||
+ | |||
+ | (Author' | ||
+ | |||
+ | 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 "/ | ||
+ | rs-mid | ||
+ | " | ||
+ | " | ||
+ | (list tuti: | ||
+ | 0.1 | ||
+ | 1.5 | ||
+ | t | ||
+ | (roslisp: | ||
+ | </ | ||
+ | |||
+ | 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 ==== | ||
+ | |||
+ | MoveIt!' | ||
+ | |||
+ | 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, | ||
+ | |||
+ | 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 : | ||
+ | </ | ||
+ | |||
+ | Let's query the matrix first. Try: | ||
+ | |||
+ | <code lisp> | ||
+ | (get-collision-matrix-entry ps-acm " | ||
+ | </ | ||
+ | |||
+ | (Note: cram-moveit stores objects with names in all-caps, and those are the names it sends to MoveIt! when adding/ | ||
+ | |||
+ | 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 " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | </ | ||
+ | |||
+ | (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) | ||
+ | </ | ||
+ | |||
+ | and lets try the cartesian path request again: | ||
+ | |||
+ | <code lisp> | ||
+ | (compute-cartesian-path "/ | ||
+ | rs-mid | ||
+ | " | ||
+ | " | ||
+ | (list tuti: | ||
+ | 0.1 | ||
+ | 1.5 | ||
+ | t | ||
+ | (roslisp: | ||
+ | </ | ||
+ | |||
+ | 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 " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (setf ps-acm (set-collision-matrix-entry ps-acm " | ||
+ | (set-planning-scene-collision-matrix ps-acm) | ||
+ | </ | ||
+ | |||
+ | A similar effect can be achieved by removing and readding the cube to the planning scene like so: | ||
+ | |||
+ | <code lisp> | ||
+ | (remove-collision-object " | ||
+ | (add-collision-object " | ||
+ | </ | ||
+ | |||
+ | because MoveIt! will also remove acm entries associated to a removed object (TODO: or will, once a pull request is merged.) | ||
+ | |||
+ | ===== 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' | ||
==== Primer to MoveIt! kinematic constraints ==== | ==== Primer to MoveIt! kinematic constraints ==== | ||
Line 54: | Line 260: | ||
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, | ||
+ | |||
+ | Let's 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 " | ||
+ | </ | ||
+ | |||
+ | (Note the use of the &key parameter start-robot-state to plan a motion from a state that might not be the robot' | ||
+ | |||
+ | 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: | ||
+ | : | ||
+ | | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | :x 0.0 | ||
+ | :y 0.0 | ||
+ | :z 0.0 | ||
+ | :w 1.0) | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | : | ||
+ | (plan-link-movement " | ||
+ | : | ||
+ | </ | ||
+ | |||
+ | (We leave some tolerance in the axis to help the sampler; a too strict constraint may be impossible to satisfy anyway.) | ||
+ | |||
+ | Now look in the RViz window. You should see that the planned path keeps the end effector in the same orientation while it moves around the cube. (Author' | ||
+ | |||
+ | You can also define path constraints for **compute-cartesian-path** and **move-link-pose**, | ||
+ | |||
+ | == Next == | ||
+ | |||
+ | Coming soon! | ||
+ |