Package Summary

Provides manipulator cartesian jogging.

  • Maintainer status: maintained
  • Maintainer: Blake Anderson <blakeanderson AT utexas DOT edu>
  • Author: Brian O'Neil, Blake Anderson <blakeanderson AT utexas DOT edu>, Andy Zelenak <andyz AT utexas DOT edu>
  • License: specified in-file
  • Source: git https://github.com/UTNuclearRoboticsPublic/jog_arm.git (branch: indigo)

Package Summary

Provides real-time manipulator cartesian jogging.

  • Maintainer status: developed
  • Maintainer: Blake Anderson <blakeanderson AT utexas DOT edu>, Andy Zelenak <andyz AT utexas DOT edu>
  • Author: Brian O'Neil, Andy Zelenak <andyz AT utexas DOT edu>, Blake Anderson, Nitish Sharma, Alexander Rössler <alex AT machinekoder DOT com>
  • License: BSD 3-Clause
  • Source: git https://github.com/UTNuclearRoboticsPublic/jog_arm.git (branch: kinetic)

Description

Send and adjust robot motion commands in real time. Tested on a UR5. (Video)

Usage

Clone and build this package. Make sure the joy package is installed:

sudo apt install ros-kinetic-joy

Launch your robot driver. Check that MoveIt! works by sending some commands in RViz. Then, the package includes a launch file for quick testing. Plug an XBox controller or similar into your computer and run the launch file:

(XBox controller)

roslaunch jog_arm jog_with_xbox.launch

or

(SpaceMouse)

roslaunch jog_arm jog_with_spacenav.launch

The package parameters can be modified via YAML file. See /config/jog_settings.yaml.

Features

  • Adjust the robot trajectory in real time at frequencies upward of 100 Hz.
  • The robot slows as a singularity is approached. It is possible to reverse out of singularities. The YAML file allows the singularity threshold to be adjusted.
  • Being multi-threaded, the node sustains a constant joint publish rate, despite incoming data rates that may vary.
  • Joint velocities are filtered to prevent abrupt jerks.
  • Collision detection.

YAML parameters

Loading

Store these parameters in a .yaml file. The parameters can then be loaded to the ROS parameter server from a launch file like this:

<rosparam command="load" file="$(find jog_arm)/config/jog_settings.yaml" />

Available parameters

  • gazebo: set to true if running a Gazebo simulation. It requires slightly different trajectory messages.

  • collision_check: set to true to enable collision checking.

  • command_in_type: "unitless" for inputs in the range [-1:1]. "speed_units" if inputs are in m/s and rad/s

  • cartesian_command_in_topic: topic for incoming xyz commands

  • joint_command_in_topic: topic for incoming joint angle commands

  • command_frame: name of the tf frame of incoming commands.

  • incoming_command_timeout: for safety, stop jogging when fresh commands haven't been received for this many seconds.

  • joint_topic: where does the arm driver publish the joint names/values? Typically joint_states.

  • move_group_name: name of the MoveIt group of interest (as defined in the SRDF). Must be a serial chain since the Jacobian is used.

  • lower_singularity_threshold: start decelerating when the condition number hits this (close to singularity)

  • hard_stop_singularity_threshold: stop when the arm is very close to singularity. Should be > lower_singularity_threshold parameter. Larger ==> Can be closer to a singularity before stopping.

  • lower_collision_proximity_threshold: Start decelerating when a collision is this far [m]. Warning: collision detection only works well at slower robot speeds.

  • hard_stop_collision_proximity_threshold: Stop when a collision is this far [m]

  • command_out_topic: name of the outgoing trajectory_msgs/JointTrajectory topic. Your robot driver should be subscribed to this.

  • low_pass_filter_coeff: a larger coefficient yields smoother velocity profiles, but more lag.

  • publish_period: define the rate of outgoing joint commands. Typically in the 60-125 Hz range.

  • publish_delay: a time offset that is added to every outgoing message. Needed because the start of motion must be after the current time, or the point is skipped.

  • scale/linear: incoming joystick commands are typically in the range [-1, 1]. Use this value to scale them to something reasonable for your robot. This defines the max linear velocity in meters per pub_period. max_velocity = linear_scale/pub_period, units are [m/s].

  • scale/angular: incoming joystick commands are typically in the range [-1, 1]. Use this value to scale them to something reasonable for your robot. This defines the max angular velocity in radians per pub_period. max_velocity = angular_scale/pub_period, units are [rad/s].

  • warning_topic: publish true to this topic if jogging halts due to collision/joint limit/velocity limit/singularity.

  • joint_limit_margin: added as a buffer to joint limits [radians]. If moving quickly, make this larger.

  • publish_joint_positions: can save some bandwidth as most robots only require positions or velocities

  • publish_joint_velocities: can save some bandwidth as most robots only require positions or velocities

For Multiple Arms

When jogging multiple arms, add a YAML file and a parameter namespace for each to prevent clashes:

<!-- load parameters for the right arm (note the 'ns' attribute) -->
<rosparam command="load" file="$(find pkg_name)/config/right_arm_jog_settings.yaml" ns="right_arm" />

<!-- start a jog_arm instance for the right arm -->
<node name="right_jog_arm_server" pkg="jog_arm" type="jog_arm_server" output="screen" >
  <param name="parameter_ns" type="string" value="right_arm" />
</node>

<!-- load parameters for the left arm (note the 'ns' attribute) -->
<rosparam command="load" file="$(find pkg_name)/config/left_arm_jog_settings.yaml" ns="left_arm" />

<!-- start a jog_arm instance for the left arm -->
<node name="left_jog_arm_server" pkg="jog_arm" type="jog_arm_server" output="screen" >
  <param name="parameter_ns" type="string" value="left_arm" />
</node>

Good real-time performance in ROS1

There's no guarantee in ROS1 that messages will arrive in a timely manner, or even at all. Here are some tips to get better performance:

* Use hard-wired ethernet cables, never wifi.

* Try to minimize unnecessary network traffic.

* Try to keep a low computational load on the computer running the robot driver.

* Use joint velocity commands rather than joint position commands, if possible. Universal Robots using the ur_modern_driver do this, by default.

Troubleshooting

* Motion is jerky: try increasing incoming_cmd_timeout, then try increasing low_pass_filter_coeff. Finally, try increasing both singularity_threshold and hard_stop_singularity_threshold.

* I keep jogging into singularities!: decrease singularity_threshold and hard_stop_singularity_threshold. Also, try a larger difference between the two... which will give the robot more time to decelerate as a singularity approaches.

Wiki: jog_arm (last edited 2018-09-19 17:13:45 by AndyZe)