5 import moveit_commander
6 import geometry_msgs.msg
8 from tf.transformations
import quaternion_from_euler
12 rospy.init_node(
"crane_x7_pick_and_place_controller")
13 robot = moveit_commander.RobotCommander()
14 arm = moveit_commander.MoveGroupCommander(
"arm")
15 arm.set_max_velocity_scaling_factor(0.1)
16 arm.set_max_acceleration_scaling_factor(1.0)
17 gripper = moveit_commander.MoveGroupCommander(
"gripper")
19 while len([s
for s
in rosnode.get_node_names()
if 'rviz' in s]) == 0:
24 print(robot.get_group_names())
26 print(
"Current state:")
27 print(robot.get_current_state())
30 arm_initial_pose = arm.get_current_pose().pose
31 print(
"Arm initial pose:")
32 print(arm_initial_pose)
35 gripper.set_joint_value_target([0.9, 0.9])
39 arm.set_named_target(
"home")
41 gripper.set_joint_value_target([0.7, 0.7])
45 target_pose = geometry_msgs.msg.Pose()
46 target_pose.position.x = 0.2
47 target_pose.position.y = 0.0
48 target_pose.position.z = 0.3
49 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
50 target_pose.orientation.x = q[0]
51 target_pose.orientation.y = q[1]
52 target_pose.orientation.z = q[2]
53 target_pose.orientation.w = q[3]
54 arm.set_pose_target(target_pose)
58 gripper.set_joint_value_target([0.7, 0.7])
62 target_pose = geometry_msgs.msg.Pose()
63 target_pose.position.x = 0.2
64 target_pose.position.y = 0.0
65 target_pose.position.z = 0.13
66 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
67 target_pose.orientation.x = q[0]
68 target_pose.orientation.y = q[1]
69 target_pose.orientation.z = q[2]
70 target_pose.orientation.w = q[3]
71 arm.set_pose_target(target_pose)
75 gripper.set_joint_value_target([0.4, 0.4])
79 target_pose = geometry_msgs.msg.Pose()
80 target_pose.position.x = 0.2
81 target_pose.position.y = 0.0
82 target_pose.position.z = 0.3
83 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
84 target_pose.orientation.x = q[0]
85 target_pose.orientation.y = q[1]
86 target_pose.orientation.z = q[2]
87 target_pose.orientation.w = q[3]
88 arm.set_pose_target(target_pose)
92 target_pose = geometry_msgs.msg.Pose()
93 target_pose.position.x = 0.2
94 target_pose.position.y = 0.2
95 target_pose.position.z = 0.3
96 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
97 target_pose.orientation.x = q[0]
98 target_pose.orientation.y = q[1]
99 target_pose.orientation.z = q[2]
100 target_pose.orientation.w = q[3]
101 arm.set_pose_target(target_pose)
105 target_pose = geometry_msgs.msg.Pose()
106 target_pose.position.x = 0.2
107 target_pose.position.y = 0.2
108 target_pose.position.z = 0.13
109 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
110 target_pose.orientation.x = q[0]
111 target_pose.orientation.y = q[1]
112 target_pose.orientation.z = q[2]
113 target_pose.orientation.w = q[3]
114 arm.set_pose_target(target_pose)
118 gripper.set_joint_value_target([0.7, 0.7])
122 target_pose = geometry_msgs.msg.Pose()
123 target_pose.position.x = 0.2
124 target_pose.position.y = 0.2
125 target_pose.position.z = 0.2
126 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0)
127 target_pose.orientation.x = q[0]
128 target_pose.orientation.y = q[1]
129 target_pose.orientation.z = q[2]
130 target_pose.orientation.w = q[3]
131 arm.set_pose_target(target_pose)
135 arm.set_named_target(
"home")
141 if __name__ ==
'__main__':
144 if not rospy.is_shutdown():
146 except rospy.ROSInterruptException: