tuto using part1
Make sure you installed the extra packages required for this tutorial as explained in the home page
In tutorial 2, a Shadow hand is mounted on a robot arm UR10 from Universal Robot.
The robot_description and the full MoveIt! configuration is given in the repository.
It is not necessary to follow tutorial 1 as a prerequisite, since the MoveIt! config and the robot_description are provided. It is however recommended to have a look at the robot structure of the hand to understand the groups involved
- Access all necessary MoveIt! planning capabilities via a program
- Familiarize with planning settings
This tutorial is dedicated to the 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
- Scene interaction:
PlanningSceneInterface- add / remove objects:
add_box,add_mesh,remove_world_object
- add / remove objects:
- 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
- instantiation:
- In the dexterous-manipulation-tutorial folder, create a python package depending on rospy, rospack, moveit_commander, moveit_msgs, geometry_msgs --rosdistro kinetic
catkin_create_pkg grasp_app rospy rospack moveit_commander moveit_msgs geometry_msgs
cd grasp_app/src- Create
grasp_app.pywith your favorite editor and make it executable
chmod a+x grasp_app.py- 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 It should display Starting grasp app.
- Access the planning scene and the robot
scene = PlanningSceneInterface()
rospy.sleep(1) - 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) - 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- Add a flat box at the pose
scene.add_box("ground", p, (3, 3, 0.02))- Access some resources given in the tutorial (cup and pen)
rospack = rospkg.RosPack()
resourcepath = rospack.get_path('grasp_app')+"/../resources/"- 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')- Modify the previous pose and add mesh for the pen
p.pose.position.x = 0.72
p.pose.position.z = 0.06
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')- Test it, but first start the planning environment (
roslaunch shadow_ur10_moveit_config demo.launch)
roslaunch sr_multi_moveit_config demo.launch robot_description:=`rospack find sr_multi_description`/urdf/right_srhand_ur10_joint_limited.urdf.xacro robot_config_file:=`rospack find sr_multi_moveit_config`/config/robot_configs/right_sh_ur10.yaml
Then run your application
rosrun grasp_app grasp_app.py You should see a plane, a cup and a pen inside, all added to the scene in RViz
The group class permits to access a specific group of your robot system (SRDF)
- Instantiate the RobotCommander class and list the available planning groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()which should display
Available groups: ['rh_fingers', 'rh_first_finger', 'rh_forearm', 'rh_little_finger', 'rh_manipulator', 'rh_middle_finger', 'rh_ring_finger', 'rh_thumb', 'rh_wrist', 'right_arm', 'right_arm_and_hand', 'right_arm_and_manipulator', 'right_arm_and_wrist', 'right_hand']- Initialize the arm group, set the start state to current state, and select the planner and planning time
arm = MoveGroupCommander("right_arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTConnectConfigDefault")
arm.set_planning_time(5.0)- Set a target pose using a pre-stored pose called "ra_up"
gamma
arm.set_named_target("ra_up")- Plan the motion to the target and try it
motionplan = arm.plan()You should see the motion plan being displayed in a loop.
- Add the execution of the previous motion plan (and try it). The execution was made possible by setting
allow_trajectory_executiontoTruein the MoveIt! launch files.
arm.execute(motionplan)
rospy.sleep(1)
You should see the arm in the final target posture (the motion plan is still displayed in a loop)
gamma pose was removed from srdf, only ra-up remains, so picture does not match
- Instead of a named target, one cone also set joint values. And directly execute the plan (if valid)
arm.set_joint_value_target([0.23, -0.7, 1, 1.2, -1.3, 1.4])
arm.go()
rospy.sleep(1)
- Repeat similar steps for the hand group, using another planner, with more time
hand = MoveGroupCommander("right_hand")
hand.set_start_state_to_current_state()
hand.set_planner_id("LBKPIECEkConfigDefault")
hand.set_planning_time(10.0)
hand.set_named_target("open")
hand.go()
You should see a final state like this
victory pose was removed from srdf, only pack remains, so picture does not match
Victory !, you finished the first part of the second tutorial
In next part we will grasp the pen without colliding with the cup.