Only released in EOL distros:
Package Summary
The ROS JACO Arm stack provides a ROS interface for the Kinova Robotics JACO robotic manipulator arm. This stack provides access to the Kinova JACO C++ hardware API through ROS.
- Author: Jeff Schmidt (Clearpath), Alex Bencz (Clearpath), Matt DeDonato (WPI)
- License: BSD
- Source: git https://github.com/Kinovarobotics/jaco-ros.git (branch: master)
The ROS JACO Arm module provides a ROS interface for the Kinova Robotics JACO robotic manipulator arm. This module exposes the Kinova JACO C++ hardware API through ROS.
Overview
This page is for posterity on Jaco fuerte and groovy generation code. For the latest version, please see jaco_ros
The JACO API is exposed to ROS using a combination of actionlib (for sending trajectory commands to the arm), services (for instant control such as homing the arm or e-stop) and published topics (joint feedback). The Arm may be commanded using either angular commands or Cartesian co-ordinates.
In addition, a transform publisher enables visualization of the arm via rviz.
There are three actionlib modules available: arm_pose, arm_joint_angles, and set_finger_position. These server modules accept coordinates which are passed on to the Kinova JACO API for controlling the arm.
The services available are: home_arm, stop, and start. These services require no input goals, and are intended for quick control of basic arm functions. When called, home_arm will halt all other movement and return the arm to its “home” position. The stop service is a software e-stop, which instantly stops the arm and prevents any further movement until the start service is called.
Published topics are: cartesian_velocity, joint_velocity, finger_position, joint_angles, joint_state, and tool_position. The cartesian_velocity and joint_velocity are both subscribers which may be used to set the joint velocity limits of the arm. The finger_position and joint_angles topics publish the raw angular position of the fingers and joints, respectively, in degrees. The joint_state topic publishes via sensor_msgs the transformed joint angles in radians. The tool_position topic publishes the Cartesian co-ordinates of the arm and end effector via geometry_msgs.
ROS-JACO Module Architecture
The jaco_arm_driver node acts as an interface between the Kinova JACO C++ API and the various actionservers, message services and topics used to interface with the arm.
The jaco_tf_updater node subscribes to the jaco_arm_driver node to obtain current joint angle information from the node. It then publishes a transform which may be used in visualization programs such as rviz.
Cartesian Control
Cartesian control is accomplished via actionserver. The Cartesian co-ordinates are published as a separate topic. The arm has a maximum reach of about 0.9m (90cm), so the "position" range is about +/-0.9 for all three dimensions. The three wrist joints are capable of continuous rotation, and therefore capable of being commanded up to +/-174.5 rad. You may want to limit to +/-6.28 rad in software to prevent the joints from rotating excessively.
Actionserver Topic
/jaco/arm_pose |
Parameters
float64 position.x – end effector distance from the base in meters (left-right relative to base) |
float64 position.y – end effector distance from the base in meters (forward-back relative to base) |
float64 position.z – end effector distance from the base in meters (up-down relative to base) |
float64 orientation.x – end effector quaternion orientation in radians |
float64 orientation.y – end effector quaternion orientation in radians |
float64 orientation.z – end effector quaternion orientation in radians |
float64 orientation.w – end effector quaternion orientation in radians |
Published Topic
/jaco/tool_position |
Parameters
float64 position.x – end effector distance from the base in meters (left-right relative to base) |
float64 position.y – end effector distance from the base in meters (forward-back relative to base) |
float64 position.z – end effector distance from the base in meters (up-down relative to base) |
float64 orientation.x – end effector quaternion orientation in radians |
float64 orientation.y – end effector quaternion orientation in radians |
float64 orientation.z – end effector quaternion orientation in radians |
float64 orientation.w – end effector quaternion orientation in radians |
Angular Control
Angular control is accomplished via actionserver. The joint angles are published as two separate topics.
All the joints have a range limit. Joints 1, 4, 5 and 6 have a range of -10,000 to +10,000 degrees. Joint 2 has a range of +42 to +318 degrees. Joint 3 has a range of +17 to +343 degrees. Sending a command past these limits will cause the arm to move to its hard-wired limit, then stop.
Actionserver Topic
/jaco/arm_joint_angles |
Parameters
float32 Angle_J1 – “base” joint angle in radians |
float32 Angle_J2 – “shoulder” joint angle in radians |
float32 Angle_J3 – “elbow” joint angle in radians |
float32 Angle_J4 – first “wrist” joint angle in radians |
float32 Angle_J5 – second “wrist” joint angle in radians |
float32 Angle_J6 – “hand” joint angle in radians |
Published Topics
/jaco/joint_angles |
/jaco/joint_state |
joint_angles Parameters
float32 Angle_J1 – “base” joint angle in degrees |
float32 Angle_J2 – “shoulder” joint angle in degrees |
float32 Angle_J3 – “elbow” joint angle in degrees |
float32 Angle_J4 – first “wrist” joint angle in degrees |
float32 Angle_J5 – second “wrist” joint angle in degrees |
float32 Angle_J6 – “hand” joint angle in degrees |
joint_state Parameters
string name – array containing the names of the joints |
float64[] position – array containing joint positions, in transformed radians, of the joints |
float64[] velocity – array containing the joint velocities (placeholder, contains no data) |
float64[] effort – array containing the joint forces in newtons (placeholder, contains no data) |
Finger Control
Finger control is accomplished via actionserver. The finger angles are published as a separate topic. The range of input for all three fingers is 0 (fully open) to 60 (fully closed).
Subscribed Topic
/jaco/set_finger_position |
Parameters
float32 Finger_1 – position of finger 1 in degrees |
float32 Finger_2 – position of finger 2 in degrees |
float32 Finger_3 – position of finger 3 in degrees |
Published Topic
/jaco/finger_position |
Parameters
float32 Finger_1 – position of finger 1 in degrees |
float32 Finger_2 – position of finger 2 in degrees |
float32 Finger_3 – position of finger 3 in degrees |
Services
These services may be called at any time to enact basic functions on the arm. They will override any other actions being carried out by the arm.
Homing the Arm
When called, this service will return the arm to its pre-programmed “home” position. It is the equivalent of holding down the “home” button on the pendant controller. The service requires no input parameters, and simply reports when the arm has returned home.
Service Topic
/jaco/home_arm |
Parameters
string homearm_result – a string containing the results of the home_arm service |
Emergency Stop
When called, this service will immediately stop the arm if it is moving, erase any trajectories still residing in the JACO arm’s FIFO, and enable a software e-stop flag. This flag will prevent any further movement of the arm, including homing. Joint angle feedback will continue to function. The service requires no input parameters.
Service Topic
/jaco/stop |
Parameters
string stop_result – a string containing the results of the stop service |
Start
When called, this service will disable the software e-stop flag, and restore control of the arm. The service requires no input parameters.
Service Topic
/jaco/start |
Parameters
string start_result – a string containing the results of the start service |
Joint Velocity
To be completed.
Launch file
The jaco_arm.launch should be run prior to using the JACO arm. It launches two nodes, jaco_arm_driver and jaco_tf_updater. These nodes then perform a number of operations that prepare the arm for use.
On launch, the jaco_arm_driver announces all of the configurations stored in the JACO arm’s permanent memory. These are settings that, currently, are most easily set using the Windows-only Kinova GUI. The arm is also automatically homed, and the fingers initialized.
The jaco_tf_updater begins publishing transform data as soon as it becomes available from the jaco_arm_driver node.
Installation
To make ros-jaco-arm part of your workspace, follow these steps:
roscd |
rosws set ros-jaco-arm --git https://github.com/Kinovarobotics/jaco-ros.git |
rosws update ros-jaco-arm |
cd ros-jaco-arm |
rosmake |
If you would like the jaco_arm_driver and jaco_tf_updater nodes to launch automatically when ROS is started, copy the jaco_arm.launch file contained in the /launch folder into the relevant /core.d folder.
Execution
This package has been tested in Ubuntu 12.04 LTS with ROS Fuerte.
Basics
To “home” the arm
rosservice call jaco/home_arm |
To activate the e-stop (emergency stop) function
rosservice call jaco/stop |
To restore control of the arm
rosservice call jaco/start |
Joint Sensors
To obtain the raw joint angles in degrees
rostopic echo jaco/joint_angles |
To obtain the “transformed” joint angles in radians in a standard sensor_msgs format
rostopic echo jaco/joint_state |
To obtain the finger angles in degrees
rostopic echo jaco/finger_position |
To obtain the arm’s position in Cartesian units in a standard geometry_msgs format
rostopic echo jaco/tool_position |
Arm Control
Several sample python script action clients are available for manually controlling the arm. These scripts are located in the /test directory.
To set the joint angles using DH “transformed” angles in radians, use angle_action_client.py. If fewer than the required six joint angles are input, the arm will default to a "test" position.
./angle_action_client.py <Angle_J1> <Angle_J2> <Angle_J3> <Angle_J4> <Angle_J5> <Angle_J6> |
./angle_action_client.py 1.529 -1.380 -0.144 0.155 0.696 3.310 |
To set the arm position using Cartesian co-ordinates
./pose_action_client.py <pos x> <pos y> <pos z> <ori x> <ori y> <ori z> <ori w> |
./pose_action_client.py -0.314 -0.339 0.600 -0.591 -0.519 0.324 0.525 |
To set the finger positions using angles in degrees
./finger_action_client.py <finger 1> <finger 2> <finger 3> |
./finger_action_client.py 40 40 40 |
Known Limitations
The current Jaco C++ API supports only a single arm, and thus the ROS node supports a single arm as well.
The joint_state topic currently reports only the arm position. Velocity and Effort are placeholders for future compatibility.
Some virtualization software products are known to work well with this package, while others do not. The issue appears to be related to proper handover of access to the USB port to the API. Parallels and VMWare are able to do this properly, while VirtualBox causes the API to fail with a "1015" error.
Additional Resources
The transformation equations used to convert from the “DH Parameters” to physical angles are listed in the jaco_kinematics.pdf document, included as part of this package.
The Kinova JACO website: http://kinovarobotics.com/products/jaco-research-edition/
Report a Bug
Any bugs, issues or suggestions may be sent to skynet@clearpathrobotics.com
<<TracLink(REPO COMPONENT)>>