Motion Interface Go-To-Joint Angles

Created by Rico Stodt, Modified on Fri, 11 Feb, 2022 at 10:52 AM by Rico Stodt

This example shows how to use the motion interface to move Sawyers´s arm to a specific location in joint space.

Contents
Introduction
Usage
Arguments
Code Walkthrough

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.

import rospy
import argparse
from intera_motion_interface import (
    MotionTrajectory,
    MotionWaypoint,
    MotionWaypointOptions
)
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.

arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                 description=main.__doc__)
parser.add_argument(
    "-q", "--joint_angles", type=float,
    nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
    help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
    "-s",  "--speed_ratio", type=float, default=0.5,
    help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
    "-a",  "--accel_ratio", type=float, default=0.5,
    help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
parser.add_argument(
    "--timeout", type=float, default=None,
    help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
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.

rospy.init_node('go_to_joint_angles_py')
limb = Limb()
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.

wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=args.speed_ratio,
                                         max_joint_accel=args.accel_ratio)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)

joint_angles = limb.joint_ordered_angles()

waypoint.set_joint_angles(joint_angles = joint_angles)
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.

waypoint.set_joint_angles(joint_angles = args.joint_angles)
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.

result = traj.send_trajectory(timeout=args.timeout)
if result is None:
    rospy.logerr('Trajectory FAILED to send')
    return

if result.result:
    rospy.loginfo('Motion controller successfully finished the trajectory!')
else:
    rospy.logerr('Motion controller failed to complete the trajectory with error %s',
                 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.

Motion Interface Go-To-Joint Angles

Contents

Introduction

Usage

Arguments

Code Walkthrough