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 left 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 # For more information, please see:
22 # https://dexterous-hand.readthedocs.io/en/latest/user_guide/2_software_description.html#robot-commander
23 
24 # roslaunch commands used with this script to launch the robot:
25 # real robot with a NUC (or a separate computer with an RT kernel):
26 # roslaunch sr_left_ur10arm_hand.launch external_control_loop:=true sim:=false scene:=true start_home:=true
27 # simulated robot:
28 # roslaunch sr_left_ur10arm_hand.launch sim:=true scene:=true start_home:=true
29 
30 # PLEASE NOTE: No scene is used in this script and fast movements of the robot. Use on real robot at your own risk.
31 # It is recommended to run this script in simulation first.
32 
33 import rospy
34 from sr_robot_commander.sr_arm_commander import SrArmCommander
35 from sr_robot_commander.sr_hand_commander import SrHandCommander
36 
37 rospy.init_node("basic_arm_examples", anonymous=True)
38 
39 hand_commander = SrHandCommander(name="left_hand", prefix="lh")
40 arm_commander = SrArmCommander(name="left_arm", set_ground=False)
41 
42 # Specify goals for hand and arm
43 hand_joint_goals_1 = {'lh_RFJ2': 1.59, 'lh_RFJ3': 1.49, 'lh_RFJ1': 1.47, 'lh_RFJ4': -0.01, 'lh_LFJ4': 0.02,
44  'lh_LFJ5': 0.061, 'lh_LFJ1': 1.41, 'lh_LFJ2': 1.60, 'lh_LFJ3': 1.49, 'lh_THJ2': 0.64,
45  'lh_THJ3': -0.088, 'lh_THJ1': 0.43, 'lh_THJ4': 0.49, 'lh_THJ5': 0.35, 'lh_FFJ4': -0.02,
46  'lh_FFJ2': 1.71, 'lh_FFJ3': 1.49, 'lh_FFJ1': 1.25, 'lh_MFJ3': 1.49, 'lh_MFJ2': 1.66,
47  'lh_MFJ1': 1.31, 'lh_MFJ4': -0.02}
48 
49 hand_joint_goals_2 = {'lh_RFJ2': 0.55, 'lh_RFJ3': 0.08, 'lh_RFJ1': 0.03, 'lh_RFJ4': -0.15, 'lh_LFJ4': -0.35,
50  'lh_LFJ5': 0.23, 'lh_LFJ1': 0.02, 'lh_LFJ2': 0.49, 'lh_LFJ3': -0.02, 'lh_THJ2': -0.08,
51  'lh_THJ3': -0.08, 'lh_THJ1': 0.15, 'lh_THJ4': 0.56, 'lh_THJ5': -0.17, 'lh_FFJ4': -0.34,
52  'lh_FFJ2': 0.30, 'lh_FFJ3': 0.16, 'lh_FFJ1': 0.01, 'lh_MFJ3': 0.19, 'lh_MFJ2': 0.50,
53  'lh_MFJ1': 0.00, 'lh_MFJ4': -0.07}
54 
55 hand_joint_goals_3 = {'lh_RFJ2': 0.63, 'lh_RFJ3': 0.77, 'lh_RFJ1': 0.033, 'lh_RFJ4': -0.02, 'lh_LFJ4': -0.32,
56  'lh_LFJ5': 0.67, 'lh_LFJ1': 0.02, 'lh_LFJ2': 0.73, 'lh_LFJ3': 0.21, 'lh_THJ2': -0.06,
57  'lh_THJ3': -0.04, 'lh_THJ1': 0.39, 'lh_THJ4': 0.85, 'lh_THJ5': 0.40, 'lh_FFJ4': -0.35,
58  'lh_FFJ2': 0.90, 'lh_FFJ3': 0.56, 'lh_FFJ1': 0.02, 'lh_MFJ3': 0.59, 'lh_MFJ2': 0.84,
59  'lh_MFJ1': 0.05, 'lh_MFJ4': -0.08}
60 
61 hand_joint_goals_4 = {'lh_RFJ2': 0.57, 'lh_RFJ3': 0.27, 'lh_RFJ1': 0.04, 'lh_RFJ4': -0.01, 'lh_LFJ4': -0.28,
62  'lh_LFJ5': 0.39, 'lh_LFJ1': 0.01, 'lh_LFJ2': 0.72, 'lh_LFJ3': -0.12, 'lh_THJ2': -0.19,
63  'lh_THJ3': -0.05, 'lh_THJ1': 0.38, 'lh_THJ4': 0.85, 'lh_THJ5': -0.12, 'lh_FFJ4': -0.32,
64  'lh_FFJ2': 0.64, 'lh_FFJ3': -0.03, 'lh_FFJ1': 0.04, 'lh_MFJ3': 0.04, 'lh_MFJ2': 0.83,
65  'lh_MFJ1': 0.01, 'lh_MFJ4': -0.05}
66 
67 hand_joint_goals_5 = {'lh_RFJ2': 1.58, 'lh_RFJ3': 1.52, 'lh_RFJ1': 1.34, 'lh_RFJ4': -0.06, 'lh_LFJ4': -0.20,
68  'lh_LFJ5': 0.09, 'lh_LFJ1': 1.47, 'lh_LFJ2': 1.57, 'lh_LFJ3': 1.40, 'lh_THJ2': -0.01,
69  'lh_THJ3': -0.041, 'lh_THJ1': 0.29, 'lh_THJ4': 0.59, 'lh_THJ5': -1.36, 'lh_FFJ4': 0.03,
70  'lh_FFJ2': 1.72, 'lh_FFJ3': 1.41, 'lh_FFJ1': 1.21, 'lh_MFJ3': 1.39, 'lh_MFJ2': 1.65,
71  'lh_MFJ1': 1.33, 'lh_MFJ4': 0.12}
72 
73 arm_joint_goals_1 = {'la_shoulder_lift_joint': -1.87, 'la_elbow_joint': 1.76, 'la_wrist_2_joint': 0.03,
74  'la_wrist_1_joint': -0.86, 'la_shoulder_pan_joint': -2.64, 'la_wrist_3_joint': 0.69,
75  'lh_WRJ2': -0.02, 'lh_WRJ1': 0.03}
76 
77 arm_joint_goals_2 = {'la_shoulder_lift_joint': -1.86, 'la_elbow_joint': 1.85, 'la_wrist_2_joint': -0.19,
78  'la_wrist_1_joint': -0.96, 'la_shoulder_pan_joint': -1.78, 'la_wrist_3_joint': 1.06,
79  'lh_WRJ2': -0.03, 'lh_WRJ1': -0.02}
80 
81 arm_joint_goals_3 = {'la_shoulder_lift_joint': -1.86, 'la_elbow_joint': 1.90, 'la_wrist_2_joint': -0.18,
82  'la_wrist_1_joint': -0.96, 'la_shoulder_pan_joint': -1.78, 'la_wrist_3_joint': 1.06,
83  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
84 
85 arm_joint_goals_4 = {'la_shoulder_lift_joint': -1.33, 'la_elbow_joint': 1.11, 'la_wrist_2_joint': 1.00,
86  'la_wrist_1_joint': 0.13, 'la_shoulder_pan_joint': -1.49, 'la_wrist_3_joint': 3.27,
87  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
88 
89 arm_joint_goals_5 = {'la_shoulder_lift_joint': -1.45, 'la_elbow_joint': 1.11, 'la_wrist_2_joint': 0.90,
90  'la_wrist_1_joint': 0.45, 'la_shoulder_pan_joint': -0.95, 'la_wrist_3_joint': 0.09,
91  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.16}
92 
93 arm_joint_goals_6 = {'la_shoulder_lift_joint': -1.35, 'la_elbow_joint': 1.16, 'la_wrist_2_joint': 0.96,
94  'la_wrist_1_joint': 0.39, 'la_shoulder_pan_joint': -0.91, 'la_wrist_3_joint': 0.09,
95  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
96 
97 arm_joint_goals_7 = {'la_shoulder_lift_joint': -1.35, 'la_elbow_joint': 1.04, 'la_wrist_2_joint': 1.55,
98  'la_wrist_1_joint': 0.08, 'la_shoulder_pan_joint': -1.64, 'la_wrist_3_joint': -1.41,
99  'lh_WRJ2': -0.03, 'lh_WRJ1': 0.15}
100 
101 arm_joint_goals_8 = {'la_shoulder_lift_joint': -1.55, 'la_elbow_joint': 1.41, 'la_wrist_2_joint': 0.02,
102  'la_wrist_1_joint': 0.61, 'la_shoulder_pan_joint': -1.55, 'la_wrist_3_joint': -0.57,
103  'lh_WRJ2': -0.04, 'lh_WRJ1': 0.16}
104 
105 # Move through each goal
106 # joint states are sent to the commanders, with a time for execution and a flag as to whether
107 # or not the commander should wait for the command to complete before moving to the next command.
108 
109 # Move hand and arm
110 joint_goals = hand_joint_goals_1
111 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
112 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
113 joint_goals = arm_joint_goals_1
114 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
115 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
116 
117 # Move hand and arm
118 joint_goals = hand_joint_goals_2
119 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
120 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
121 joint_goals = arm_joint_goals_2
122 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
123 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
124 
125 # Move arm
126 joint_goals = arm_joint_goals_3
127 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
128 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 1.0, True)
129 
130 # Move hand
131 joint_goals = hand_joint_goals_3
132 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
133 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
134 
135 # Move arm
136 joint_goals = arm_joint_goals_6
137 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
138 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
139 
140 # Move hand
141 joint_goals = hand_joint_goals_4
142 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
143 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
144 
145 # Move arm
146 joint_goals = arm_joint_goals_5
147 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
148 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 2.0, True)
149 
150 # Move arm and hand
151 joint_goals = arm_joint_goals_7
152 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
153 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
154 joint_goals = hand_joint_goals_5
155 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
156 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
157 
158 # Moving to a stored named target, stored targets can be viewed in MoveIt in the planning tab
159 named_target = "la_home"
160 print("Moving arm to named target " + named_target)
161 arm_commander.move_to_named_target(named_target)
162 
163 rospy.sleep(2.0)
164 
165 # Moving to a stored named target, stored targets can be viewed in MoveIt in the planning tab
166 rospy.loginfo("Moving hand to joint state: open")
167 hand_commander.move_to_named_target("open")

sr_example
Author(s): Ugo Cupcic
autogenerated on Mon Feb 28 2022 23:52:10