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 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 <blakeanderson AT utexas DOT edu>, Nitish Sharma
  • License: specified in-file
  • 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.

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:

roslaunch jog_arm test_with_joystick.launch

If you don't have an XBox controller, the Python file /src/joy_to_twist.py can easily be modified for keyboard commands, etc.

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

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

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

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

* coll_check: set to true to enable collision checking.

* cmd_in_topic: topic for incoming commands, e.g. from a joystick.

* cmd_frame: name of the tf frame of incoming commands.

* incoming_cmd_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. Must be a serial chain since the Jacobian is used.

* singularity_threshold: how sensitive is singularity detection? Larger ==> Can be closer to a singularity before slowing.

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

* cmd_out_topic: name of the outgoing trajectory_msgs::JointTrajectory topic. Your robot driver should be subscribed to this.

* planning_frame: typically should match the MoveIt! planning frame.

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

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

* 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. Units are [m/s]. max_velocity = angular_scale/pub_period

* 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. Units are [rad/s]. max_velocity = angular_scale/pub_period

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

For Multiple Arms

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

  • <rosparam command="load" file="$(find pkg_name)/config/right_arm_jog_settings.yaml" ns="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>

    <rosparam command="load" file="$(find pkg_name)/config/left_arm_jog_settings.yaml" ns="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>

Wiki: jog_arm (last edited 2018-04-20 20:57:08 by AndyZe)