tuto using part1

Tutorial 2 : Using the planning environment

Prerequisite

It is not necessary to follow tutorial 1 as a prerequisite, since the MoveIt! config and the robot_description are provided. It is however recommanded to have a look at the robot structure of the hand to understand the groups involved

Part 1 : Access MoveIt! capabilities by program

Objective

• Access all necessary MoveIt! planning capabilities via a program
• Familiarize with planning settings

Python API for MoveIt!

This tutorial is dedicated to discovery of the required elements of the Python API to MoveIt! for arm and hand grasping. For a full description of the API, visit this link

API main methods overview

• Scene interaction: PlanningSceneInterface
  • add / remove objects: add_box, add_mesh, remove_world_object
• Group interaction:
  • instantiation: MoveGroupCommander(group_name)
  • planner settings: set_planner_id , set_planning_time
  • target settings: set_start_state_to_current_state, set_joint_value_target, set_named_target
  • planning and execution: go, pick, plan, execute

Create a new application

• Create a python package depending on rospy, rospack, moveit_commander, moveit_msgs, geometry_msgs

catkin_create_pkg grasp_app rospy rospack moveit_commander moveit_msgs geometry_msgs
cd src

• Create grasp_app.py with your favorite editor and make it executable
• Add these basic code snippets as a base

#!/usr/bin/env python
 
import sys
import rospy
import rospkg, genpy
import yaml
import copy

from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander
from moveit_commander import roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState, Grasp
from geometry_msgs.msg import PoseStamped

if __name__ == "__main__":
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_grasp_app', anonymous=True)
    rospy.loginfo("Starting grasp app")
    # add some code here
    
    rospy.spin()
    roscpp_shutdown()
    rospy.loginfo("Stopping grasp app")

• test it

roscore
rosrun grasp_app grasp_app.py 

Interact with the scene by adding a ground plane and an object

1. Access the planning scene and the robot

scene = PlanningSceneInterface()
rospy.sleep(1)  

2. Cleaning the scene up (for subsequent calls)

scene.remove_world_object("ground")
scene.remove_world_object("cup")
scene.remove_world_object("pen")
rospy.sleep(1)  

3. Create a pose where to put objects, using the world as reference

p = PoseStamped()
p.header.frame_id = "world"
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.05
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1

4. Add a flat box at the pose

scene.add_box("ground", p, (3, 3, 0.02))

5. Access some resources given in the tutorial (cup and pen)

rospack = rospkg.RosPack()
resourcepath = rospack.get_path('grasp_app')+"/../resources/"

6. Modify the previous pose and add mesh for the cup

p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')

7. Modify the previous pose and add mesh for the cup

p.pose.position.x = 0.72
p.pose.position.z = 0.05
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')

8. Test it, but first start the planning environment

roslaunch shadow_ur10_moveit_config demo.launch
rosrun grasp_app grasp_app.py 

You should see a plane, a cup and an object inside added to the scene in rviz

Using the group

The group class permit to access a specific group of your robot system (SRDF)

1. Intantiate the RobotCommander class and list the available planning groups

robot = RobotCommander()
print "Available groups: ",robot.get_group_names()

which should display

['arm', 'armpalm', 'fingers', 'first_finger', 'hand', 'little_finger', 'middle_finger', 'ring_finger', 'thumb']

2. Initialize the arm group, set the start state to current state, and select the planner and planning time

arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault") 
arm.set_planning_time(5.0)

3. Set a target pose using a pre-stored pose called "gamma"

arm.set_named_target("gamma")

4. Plan the motion to the target and try it

motionplan = arm.plan()

You should see the motion plan being displayed in a loop.

6. Add the execution of the previous motion plan (and try it)

arm.execute(motionplan)

You should see the arm in the final target posture (the motion plan is still displayed in a loop)

7. Add a new target and a direct execution with the command go

arm.set_named_target("victory")
arm.go()

8. Repeat similar steps for the hand group, using another planner, with more time

hand = MoveGroupCommander("hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault") 
hand.set_planning_time(10.0)
hand.set_named_target("victory")
hand.go()

You should see a final state like this

Victory !, you finished the first part of the second tutorial

In next part we will grasp the pen without colliding with the cup.