Joint Trajectory Client - Code Walkthrough

From sdk-wiki
Jump to: navigation, search

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.

  1. import argparse
  2. import sys
  3.  
  4. from copy import copy
  5.  
  6. import rospy
  7.  
  8. import actionlib
  9.  
  10. from control_msgs.msg import (
  11.     FollowJointTrajectoryAction,
  12.     FollowJointTrajectoryGoal,
  13. )
  14. from trajectory_msgs.msg import (
  15.     JointTrajectoryPoint,
  16. )
  17.  
  18. import baxter_interface
  19.  
  20. 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.

  1. class Trajectory(object):
  2.     def __init__(self, limb):
  3.         ns = 'robot/limb/' + limb + '/'
  4.         self._client = actionlib.SimpleActionClient(
  5.             ns + "follow_joint_trajectory",
  6.             FollowJointTrajectoryAction,
  7.         )
  8.         self._goal = FollowJointTrajectoryGoal()
  9.         server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
  10.         if not server_up:
  11.             rospy.logerr("Timed out waiting for Joint Trajectory"
  12.                          " Action Server to connect. Start the action server"
  13.                          " before running example.")
  14.             rospy.signal_shutdown("Timed out waiting for Action Server")
  15.             sys.exit(1)
  16.         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.

  1.     def add_point(self, positions, time):
  2.         point = JointTrajectoryPoint()
  3.         point.positions = copy(positions)
  4.         point.time_from_start = rospy.Duration(time)
  5.         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.

  1.     def start(self):
  2.         self._goal.trajectory.header.stamp = rospy.Time.now()
  3.         self._client.send_goal(self._goal)

The request message is sent to the joint trajectory action server to start the execution of the trajectory.

  1.     def stop(self):
  2.         self._client.cancel_goal()

The trajectory execution is stopped by sending cancel goals to the joint trajectory action server.

  1.     def wait(self, timeout=15.0):
  2.         self._client.wait_for_result(timeout=rospy.Duration(timeout))

This method waits for the completion of the trajectory execution by joint trajectory action server.

  1.     def result(self):
  2.         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.

  1.     def clear(self, limb):
  2.         self._goal = FollowJointTrajectoryGoal()
  3.         self._goal.trajectory.joint_names = [limb + '_' + joint for joint in \
  4.             ['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.

  1. def main():
  2.     """RSDK Joint Trajectory Example: Simple Action Client
  3.  
  4.    Creates a client of the Joint Trajectory Action Server
  5.    to send commands of standard action type,
  6.    control_msgs/FollowJointTrajectoryAction.
  7.  
  8.    Make sure to start the joint_trajectory_action_server.py
  9.    first. Then run this example on a specified limb to
  10.    command a short series of trajectory points for the arm
  11.    to follow.
  12.    """
  13.     arg_fmt = argparse.RawDescriptionHelpFormatter
  14.     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
  15.                                      description=main.__doc__)
  16.     required = parser.add_argument_group('required arguments')
  17.     required.add_argument(
  18.         '-l', '--limb', required=True, choices=['left', 'right'],
  19.         help='send joint trajectory to which limb'
  20.     )
  21.     args = parser.parse_args(rospy.myargv()[1:])
  22.     limb = args.limb

The side of the limb on which the example is to be executed is parsed from the command line arguments.

  1.     traj = Trajectory(limb)
  2.     rospy.on_shutdown(traj.stop)
  3.     # Command Current Joint Positions first
  4.     limb_interface = baxter_interface.limb.Limb(limb)
  5.     current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
  6.     traj.add_point(current_angles, 0.0)
  7.  
  8.     # Command Current Joint Positions first
  9.     limb_interface = baxter_interface.limb.Limb(limb)
  10.     current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
  11.     traj.add_point(current_angles, 0.0)
  12.  
  13.     p1 = positions[limb]
  14.     traj.add_point(p1, 7.0)
  15.     traj.add_point([x * 0.75 for x in p1], 9.0)
  16.     traj.add_point([x * 1.25 for x in p1], 12.0)
  17.     traj.start()
  18.     traj.wait(15.0)
  19.     print("Exiting - Joint Trajectory Action Test Complete")
  20.  
  21. if __name__ == "__main__":
  22.     main()

The joint positions to be passed to the action server are hard coded and then the start() method is called as explained above.