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:moveit [2015/06/02 08:03] – mpomarlantutorials:intermediate:moveit [2016/07/27 14:36] (current) – [Initializing cram_moveit] mpomarlan
Line 3: Line 3:
 **Description:** In this tutorial you will be introduced to some of the functionalities offered by MoveIt! and the interfaces between it and CRAM. **Description:** In this tutorial you will be introduced to some of the functionalities offered by MoveIt! and the interfaces between it and CRAM.
  
-**Next Tutorial:** (coming soon)+**Next Tutorial:** [[tutorials:intermediate:collisions_and_constraints|Collisions and Constraints in MoveIt!]]
  
 ===== Setting up your system ===== ===== Setting up your system =====
Line 11: Line 11:
 Fortunately, CRAM already offers you interfaces to some other useful software packages, and this tutorial will present one of them: MoveIt!. Of course, to run the tutorial, you will have to make sure MoveIt! exists on your system. For installation guidelines, [[http://moveit.ros.org/install/|see here]]. Fortunately, CRAM already offers you interfaces to some other useful software packages, and this tutorial will present one of them: MoveIt!. Of course, to run the tutorial, you will have to make sure MoveIt! exists on your system. For installation guidelines, [[http://moveit.ros.org/install/|see here]].
  
-You will also need the tutorial initialization code. Go to the src folder of your ROS workspace and check whether the cram_tutorials folder exists, and whether it contains cram_intermediate_tutorials. If not, retrieve this code [[https://github.com/cram-code/cram_tutorials|from github]].+You will also need the tutorial initialization code. Go to the src folder of your ROS workspace and check whether the cram_tutorials folder exists, and whether it contains cram_intermediate_tutorials. If not, retrieve this code [[https://github.com/cram2/cram_tutorials|from github]].
  
-===== Initializing cram_bridge/cram_moveit =====+===== Initializing cram_moveit =====
  
 You will need three terminal tabs. In one of them, run You will need three terminal tabs. In one of them, run
Line 23: Line 23:
 In another tab, run In another tab, run
  
-<code>roscd pr2_moveit_config +<code> 
-roslaunch ./launch/demo.launch+roslaunch pr2_moveit_config demo.launch
 </code> </code>
  
Line 31: Line 31:
 Finally, in the last tab run the roslisp REPL. Once it has started up, type the following at its command prompt: Finally, in the last tab run the roslisp REPL. Once it has started up, type the following at its command prompt:
  
-<code lisp>(swank:operate-on-system-for-emacs "cram-intermediate-tutorial" (quote load-op)) +<code lisp> 
- +CL-USER> (ros-load:load-system "cram_intermediate_tutorial" :cram-intermediate-tutorial) 
-(in-package :moveit) +CL-USER> (tuti:init-cram-moveit-tutorial) 
- +CL-USER> (in-package :cram-moveit) 
-(tuti:init-cram-moveit-tutorial)</code>+</code>
  
-This will load the tutorial code (and its dependencies, in particular cram-moveit), and will initialize a few variables we'll be using later. Also, it will start a ROS node from the REPL, which you need to do in order to send messages and service requests to other ROS nodes like move_group, and it will initialize the communication with MoveIt!. It defines publishers for updates to the planning scene (so you can modify what objects exist in the robot's environment), subscribes to robot joint state updates, and creates an actionclient object so that you can send requests to move_group.+This will load the tutorial code (and its dependencies, in particular ''cram-moveit''), and will initialize a few variables we'll be using later. Also, it will start a ROS node from the REPL, which you need to do in order to send messages and service requests to other ROS nodes like ''move_group'', and it will initialize the communication with MoveIt!. It defines publishers for updates to the planning scene (so you can modify what objects exist in the robot's environment), subscribes to robot joint state updates, and creates an ActionLib client object so that you can send requests to ''move_group''.
  
-For now, tuti:init-cram-moveit-tutorial will do all that for you. In your own programs, if you need to make sure those steps happen then you will have to call (or make sure that they are called)+For now, ''tuti:init-cram-moveit-tutorial'' will do all that for you. In your own programs you will have to do the initialization yourself by calling:
  
 <code lisp> <code lisp>
-  (roslisp:start-ros-node node-name) +  (roslisp-utilities:startup-ros) 
-  (moveit:init-moveit-bridge)+  (cram-moveit:init-moveit-bridge)
 </code> </code>
  
Line 84: Line 84:
 </code> </code>
  
-Notice that it's a list of lists (joint-name joint-values), which you could search for the joints that interest you. Or, better, you could just ask MoveIt! for the joints you're interested in:+Notice that it's a list of lists ''(joint-name joint-values)'', which you could search for the joints that interest you. Or, better, you could just ask MoveIt! for the joints you're interested in:
  
 <code lisp> <code lisp>
Line 104: Line 104:
 </code> </code>
  
-Play a bit with the robot in RViz: move one arm (remember to click Plan and Execute to make sure the movement actually happens!), then use joint-states to look at some joint on the arm and see that it changes.+Play a bit with the robot in RViz: move one arm (remember to click Plan and Execute to make sure the movement actually happens!), then use ''joint-states'' to look at some joint on the arm and see that it changes.
  
-=== Moving the robot ===+Another function you can use to get the current robot state is ''get-planning-scene-info''. To get a robot state ROS message (as opposed to a list of name-value pairs), use the following: 
 + 
 +<code lisp> 
 +(second (first (get-planning-scene-info :robot-state T))) 
 +</code> 
 + 
 +==== Moving the robot ====
  
 But actually, you can ask the robot to move from CRAM as well. In the REPL window, try the following: But actually, you can ask the robot to move from CRAM as well. In the REPL window, try the following:
Line 116: Line 122:
 and look in the RViz window for a reaction. After some planning time, usually a fraction of a second, the robot's right wrist will move to the requested location. and look in the RViz window for a reaction. After some planning time, usually a fraction of a second, the robot's right wrist will move to the requested location.
  
-The parameters to move-link-pose are, in order, a link name, a planning group, and a pose-stamped object representing the pose to move to. The pose is something you will typically define yourself in your programs; either hard-coded (as in this tutorial) or through some generation process like [[tutorials:beginner:location_designators|location designator resolution]].+The parameters to ''move-link-pose'' are, in order, a link name, a planning group, and a ''pose-stamped'' object representing the pose to move to. The pose is something you will typically define yourself in your programs; either hard-coded (as in this tutorial) or through some generation process like [[tutorials:beginner:location_designators|location designator resolution]].
  
-Link names and planning groups are found in the *.srdf file for the robot you're working with. For this tutorial, this is the default *.srdf file in the pr2_moveit_config package. You can run MoveIt! with other robots as well ([[http://moveit.ros.org/wiki/PR2/Setup_Assistant/Quick_Start|here's a tutorial]] on how to configure it, even if it shows another configuration for the PR2), in which case you'll just have to use the *.srdf defined for that robot. Link names are self explanatory, and defined in the robot definition files. Planning groups are defined in the *.srdf, and represent kinematic chains on the robot: lists of links that will typically move together to achieve some goal.+Link names and planning groups are found in the ''*.srdf'' file for the robot you're working with. For this tutorial, this is the default ''*.srdf'' file in the ''pr2_moveit_config'' package. You can run MoveIt! with other robots as well ([[http://moveit.ros.org/wiki/PR2/Setup_Assistant/Quick_Start|here's a tutorial]] on how to configure it, even if it shows another configuration for the PR2), in which case you'll just have to use the ''*.srdf'' defined for that robot. Link names are self explanatory, and defined in the robot definition files. Planning groups are defined in the ''*.srdf'', and represent kinematic chains on the robot: lists of links that will typically move together to achieve some goal.
  
 Sometimes you just want to know a trajectory between a robot's state and some goal, but you don't want to also execute the motion. For such situations, you can run something like: Sometimes you just want to know a trajectory between a robot's state and some goal, but you don't want to also execute the motion. For such situations, you can run something like:
Line 124: Line 130:
 <code lisp> <code lisp>
 (multiple-value-bind (start-robot-state-p planned-trajectory-p)  (multiple-value-bind (start-robot-state-p planned-trajectory-p) 
-                     (plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right*)  +    (plan-link-movement "r_wrist_roll_link" "right_arm" tuti:*pose-right*)  
-                     (setf tuti:*start-robot-state* start-robot-state-p)  +  (setf tuti:*start-robot-state* start-robot-state-p)  
-                     (setf tuti:*planned-trajectory* planned-trajectory-p))+  (setf tuti:*planned-trajectory* planned-trajectory-p))
 </code> </code>
  
Line 140: Line 146:
  
 And if you look at the RViz window you should see that this time the robot's state does eventually become the goal state. And if you look at the RViz window you should see that this time the robot's state does eventually become the goal state.
 +
 +So far all movement requests have involved a beneath-the-hood call to MoveIt!'s motion planning, which makes motion generation more capable to handle complicated environments with obstacles in them, and allow motion trajectories to be generated when needed rather than pre-programmed.
 +
 +However, motion planning has its own problems; it often produces strange looking trajectories and takes an unpredictable amount of time to complete, usually several tenths of a second. Put another way, motion planning is often overkill for the task you want the robot to do. Perhaps you have some other, simpler method to generate some waypoints, for example, based on some features of the environment, and are just interested to know if some "simple" movements can connect the waypoints.
 +
 +For such purposes, you can use the compute-cartesian-path function. Here's an example call:
 +
 +<code lisp>
 +(move-link-pose "r_wrist_roll_link" "right_arm" tuti:*pose-mid*)
 +(compute-cartesian-path "/odom_combined"
 +                        (second (first (get-planning-scene-info :robot-state T)))
 +                        "right_arm" 
 +                        "r_wrist_roll_link" 
 +                        (list tuti:*pose-right-msg*)
 +                        0.1 
 +                        1.5 
 +                        t 
 +                        :path-constraints-msg (roslisp:make-msg "moveit_msgs/Constraints"))
 +</code>
 +
 +which you are encouraged to try in the REPL window. The snippet above first makes sure that the robot goes to a certain position, then issues a request to compute a cartesian path to another pose. get-planning-scene-info is another cram-moveit function that we'll look at in the next section, for now we're just using it here to retrieve the current robot state.
 +
 +First, let's see what happened in the RViz window. Notice that the robot state hasn't changed yet- compute-cartesian-path simply computes a trajectory, it won't execute it. You can do that with execute-trajectory as described above. In order to see the generated trajectory, enable the "Loop animation" option in RViz's "Planned Path" section of the side menu. You should see the robot sweeping from center to right with its right arm. 
 +
 +Notice how the arm's wrist moves in a straight line between the start and endpoint. In general, compute-cartesian-path will generate trajectories that link the waypoints via linear motions in cartesian space; that is, the link you specify waypoints for will move in a line between them.
 +
 +Let's now look at the return values provided by compute-cartesian-path. In the REPL window, you should see something like this
 +
 +<code lisp>
 +(("start state" <a robot_state ROS message here>) 
 + ("trajectory" <a robot_trajectory ROS message>) 
 + ("completion fraction" <a number, 1.0 if trajectory successful>))
 +</code>
 +
 +You can rely on the order of the named pairs in the response to always be the same ("start state", then "trajectory", then "completion fraction"); the names are there simply for user readability. So if you wanted, for example, to simply extract the trajectory from the response (so that you could execute it later) you could write something like
 +
 +<code lisp>
 +(second (second (compute-cartesian-path "/odom_combined"
 +                                        (second (first (get-planning-scene-info :robot-state T)))
 +                                        "right_arm" 
 +                                        "r_wrist_roll_link" 
 +                                        (list tuti:*pose-right-msg*) 
 +                                        0.1 
 +                                        1.5 
 +                                        t 
 +                                        (roslisp:make-msg "moveit_msgs/Constraints"))))
 +</code>
 +
 +The third part of the return value ("completion fraction") tells you whether the trajectory was successfully generated and if not, how much of it could be done. A complete trajectory will have a completion fraction of 1.0. A failed trajectory will be somewhere between 1.0 and 0.0. Failures in generating a trajectory can have several causes, but the most important ones are either collisions with obstacles (the robot can't move along a line to the next waypoint because an obstacle blocks the way) or IK failures: either the IK solver found no solution for a point along the trajectory, or it did find one but it requires the arm configuration to "jump" and is therefore an impossible motion.
 +
 +Now let's look at the parameters. The first one ("/odom_combined" in the example) is the frame in which the waypoints are given. The second parameter is a robot state message that represents the state at the start of the trajectory. The next two parameters are strings that identify the planning group (the set of links to move) and respectively the link for which we define waypoints. 
 +
 +The next parameter is a list of waypoints (represented by pose messages); in our example, there's only one waypoint, but there can be several. The trajectory of the named link will be made of line segments connecting the waypoints.
 +
 +The next two parameters are maximum distance (in Cartesian space) between points on the generated trajectory (so the smaller this number, the more points on the generated trajectory), and respectively a jump threshold, a distance in joint space that, if exceeded between consecutive points, is interpreted as a jump in IK solutions. Setting this number may require some fine tuning.
 +
 +Finally, we have a true/false value indicating whether trajectory generation should care about obstacle collisions, and a ROS message describing what other kinematic constraints the trajectory should satisfy. We'll look at constraints in more depth in the next tutorial.
  
 ==== Forward and Inverse Kinematics ==== ==== Forward and Inverse Kinematics ====
Line 305: Line 368:
 You may use robot states different than the current one when, for example, you want to see whether the robot can reach a location on a table with the robot base placed at various positions around that table.  You may use robot states different than the current one when, for example, you want to see whether the robot can reach a location on a table with the robot base placed at various positions around that table. 
  
-compute-ik will also account for collisions with objects in the environment, as well as with the robot itself; the solution it returns (if any) is guaranteed to be collision free. You can also tell MoveIt! that some collisions are ok and will not invalidate a solution. This may happen for example when you want to grab an item from the environment: the item and the gripper will be in collision, but that's the goal!+compute-ik will also account for collisions with objects in the environment, as well as with the robot itself; the solution it returns (if any) is guaranteed to be collision free. You can also tell MoveIt! that some collisions are ok and will not invalidate a solution (we will look into this in [[tutorials:intermediate:collisions_and_constraints|the next tutorial]]). This may happen for example when you want to grab an item from the environment: the item and the gripper will be in collision, but that's the goal! 
 + 
 +Sometimes you may want to tell MoveIt! to not do any collision checking during IK queries. You can do this by using the key parameter :avoid-collisions like so: 
 + 
 +<code lisp> 
 +(compute-ik link-name planning-group-name pose :avoid-collisions nil) 
 +</code>
  
 ==== Manipulating the environment ==== ==== Manipulating the environment ====
Line 405: Line 474:
 when you want to clear the objects you've added to the MoveIt! planning scene from CRAM, but want to keep cram-moveit's list of objects intact. when you want to clear the objects you've added to the MoveIt! planning scene from CRAM, but want to keep cram-moveit's list of objects intact.
  
-==== Collision checking ====+So far we've only written to MoveIt!'s planning scene. It may be useful however to also query it for its current state. To this end, cram-moveit provides the function get-planning-scene-info. Let's look at an example query:
  
-(Work in progress: MoveIt! offers a service to check whether a robot state satisfies a set of constraints (e.g., non-collision with obstacles), and allows adjustments to the Allowed Collision Matrix. However CRAM doesn't yet have a function to wrap calls to this service. Coming soon!) +<code lisp> 
 +(get-planning-scene-info :scene-settings T) 
 +</code> 
 + 
 +for which the response will look something like 
 + 
 +<code lisp> 
 +(("planning scene name" "(noname)+") ("robot model name" "pr2")) 
 +</code> 
 + 
 +Notice how the response is again a list of named pairs. What is in the list depends on the nature of the query. For a query like 
 + 
 +<code lisp> 
 +(get-planning-scene-info :world-object-names T) 
 +</code> 
 + 
 +you will instead get as a response (assuming you still have the cube object in the planning scene) 
 + 
 +<code lisp> 
 +(("world object names" ("CUBE"))) 
 +</code> 
 + 
 +so as you can see the partner for "world object names" is a list of object names. 
 + 
 +Earlier in this tutorial we've also seen  
 + 
 +<code lisp> 
 +(get-planning-scene-info :robot-state T) 
 +</code> 
 + 
 +which puts a pair formed of "robot state" and a robot state message describing the current robot state on the returned list. 
 + 
 +You can combine several queries about the planning scene into one: 
 + 
 +<code lisp> 
 +(get-planning-scene-info :world-object-names T :scene-settings T) 
 +</code> 
 + 
 +and the response will be a concatenation of the responses of the individual queries: 
 + 
 +<code lisp> 
 +(("planning scene name" "(noname)+") ("robot model name" "pr2") 
 + ("world object names" ("CUBE"))) 
 +</code> 
 + 
 +When you issue several queries in a get-planning-scene-info call, do NOT rely on the order of the pairs in the list; use the first element of the pair to identify it. The docstring for get-planning-scene-info will explain what queries are available and what responses they produce.
  
 == Next == == Next ==
  
-(coming soon)+We've mentioned the topic of obstacles and collisions, [[tutorials:intermediate:collisions_and_constraints|it's time to look at it more in-depth]] ... 
 +