sr_basic_right_hand_arm_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.
17 # At the start and end of the sequence, both the hand and arm will spend 20s in teach mode,
18 # This allows the user to manually move the hand and arm. New goals can be easily generated
19 # using the script 'sr_print_joints_position.py
20 # PLEASE NOTE: move_to_joint_value_target_unsafe is used in this script, so collision checks
21 # between the hand and arm are not made!
22 
23 import rospy
24 from sr_robot_commander.sr_arm_commander import SrArmCommander
25 from sr_robot_commander.sr_hand_commander import SrHandCommander
26 
27 
28 rospy.init_node("basic_hand_arm_example", anonymous=True)
29 
30 hand_commander = SrHandCommander()
31 arm_commander = SrArmCommander()
32 
33 # sleep for some time during which the arm can be moved around by pushing it
34 # but be careful to get away before the time runs out. You are warned
35 rospy.loginfo("Set arm teach mode ON")
36 arm_commander.set_teach_mode(True)
37 rospy.sleep(20.0)
38 
39 rospy.loginfo("Set arm teach mode OFF")
40 arm_commander.set_teach_mode(False)
41 
42 # Specify goals for hand and arm
43 hand_joints_goal_1 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0,
44  'rh_MFJ1': 0.35, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0,
45  'rh_RFJ1': 0.35, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
46  'rh_LFJ1': 0.35, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
47  'rh_THJ1': 0.35, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0, 'rh_THJ4': 0.0, 'rh_THJ5': 0.0}
48 
49 hand_joints_goal_2 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 1.5707, 'rh_FFJ3': 1.5707, 'rh_FFJ4': 0.0,
50  'rh_MFJ1': 0.35, 'rh_MFJ2': 1.5707, 'rh_MFJ3': 1.5707, 'rh_MFJ4': 0.0,
51  'rh_RFJ1': 0.35, 'rh_RFJ2': 1.5707, 'rh_RFJ3': 1.5707, 'rh_RFJ4': 0.0,
52  'rh_LFJ1': 0.35, 'rh_LFJ2': 1.5707, 'rh_LFJ3': 1.5707, 'rh_LFJ4': 0.0,
53  'rh_THJ1': 0.35, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0, 'rh_THJ4': 0.0, 'rh_THJ5': 0.0}
54 
55 hand_joints_goal_3 = {'rh_FFJ1': 0.35, 'rh_FFJ2': 0.0, 'rh_MFJ3': 0.0}
56 
57 arm_joints_goal_1 = {'ra_shoulder_pan_joint': 0.43, 'ra_elbow_joint': 2.12,
58  'ra_wrist_1_joint': -1.71, 'ra_wrist_2_joint': 1.48,
59  'ra_shoulder_lift_joint': -2.58, 'ra_wrist_3_joint': 1.62,
60  'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
61 
62 arm_joints_goal_2 = {'ra_shoulder_pan_joint': 0.42, 'ra_elbow_joint': 1.97,
63  'ra_wrist_1_joint': -0.89, 'ra_wrist_2_joint': -0.92,
64  'ra_shoulder_lift_joint': -1.93, 'ra_wrist_3_joint': 0.71,
65  'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
66 
67 # Move through each goal
68 # joint states are sent to the commanders, with a time for execution and a flag as to whether
69 # or not the commander should wait for the command to complete before moving to the next command.
70 
71 # Move hand
72 joint_goals = hand_joints_goal_1
73 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
74 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 1.0, False)
75 
76 # Move arm
77 joint_goals = arm_joints_goal_1
78 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
79 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
80 
81 
82 # Move hand
83 joint_goals = hand_joints_goal_2
84 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
85 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
86 
87 
88 # Move hand
89 joint_goals = hand_joints_goal_3
90 rospy.loginfo("Moving hand to joint states\n" + str(joint_goals) + "\n")
91 hand_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, False)
92 
93 # Move arm
94 joint_goals = arm_joints_goal_2
95 rospy.loginfo("Moving arm to joint states\n" + str(joint_goals) + "\n")
96 arm_commander.move_to_joint_value_target_unsafe(joint_goals, 3.0, True)
97 
98 
99 hand_commander.set_teach_mode(True)
100 # sleep for some time during which the hand joints can be moved manually
101 rospy.sleep(20.0)
102 rospy.loginfo("Set hand teach mode OFF")
103 hand_commander.set_teach_mode(False)


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