sr_left_arm_examples.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 some of the functions of the arm commander.
17 # The arm is moved through a sequence of goals generated via different functions in the commander.
18 
19 import rospy
20 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
21 from sr_robot_commander.sr_arm_commander import SrArmCommander
22 
23 rospy.init_node("basic_left_arm_example", anonymous=True)
24 
25 arm_commander = SrArmCommander(name="left_arm", set_ground=False)
26 
27 rospy.sleep(rospy.Duration(2))
28 
29 # The arm commander generates a plan to a new pose before the pose is executed.
30 pose_1 = [0.5, -0.3, 1.2, 0, 1.57, 0]
31 print("Planning the move to the first pose:\n" + str(pose_1) + "\n")
32 arm_commander.plan_to_pose_target(pose_1)
33 print("Finished planning, moving the arm now.")
34 # Can only execute if a plan has been generated
35 arm_commander.execute()
36 
37 rospy.sleep(6.0)
38 
39 pose_2 = [0.5, 0.3, 1.2, 0, 1.57, 0]
40 print("Planning the move to the second pose:\n" + str(pose_2) + "\n")
41 arm_commander.plan_to_pose_target(pose_2)
42 print("Finished planning, moving the arm now.")
43 arm_commander.execute()
44 
45 # Here a pose is provided and the arm commander moves the arm to it
46 pose_1 = [0.5, -0.3, 1.2, 0, 1.57, 0]
47 print("Moving arm to pose:\n" + str(pose_1) + "\n")
48 arm_commander.move_to_pose_target(pose_1, wait=True)
49 
50 rospy.sleep(6.0)
51 
52 pose_2 = [0.5, 0.3, 1.2, 0, 1.57, 0]
53 print("Moving arm to pose:\n" + str(pose_2) + "\n")
54 arm_commander.move_to_pose_target(pose_2, wait=True)
55 
56 # The goal given here is in cartesian co-ordinates and is the goal for the end effector
57 position_1 = [0.5, -0.3, 1.2]
58 print("Moving arm to position:\n" + str(position_1) + "\n")
59 arm_commander.move_to_position_target(position_1)
60 
61 rospy.sleep(rospy.Duration(5))
62 
63 # Planning to a cartesian co-ordinate goal
64 position_2 = [0.5, 0.3, 1.2]
65 print("Planning the move to the second position:\n" + str(position_2) + "\n")
66 arm_commander.plan_to_position_target(position_2)
67 print("Finished planning, moving the arm now.")
68 arm_commander.execute()
69 
70 rospy.sleep(rospy.Duration(5))
71 
72 # Read the current joint positions
73 print("Arm joints position:\n" +
74  str(arm_commander.get_joints_position()) + "\n")
75 
76 joints_states_1 = {'la_shoulder_pan_joint': 0.43, 'la_elbow_joint': 2.12, 'la_wrist_1_joint': -1.71,
77  'la_wrist_2_joint': 1.48, 'la_shoulder_lift_joint': -2.58, 'la_wrist_3_joint': 1.62,
78  'lh_WRJ1': 0.0, 'lh_WRJ2': 0.0}
79 
80 print("Moving arm to joints position:\n" + str(joints_states_1) + "\n")
81 
82 arm_commander.move_to_joint_value_target(joints_states_1)
83 rospy.sleep(rospy.Duration(5))
84 
85 joints_states_2 = {'la_shoulder_pan_joint': 0.42, 'la_elbow_joint': 1.97, 'la_wrist_1_joint': -0.89,
86  'la_wrist_2_joint': -0.92, 'la_shoulder_lift_joint': -1.93, 'la_wrist_3_joint': 0.71,
87  'lh_WRJ1': 0.0, 'lh_WRJ2': 0.0}
88 
89 print("Moving arm to joints position:\n" + str(joints_states_2) + "\n")
90 
91 arm_commander.move_to_joint_value_target(joints_states_2)
92 rospy.sleep(rospy.Duration(5))
93 
94 print("Arm joints position:\n" + str(arm_commander.get_joints_position()) + "\n")
95 
96 joints_states_3 = {'la_shoulder_pan_joint': 1.61, 'la_elbow_joint': 1.15, 'la_wrist_1_joint': -0.24,
97  'la_wrist_2_joint': 0.49, 'la_shoulder_lift_joint': -1.58, 'la_wrist_3_joint': 2.11,
98  'lh_WRJ1': 0.0, 'lh_WRJ2': 0.0}
99 
100 # A trajectory is generated from the 3 positions for each joint, specified in joint_states_1, joint_states_2
101 # and joint_states_3
102 print("Running joints trajectory")
103 
104 joint_trajectory = JointTrajectory()
105 joint_trajectory.header.stamp = rospy.Time.now()
106 joint_trajectory.joint_names = list(joints_states_1.keys())
107 joint_trajectory.points = []
108 time_from_start = rospy.Duration(5)
109 
110 for joints_states in [joints_states_1, joints_states_2, joints_states_3]:
111  trajectory_point = JointTrajectoryPoint()
112  trajectory_point.time_from_start = time_from_start
113  time_from_start = time_from_start + rospy.Duration(5)
114 
115  trajectory_point.positions = []
116  trajectory_point.velocities = []
117  trajectory_point.accelerations = []
118  trajectory_point.effort = []
119  for key in joint_trajectory.joint_names:
120  trajectory_point.positions.append(joints_states[key])
121  trajectory_point.velocities.append(0.0)
122  trajectory_point.accelerations.append(0.0)
123  trajectory_point.effort.append(0.0)
124 
125  joint_trajectory.points.append(trajectory_point)
126 
127 arm_commander.run_joint_trajectory(joint_trajectory)
128 
129 rospy.sleep(rospy.Duration(15))
130 
131 # Moving to a stored named target, stored targets can be viewed in MoveIt in the planning tab
132 named_target = "gamma"
133 print("Moving arm to named target: " + named_target)
134 arm_commander.move_to_named_target(named_target)
135 
136 rospy.sleep(rospy.Duration(3))
137 
138 # Current positions and velocities are read
139 print("Arm joints position:\n" +
140  str(arm_commander.get_joints_position()) + "\n")
141 print("Arm joints velocities:\n" +
142  str(arm_commander.get_joints_velocity()) + "\n")


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