sr_left_pick_place_example.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Copyright 2019 Shadow Robot Company Ltd.
3 #
4 # This program is free software: you can redistribute it and/or modify it
5 # under the terms of the GNU General Public License as published by the Free
6 # Software Foundation version 2 of the License.
7 #
8 # This program is distributed in the hope that it will be useful, but WITHOUT
9 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
10 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
11 # more details.
12 #
13 # You should have received a copy of the GNU General Public License along
14 # with this program. If not, see <http://www.gnu.org/licenses/>.
15 
16 # This example demonstrates how to move the right hand and arm through a sequence of joint goals that
17 # comprise a simple 'pick and place' demo.
18 # PLEASE NOTE: move_to_joint_value_target_unsafe is used in this script, so collision checks
19 # between the hand and arm are not made!
20 
21 import rospy
22 from sr_robot_commander.sr_arm_commander import SrArmCommander
23 from sr_robot_commander.sr_hand_commander import SrHandCommander
24 
25 rospy.init_node("basic_arm_examples", anonymous=True)
26 
27 hand_commander = SrHandCommander(name="left_hand", prefix="lh")
28 arm_commander = SrArmCommander(name="left_arm", set_ground=False)
29 
30 # Specify goals for hand and arm
31 
32 hand_joint_goals_1 = {'lh_RFJ2': 1.59, 'lh_RFJ3': 1.49, 'lh_RFJ1': 1.47, 'lh_RFJ4': -0.01, 'lh_LFJ4': 0.02,
33  'lh_LFJ5': 0.061, 'lh_LFJ1': 1.41, 'lh_LFJ2': 1.60, 'lh_LFJ3': 1.49, 'lh_THJ2': 0.64,
34  'lh_THJ3': -0.088, 'lh_THJ1': 0.43, 'lh_THJ4': 0.49, 'lh_THJ5': 0.35, 'lh_FFJ4': -0.02,
35  'lh_FFJ2': 1.71, 'lh_FFJ3': 1.49, 'lh_FFJ1': 1.25, 'lh_MFJ3': 1.49, 'lh_MFJ2': 1.66,
36  'lh_MFJ1': 1.31, 'lh_MFJ4': -0.02}
37 
38 hand_joint_goals_2 = {'lh_RFJ2': 0.55, 'lh_RFJ3': 0.08, 'lh_RFJ1': 0.03, 'lh_RFJ4': -0.15, 'lh_LFJ4': -0.35,
39  'lh_LFJ5': 0.23, 'lh_LFJ1': 0.02, 'lh_LFJ2': 0.49, 'lh_LFJ3': -0.02, 'lh_THJ2': -0.08,
40  'lh_THJ3': -0.08, 'lh_THJ1': 0.15, 'lh_THJ4': 0.56, 'lh_THJ5': -0.17, 'lh_FFJ4': -0.34,
41  'lh_FFJ2': 0.30, 'lh_FFJ3': 0.16, 'lh_FFJ1': 0.01, 'lh_MFJ3': 0.19, 'lh_MFJ2': 0.50,
42  'lh_MFJ1': 0.00, 'lh_MFJ4': -0.07}
43 
44 hand_joint_goals_3 = {'lh_RFJ2': 0.63, 'lh_RFJ3': 0.77, 'lh_RFJ1': 0.033, 'lh_RFJ4': -0.02, 'lh_LFJ4': -0.32,
45  'lh_LFJ5': 0.67, 'lh_LFJ1': 0.02, 'lh_LFJ2': 0.73, 'lh_LFJ3': 0.21, 'lh_THJ2': -0.06,
46  'lh_THJ3': -0.04, 'lh_THJ1': 0.39, 'lh_THJ4': 0.85, 'lh_THJ5': 0.40, 'lh_FFJ4': -0.35,
47  'lh_FFJ2': 0.90, 'lh_FFJ3': 0.56, 'lh_FFJ1': 0.02, 'lh_MFJ3': 0.59, 'lh_MFJ2': 0.84,
48  'lh_MFJ1': 0.05, 'lh_MFJ4': -0.08}
49 
50 hand_joint_goals_4 = {'lh_RFJ2': 0.57, 'lh_RFJ3': 0.27, 'lh_RFJ1': 0.04, 'lh_RFJ4': -0.01, 'lh_LFJ4': -0.28,
51  'lh_LFJ5': 0.39, 'lh_LFJ1': 0.01, 'lh_LFJ2': 0.72, 'lh_LFJ3': -0.12, 'lh_THJ2': -0.19,
52  'lh_THJ3': -0.05, 'lh_THJ1': 0.38, 'lh_THJ4': 0.85, 'lh_THJ5': -0.12, 'lh_FFJ4': -0.32,
53  'lh_FFJ2': 0.64, 'lh_FFJ3': -0.03, 'lh_FFJ1': 0.04, 'lh_MFJ3': 0.04, 'lh_MFJ2': 0.83,
54  'lh_MFJ1': 0.01, 'lh_MFJ4': -0.05}
55 
56 hand_joint_goals_5 = {'lh_RFJ2': 1.58, 'lh_RFJ3': 1.52, 'lh_RFJ1': 1.34, 'lh_RFJ4': -0.06, 'lh_LFJ4': -0.20,
57  'lh_LFJ5': 0.09, 'lh_LFJ1': 1.47, 'lh_LFJ2': 1.57, 'lh_LFJ3': 1.40, 'lh_THJ2': -0.01,
58  'lh_THJ3': -0.041, 'lh_THJ1': 0.29, 'lh_THJ4': 0.59, 'lh_THJ5': -1.36, 'lh_FFJ4': 0.03,
59  'lh_FFJ2': 1.72, 'lh_FFJ3': 1.41, 'lh_FFJ1': 1.21, 'lh_MFJ3': 1.39, 'lh_MFJ2': 1.65,
60  'lh_MFJ1': 1.33, 'lh_MFJ4': 0.12}
61 
62 arm_joint_goals_1 = {'la_shoulder_lift_joint': -1.87, 'la_elbow_joint': 1.76, 'la_wrist_2_joint': 0.03,
63  'la_wrist_1_joint': -0.86, 'la_shoulder_pan_joint': -2.64, 'la_wrist_3_joint': 0.69,
64  'lh_WRJ2': -0.02, 'lh_WRJ1': 0.03}
65 
66 arm_joint_goals_2 = {'la_shoulder_lift_joint': -1.86, 'la_elbow_joint': 1.85, 'la_wrist_2_joint': -0.19,
67  'la_wrist_1_joint': -0.96, 'la_shoulder_pan_joint': -1.78, 'la_wrist_3_joint': 1.06,
68  'lh_WRJ2': -0.03, 'lh_WRJ1': -0.02}
69 
70 arm_joint_goals_3 = {'la_shoulder_lift_joint': -1.86, 'la_elbow_joint': 1.90, 'la_wrist_2_joint': -0.18,
71  'la_wrist_1_joint': -0.96, 'la_shoulder_pan_joint': -1.78, 'la_wrist_3_joint': 1.06,
72  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
73 
74 arm_joint_goals_4 = {'la_shoulder_lift_joint': -1.33, 'la_elbow_joint': 1.11, 'la_wrist_2_joint': 1.00,
75  'la_wrist_1_joint': 0.13, 'la_shoulder_pan_joint': -1.49, 'la_wrist_3_joint': 3.27,
76  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
77 
78 arm_joint_goals_5 = {'la_shoulder_lift_joint': -1.45, 'la_elbow_joint': 1.11, 'la_wrist_2_joint': 0.90,
79  'la_wrist_1_joint': 0.45, 'la_shoulder_pan_joint': -0.95, 'la_wrist_3_joint': 0.09,
80  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.16}
81 
82 arm_joint_goals_6 = {'la_shoulder_lift_joint': -1.35, 'la_elbow_joint': 1.16, 'la_wrist_2_joint': 0.96,
83  'la_wrist_1_joint': 0.39, 'la_shoulder_pan_joint': -0.91, 'la_wrist_3_joint': 0.09,
84  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
85 
86 arm_joint_goals_7 = {'la_shoulder_lift_joint': -1.35, 'la_elbow_joint': 1.04, 'la_wrist_2_joint': 1.55,
87  'la_wrist_1_joint': 0.08, 'la_shoulder_pan_joint': -1.64, 'la_wrist_3_joint': -1.41,
88  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
89 
90 arm_joint_goals_8 = {'la_shoulder_lift_joint': -1.55, 'la_elbow_joint': 1.41, 'la_wrist_2_joint': 0.02,
91  'la_wrist_1_joint': 0.61, 'la_shoulder_pan_joint': -1.55, 'la_wrist_3_joint': -0.57,
92  'lh_WRJ2': -0.04, 'lh_WRJ1': 0.16}
93 
94 # Move through each goal
95 # joint states are sent to the commanders, with a time for execution and a flag as to whether
96 # or not the commander should wait for the command to complete before moving to the next command.
97 
98 # Move hand and arm
99 joint_goals = hand_joint_goals_1
100 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
101 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
102 joint_goals = arm_joint_goals_1
103 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
104 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
105 
106 # Move hand and arm
107 joint_goals = hand_joint_goals_2
108 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
109 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
110 joint_goals = arm_joint_goals_2
111 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
112 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
113 
114 # Move arm
115 joint_goals = arm_joint_goals_3
116 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
117 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 1.0, True)
118 
119 # Move hand
120 joint_goals = hand_joint_goals_3
121 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
122 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
123 
124 # Move arm
125 joint_goals = arm_joint_goals_6
126 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
127 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
128 
129 # Move hand
130 joint_goals = hand_joint_goals_4
131 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
132 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
133 
134 # Move arm
135 joint_goals = arm_joint_goals_5
136 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
137 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
138 
139 # Move arm and hand
140 joint_goals = arm_joint_goals_7
141 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
142 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
143 joint_goals = hand_joint_goals_5
144 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
145 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)

sr_example
Author(s): Ugo Cupcic
autogenerated on Wed Oct 14 2020 04:05:12