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/03 13:22] – 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_moveit ===== ===== Initializing cram_moveit =====
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. 
 + 
 +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 ==== ==== Moving the robot ====
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 153: Line 159:
                         "right_arm"                          "right_arm" 
                         "r_wrist_roll_link"                          "r_wrist_roll_link" 
-                        tuti:*pose-right-msg* +                        (list tuti:*pose-right-msg*)
                         0.1                          0.1 
                         1.5                          1.5 
                         t                          t 
-                        (roslisp:make-msg "moveit_msgs/Constraints"))+                        :path-constraints-msg (roslisp:make-msg "moveit_msgs/Constraints"))
 </code> </code>
  
Line 362: 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 462: 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]] ... 
 +