This example shows how to use the motion interface to move Sawyer's arm to a specific location in joint space.
Introduction
NEW functionality in the Intera 5.2 Update!
This example demonstrates how to create and send a simple trajectory to the motion interface, for a desired set of joint angles.
If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link.
Usage
Running the script with no arguments will move the arm at a moderate speed to a default pose that is directly in front of the robot with the elbow up. Additional arguments can be specified to control the target location and motion parameters.
$ rosrun intera_examples go_to_joint_angles.py
Arguments
Running the script with the -h argument will display the available arguments on the command line.
$ rosrun intera_examples go_to_joint_angles.py -h
The result of this command is included below. The arguments let you control the target joint angles (-q, --joint_angles) as well as the peak speed (-s, --speed_ratio) and acceleration (-a, --accel_ratio) along the trajectory. Both the speed and acceleration are presented here in normalized units, where 1.0 corresponds to maximum speed (or acceleration) for each joint.
usage: go_to_joint_angles.py [-h] [-q JOINT_ANGLES [JOINT_ANGLES ...]]
[-s SPEED_RATIO] [-a ACCEL_RATIO]
[--timeout TIMEOUT]
Move the robot arm to the specified configuration.
Call using:
$ rosrun intera_examples go_to_joint_angles.py [arguments: see below]
-q 0.0 0.0 0.0 0.0 0.0 0.0 0.0
--> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings
-q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1
--> Go to pose [...] with a speed ratio of 0.1
-q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1
--> Go to pose [...] with a speed ratio of 0.9 and an accel ratio of 0.1
optional arguments:
-h, --help show this help message and exit
-q JOINT_ANGLES [JOINT_ANGLES ...], --joint_angles JOINT_ANGLES [JOINT_ANGLES ...]
A list of joint angles, one for each of the 7 joints,
J0...J6
-s SPEED_RATIO, --speed_ratio SPEED_RATIO
A value between 0.001 (slow) and 1.0 (maximum joint
velocity)
-a ACCEL_RATIO, --accel_ratio ACCEL_RATIO
A value between 0.001 (slow) and 1.0 (maximum joint
accel)
--timeout TIMEOUT Max time in seconds to complete motion goal before
returning. None is interpreted as an infinite timeout.
Code Walkthrough
Now, let's break down the code.
33 import rospy
34 import argparse
35 from intera_motion_interface import (
36 MotionTrajectory,
37 MotionWaypoint,
38 MotionWaypointOptions
39 )
40 from intera_interface import Limb
We start by importing all of the packages and classes that we will need to create and send a motion trajectory request. Note the new intera_motion_interface package.
43 arg_fmt = argparse.RawDescriptionHelpFormatter
44 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
45 description=main.__doc__)
46 parser.add_argument(
47 "-q", "--joint_angles", type=float,
48 nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
49 help="A list of joint angles, one for each of the 7 joints, J0...J6")
50 parser.add_argument(
51 "-s", "--speed_ratio", type=float, default=0.5,
52 help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
53 parser.add_argument(
54 "-a", "--accel_ratio", type=float, default=0.5,
55 help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
56 parser.add_argument(
57 "--timeout", type=float, default=None,
58 help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
59 args = parser.parse_args(rospy.myargv()[1:])
This block of code uses the arg_parse package to parse the command-line arguments and convert them into python variables. It also does the work of setting a default value for each argument.
69 rospy.init_node('go_to_joint_angles_py')
70 limb = Limb()
71 traj = MotionTrajectory(limb = limb)
Next we will initialize the ROS node and then create an empty MotionTrajectory object. Much of the remaining code will be dedicated to adding information to the motion trajectory to describe the desired motion.
69 wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=args.speed_ratio,
70 max_joint_accel=args.accel_ratio)
71 waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
72
73 joint_angles = limb.joint_ordered_angles()
74
75 waypoint.set_joint_angles(joint_angles = joint_angles)
76 traj.append_waypoint(waypoint.to_msg())
Here we set up the default waypoint options and then set up a motion waypoint that describes the current position of the arm. Note that we convert the waypoint to a message before sending it to the motion trajectory. This is not required, but it illustrates an important point: the MotionWaypoint class is not the same as a 'Waypoint.msg'. The class can be used to create the waypoint message along with other helper functions. The MotionTrajectory will copy the data from the waypoint, so we can update the waypoint and use it to set the next position.
81 waypoint.set_joint_angles(joint_angles = args.joint_angles)
82 traj.append_waypoint(waypoint.to_msg())
The first line of code here sets the joint angles in the waypoint, and the second line adds the updated waypoint data as a new waypoint for the trajectory. Now the motion trajectory has two waypoints: one at the current position and one at the target position. (Strictly speaking, you not need to include the current arm position as it will be appended to the trajectory by the Motion Controller.) In this case we used the default waypoint options for both waypoints, but in general you can have different options for each waypoint. Note that the waypoint options are applied to the trajectory segment prior to that waypoint.
155 result = traj.send_trajectory(timeout=args.timeout)
156 if result is None:
157 rospy.logerr('Trajectory FAILED to send')
158 return
159
160 if result.result:
161 rospy.loginfo('Motion controller successfully finished the trajectory!')
162 else:
163 rospy.logerr('Motion controller failed to complete the trajectory with error %s',
164 result.errorId)
This final block of code sends the trajectory to the motion controller and then waits for the results. The MotionTrajectory is not a Trajectory.msg: like the MotionWaypoint, it is a utility that enables the creation (and sending!) of trajectory messages.
