This tutorial describes how to create a few simple motions using example scripts from the Intera Motion Interface.


Contents


Motion Interface Examples


The Motion Interface enables the user to easily generate smooth motions for Sawyer by specifying a sequence of waypoints: the motion controller on Sawyer will automatically generate and run a trajectory that passes through the waypoints. The user can specify a variety of options, such as a maximum speed or requiring that the endpoint move on a linear path between the waypoints.


Go to Joint Angles


The go_to_joint_angles.py script is perhaps the simplest way to use the motion interface: it moves the arm from its current position to some target set of joint angles. Note that this script will not plan around self-collisions or obstacles.


$ rosrun intera_examples go_to_joint_angles.py -h

 

Print the arguments that can be passed to the script.

 

$ rosrun intera_examples go_to_joint_angles.py

 

Run the script with default parameters: move at a moderate speed to a configuration where the arm is in front of the robot with the elbow up.

 

$ rosrun intera_examples go_to_joint_angles.py -q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1

 

Run the script with a few common arguments:

  • Set the target joint angles (ordered j0 through j6): -q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4
  • Set the speed ratio: -s 0.9
  • Set the acceleration ratio: -s 0.1

Note that the speed and acceleration ratios are both scalar values on the range 0 to 1. The actual limits that are used in planning are equal to the maximum that each joint can achieve, scaled by the ratio. Advanced users can set kinematic limits in a more direct way using the WaypointOptions class.

 

Go to Cartesian Pose


The go_to_cartesian_pose.py moves the robot from its current position to a target pose, with the end-effector following a linear path.

 

$ rosrun intera_examples go_to_cartesian_pose.py -h

 

Display the help file for the script, listing all possible arguments.

 

$ rosrun intera_examples go_to_cartesian_pose.py -p 0.4 -0.3 0.18 -o 0.0 1.0 0.0 0.0 -t right_hand

 

Go to position (x=0.4, y=-0.3, z=0.18 meters) with a quaternion orientation (0, 1, 0, 0) and tip name right_hand. The current position or orientation will be used if only one is provided explicitly.

 

$ rosrun intera_examples go_to_cartesian_pose.py -q 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0

 

Go to joint angles: 0.0 -0.9 0.0 1.8 0.0 -0.9 0.0 using default settings. If a Cartesian pose is not provided then forward kinematics will be used to compute it. If a Cartesian pose is provided, then the joint angles will be used to bias the nullspace (elbow orientation) during motion planning.

 

$ rosrun intera_examples go_to_cartesian_pose.py -R 0.01 0.02 0.03 0.1 0.2 0.3 -T

Jog arm with Relative Pose (in tip frame). In this case, move x=0.01, y=0.02, z=0.03 meters in translation and rotate roll=0.1, pitch=0.2, yaw=0.3 radians in orientation from the current end effector frame. The fixed position and orientation paramters will be ignored if provided.

 

Send Random Trajectory


The send_random_trajectory.py script is used to perform a random walk in joint space with the robot arm. It should be used with caution, as the robot does not know about its surroundings: make sure that there is plenty of space around the robot before running!


$ rosrun intera_examples send_random_trajectory.py -h

 

Provide the help file for the script, as well as default arguments.


$ rosrun intera_examples send_random_trajectory.py -n 5 -t JOINT -s 0.5

 

Send a random joint trajectory with 5 waypoints, using a speed ratio of 0.5 for all waypoints. Use default random walk settings.

 

$ rosrun intera_examples send_random_trajectory.py -d 0.1 -b 0.2

 

Send a random trajectory with default trajectory settings. Use a maximum step distance of 10% of the joint range and avoid the upper and lower joint limits by a boundary of 20% of the joint range.