Introduction
This example demonstrates the usage of the Joint Trajectory Action server that performs a smooth trajectory motion, by sending manipulated joint positions to it. A set of Joint positions are hard-coded in the main() function and these are passed to the start() method. It is here that the joint positions are sent to the joint trajectory action server.
Action Servers - robot/limb/left/follow_joint_trajectory, robot/limb/right/follow_joint_trajectory.
Code Walkthrough
Now, let's break down the code.
33 import argparse
34 import sys
35
36 from copy import copy
37
38 import rospy
39
40 import actionlib
41
42 from control_msgs.msg import (
43 FollowJointTrajectoryAction,
44 FollowJointTrajectoryGoal,
45 )
46 from trajectory_msgs.msg import (
47 JointTrajectoryPoint,
48 )
49
50 import baxter_interface
51
52 from baxter_interface import CHECK_VERSION
This imports the baxter interface for accessing the limb and the gripper class. The actionlib is imported to use its Action Server class.The CHECK_VERSION is imported to check if the software running on the robot would be compatible with this local version. It is not necessary to check the version in custom programs.
55 class Trajectory(object):
56 def __init__(self, limb):
57 ns = 'robot/limb/' + limb + '/'
58 self._client = actionlib.SimpleActionClient(
59 ns + "follow_joint_trajectory",
60 FollowJointTrajectoryAction,
61 )
62 self._goal = FollowJointTrajectoryGoal()
63 server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
64 if not server_up:
65 rospy.logerr("Timed out waiting for Joint Trajectory"
66 " Action Server to connect. Start the action server"
67 " before running example.")
68 rospy.signal_shutdown("Timed out waiting for Action Server")
69 sys.exit(1)
70 self.clear(limb)
Action server client for the corresponding limb is created. The existence of action server for the limb is checked with a timeout of 10 seconds. If they are not available, a shutdown signal is sent and the program exits.
72 def add_point(self, positions, time):
73 point = JointTrajectoryPoint()
74 point.positions = copy(positions)
75 point.time_from_start = rospy.Duration(time)
76 self._goal.trajectory.points.append(point)
This method creates a request message of the JointTrajectoryPoint type and appends the goal position based on the side of the limb/gripper that is requesting. It compiles a list of joint positions.
78 def start(self):
79 self._goal.trajectory.header.stamp = rospy.Time.now()
80 self._client.send_goal(self._goal)
The request message is sent to the joint trajectory action server to start the execution of the trajectory.
82 def stop(self):
83 self._client.cancel_goal()
The trajectory execution is stopped by sending cancel goals to the joint trajectory action server.
85 def wait(self, timeout=15.0):
86 self._client.wait_for_result(timeout=rospy.Duration(timeout))
This method waits for the completion of the trajectory execution by joint trajectory action server.
82 def result(self):
83 return self._client.get_result()
The feedback from the joint trajectory action server can be retrieved using this method. This method is not used in this example. However, the user may modify the code and use this method to retrieve feedback messages like the errors, from the action server.
91 def clear(self, limb):
92 self._goal = FollowJointTrajectoryGoal()
93 self._goal.trajectory.joint_names = [limb + '_' + joint for joint in \
94 ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
The joint names field in the _goal message is populated with the joint names in the corresponding limb.
97 def main():
98 """RSDK Joint Trajectory Example: Simple Action Client
99
100 Creates a client of the Joint Trajectory Action Server
101 to send commands of standard action type,
102 control_msgs/FollowJointTrajectoryAction.
103
104 Make sure to start the joint_trajectory_action_server.py
105 first. Then run this example on a specified limb to
106 command a short series of trajectory points for the arm
107 to follow.
108 """
109 arg_fmt = argparse.RawDescriptionHelpFormatter
110 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
111 description=main.__doc__)
112 required = parser.add_argument_group('required arguments')
113 required.add_argument(
114 '-l', '--limb', required=True, choices=['left', 'right'],
115 help='send joint trajectory to which limb'
116 )
117 args = parser.parse_args(rospy.myargv()[1:])
118 limb = args.limb
The side of the limb on which the example is to be executed is parsed from the command line arguments.
132 traj = Trajectory(limb)
133 rospy.on_shutdown(traj.stop)
134 # Command Current Joint Positions first
135 limb_interface = baxter_interface.limb.Limb(limb)
136 current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
137 traj.add_point(current_angles, 0.0)
138
139 # Command Current Joint Positions first
140 limb_interface = baxter_interface.limb.Limb(limb)
141 current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
142 traj.add_point(current_angles, 0.0)
143
144 p1 = positions[limb]
145 traj.add_point(p1, 7.0)
146 traj.add_point([x * 0.75 for x in p1], 9.0)
147 traj.add_point([x * 1.25 for x in p1], 12.0)
148 traj.start()
149 traj.wait(15.0)
150 print("Exiting - Joint Trajectory Action Test Complete")
151
152 if __name__ == "__main__":
153 main()
The joint positions to be passed to the action server are hard coded and then the start() method is called as explained above.
