Grasping with Jaco in Gazebo with MoveIt
This is an example to grasp a cube spawned at a predefined position, using the Jaco arm on the table which you can find in package jaco_on_table (part of repository jaco-arm-pkgs).
In this tutorial, the cube is spawned at a fixed predefined position. This allows the use of some helper services. However you may also spawn the cube at a random position instead.
This tutorial goes through the whole process of grasping, moving, and dropping an object step-by-step. This is to demonstrate all the actions necessary, including MoveIt! actions, in the overall grasping pipeline. This should help to understand the process, before the automated grasping tutorial does all this for you.
Familiarize yourself with the object information pipeline described on this wiki page. Other resources which may be helpful to understand the pipeline are
- object information pipeline in MoveIt! in the repository moveit-pkgs
- object information service in Gazebo in the repository gazebo-pkgs
This moveit-pkgs tutorial can help you to make sure the object information pipeline is working correctly.
Also, it is recommended to first do the simple grasping tutorial to familiarize yourself with the process incl. use of the Gazebo grasp plugin. Essentially, we are going to do the same as in the simple grasping tutorial, only that we are going to use MoveIt additionally here.
You will also need the moveit simple controller manager and the moveit RViz visualization package:
sudo apt-get install \
ros-<distro>-moveit-simple-controller-manager \
ros-<distro>-moveit-ros-visualization
As opposed to the simple grasping tutorial, we will also need to load the
"Gazebo object information service"
and the "fake object recognition". This is needed so MoveIt! gets information about the object, and we can see
it in RViz as well.
Use this launch file to load up these services additionally to the Jaco arm:
roslaunch grasp_execution_jaco_tutorial jaco_on_table_gazebo_objects_controlled.launch
Or, if you would like to load the arm using the Gazebo grasp plugin, use the additional argument:
roslaunch grasp_execution_jaco_tutorial jaco_on_table_gazebo_objects_controlled.launch load_grasp_fix:=true
In a new terminal, load up MoveIt! and RViz:
roslaunch jaco_on_table_moveit jaco_on_table_moveit_rviz.launch
Now, spawn the cube and "recognize" the object, so that it is forwarded to MoveIt!. It should also appear in RViz as bright green after the cube has been "recognized".
roslaunch gazebo_test_tools spawn_and_recognize_cube.launch
Or do both steps separately:
rosrun gazebo_test_tools cube_spawner cube1 0.2 0 0.76 robot_base
rosrun gazebo_test_tools fake_object_recognizer_cmd cube1 1
You may also spawn the cube at another position in the first command, as noted earlier. The cube_spawner tool takes as agruments:
- The name of the cube which you must use later to refer to it
- The x,y,z coordinates where to spawn the cube
- The Gazebo link name (not necessarily equal to the /tf link name!) relative to which the pose is given.
You can now either (a) use the MoveIt! RViz plugin to position the arm to grasp the
cube, or (b) you
can move the arm to the fixed position (like in the simple grasping tutorial) and then do fine adjustments
to the position with MoveIt!.
To do (a), use the interactive marker to position the arm and then go on "Plan & Execute".
To do (b):
rosrun grasp_execution_jaco_tutorial set_arm_to_cube_test
In this case, optionally you may want to reset the MoveIt! RViz plugin to display the "current state" as goal
state, so the yellow model of the arm (representing the goal state) adjusts to the current arm position, which we have just changed outside MoveIt!. This will make it more intuitive to change the arm position from its current state with the RViz marker.
Go on Planning -> Query -> Select Goal State -> <current> and click "Update".
Here is how it looks like if you want to change the goal state settings:
Now, you can grasp the cube. If you are using the Gazebo grasp plugin, a notice that the cube has been attached should be printed in the terminal where you launched Gazebo.
rosrun grasp_execution_jaco_tutorial grasp_cube_test
Alternatively, try the full action server command to grasp the cube. While the last command just closes the grippers, this generates a full Grasp.action request. This is also described on this wiki page.
roslaunch grasp_execution test_grasp.launch object_name:=cube1 robot_namespace:=jaco
Move the arm to the default "home" pose (or it could be any other pose) so that it lifts the cube. It could be any pose, but lets do the "home" pose first, and then use MoveIt! in the second step:
rosrun grasp_execution_jaco_tutorial set_arm_to_cube_test --home --no-fingers
You will notice that the cube in RViz "lags behind" in displaying its current state. This is because
settings in the fake object recognizer only publish an update at a rate which is low by default.
If you want to adjust the publishing rate, you may do it in this config file:
rosed gazebo_test_tools FakeObjectRecognizer.yaml
You now can try to use the MoveIt! RViz plugin to move the arm. Drag the interactive marker in RViz to any random pose, and then click "Plan & Execute".
You may notice that the motion planner will find the arm to be "in collision" with the object. Some versions of MoveIt! may not produce a motion plan because of this, and print errors. Other versions may produce the motion plan, but while the MoveIt! controller is executing the trajectory, the moving collision object will be a problem and lead to the trajectory execution being aborted. To fix this, we will need to attach the cube to the robot in MoveIt!, which we will do in the next step.
Note: If you want to keep using the "Plan & Execute" feature before doing the next step, you will need to ungrasp the object first (see last step in this tutorial). Then, you can move the arm again after clicking on "Plan" only, in order to re-activate the execution, which now still thinks the arm is in an invalid state.
The cube needs to be attached to the robot in MoveIt! so that it is not found "in collision" with the robot. Additionally, attaching the cube has the effect that all motion plans will consider the cube as moving with the robot hand, and then collision with the cube and the environment will be avoided.
To attach the cube to the robot in MoveIt:
rosrun grasp_execution_jaco_tutorial attach_cube_moveit_test cube1
Now, the cube will appear as purple in RViz.
Now, you can use the RViz plugin to move the arm with "Plan & Execute". But before you do this,
you will have to change the planning group to "Arm", so that it excludes the fingers from the motion plan.
Otherwise, it will open the fingers as well, and the object will slip out of the grip.
Change the planning group in the "Planning request" as indicated in the screenshot:
After you have moved the arm, you can ungrasp the cube.
rosrun grasp_execution_jaco_tutorial grasp_cube_test --ungrasp
This will open the hand, and the object in Gazebo will drop. If you are using the Gazebo grasp plugin, a notice should be printed that the object was detached.
However, in RViz, the cube will not appear in the scene as green - it is still attached to the robot in MoveIt!:
To detach the object from the robot:
rosrun grasp_execution_jaco_tutorial attach_cube_moveit_test cube1 --detach
Now, the new cube position is also applied to MoveIt!, and the cube is not attached to the robot any more.
