Move Group Python interfaceΒΆ

Follow MoveIt tutorial on Python programming here.

 1#!/usr/bin/env python3
 2
 3import sys
 4import math
 5import sys
 6import rospy
 7import moveit_commander
 8from tf.transformations import quaternion_from_euler
 9from geometry_msgs.msg import Pose
10
11import actionlib
12from control_msgs.msg import GripperCommandAction, GripperCommandGoal
13
14world_frame = "world"
15vel_scaling =  .3
16home_position = [.25, 0, .5]
17home_orientation = [180, .0, -45]
18
19# Gripper
20gripper_open = GripperCommandGoal()
21gripper_open.command.position = 0.0212
22gripper_open.command.max_effort = 0
23
24gripper_close = GripperCommandGoal()
25gripper_close.command.position = 0.02
26gripper_close.command.max_effort = 10
27
28def set_pose(xyz = [0, 0, 0], q = [0, 0, 0]):
29    pose = Pose()
30    pose.position.x = xyz[0]
31    pose.position.y = xyz[1]
32    pose.position.z = xyz[2]
33    quad = quaternion_from_euler(math.radians(q[0]), math.radians(q[1]), math.radians(q[2]))
34    pose.orientation.x = quad[0]
35    pose.orientation.y = quad[1]
36    pose.orientation.z = quad[2]
37    pose.orientation.w = quad[3]
38    return pose
39
40def plan_and_execute(group, pose):
41    group.set_pose_target(pose)
42
43    group.go(wait=True)
44    group.stop()
45    group.clear_pose_targets()
46
47def main():
48    moveit_commander.roscpp_initialize(sys.argv)
49
50    #Setup robot arm planner
51    group = moveit_commander.MoveGroupCommander("panda_arm")
52    group.set_planning_pipeline_id('pilz_industrial_motion_planner')
53    group.set_planner_id('PTP')
54    group.set_max_velocity_scaling_factor(0.2)
55    group.set_max_acceleration_scaling_factor(0.1)
56
57    #Gripper Client
58    gripper = actionlib.SimpleActionClient('franka_gripper/gripper_action', GripperCommandAction)
59    gripper.wait_for_server()
60
61    plan_and_execute(group, set_pose(home_position, home_orientation))
62    gripper.send_goal(goal=gripper_open)
63    gripper.wait_for_result()
64    gripper.send_goal(goal=gripper_close)
65    gripper.wait_for_result()
66
67if __name__ == '__main__':
68    rospy.init_node('panda_goal', anonymous=True)
69
70    main()