Only released in EOL distros:  

schunk_robots: schunk_lwa4d | schunk_lwa4p | schunk_lwa4p_extended | schunk_pg70 | schunk_pw70

schunk_grippers: schunk_ezn64 | schunk_pg70

Package Summary

Xacro model and RS232 control node for basic communication with Schunk PG70 gripper

schunk_grippers: schunk_ezn64 | schunk_pg70

Package Summary

Xacro model and RS232 control node for basic communication with Schunk PG70 gripper

Overview

This package is part of schunk_grippers stack. It allows user to control Schunk PG70 gripper over RS232 link using standard ROS services and provides Xacro model for easier integration with various robots.

Installation

This package depends on ROS serial library by William Woodall, git clone and catkin_make if not already in your workspace. Then you can proceed to clone and build of schunk_pg70source.

Permissions

The right to access a serial port is determined by the permissions of the device file (e.g. /dev/ttyS0). Serial ports are usualy owned by root and group dialout, so to access the serial device as non-root user it is convenient to add yourself to dialout group:

$ usermod -a -G dialout USER_NAME

Reboot to apply group changes.

ROS API

pg70_rs232_control

Node for interfacing PG70 by serial library. Provides ROS services for gripper control and publishes actual gripper position to joint_states

Published Topics

joint_states (sensor_msgs/JointState)
  • publishes actual gripper position

Services

schunk_pg70/reference (std_msgs/Empty)
  • Referencing the gripper
schunk_pg70/get_error (std_msgs/Empty)
  • Getting actual gripper error
schunk_pg70/acknowledge_error (std_msgs/Empty)
  • Acknowledging occured error/errors
schunk_pg70/get_position (std_msgs/Empty)
  • Getting actual gripper position
schunk_pg70/set_position (std_msgs/Float32)
  • Setting actual gripper position
schunk_pg70/stop (std_msgs/Empty)
  • Stopping gripper movement

Parameters

gripper_id (std_msgs/UInt8)
  • Gripper ID according to controller setup
portname (std_msgs/String)
  • serial port communicating with gripper
baudrate (std_msgs/Int16)
  • baudrate of serial port

Required tf Transforms

robot_end_linkschunk_pg70_base_link
  • Connecting gripper to robot

Provided tf Transforms

schunk_pg70_base_linkschunk_pg70_finger_1_link
  • Finger 1 link
schunk_pg70_base_linkschunk_pg70_finger_2_link
  • Finger 2 link

Usage

Note: Before interfacing Schunk PG70 gripper from Linux, we highly recomend to use original Windows application Schunk MTS Tool to verify that gripper is fully functional and there are no wiring or other hardware issues. Depending on default configuration it might be also necessary to perform memory reset to allow serial communication with the gripper for a first time.

Launch

To launch the driver node, use the following command:

$ roslaunch schunk_pg70 pg70_rs232_control.launch

if everything is okay, after typing

$ rostopic echo /joint_states

you should see actual gripper position being published on "joint_states" topic approximately 10 times per second:

header:
  seq: 21
  stamp:
    secs: 1443075477
    nsecs: 409227511
  frame_id: ''
name: ['pg70_finger1_joint', 'pg70_finger2_joint']
position: [0.010000248908996583, 0.010000248908996583]
velocity: []
effort: []
---
header:
  seq: 22
  stamp:
    secs: 1443075477
    nsecs: 511969796
  frame_id: ''
name: ['pg70_finger1_joint', 'pg70_finger2_joint']
position: [0.010000248908996583, 0.010000248908996583]
velocity: []
effort: []

Test the driver

To get the list of available services type:

$ rosservice list

If everything is okay, you should notice 6 services being advertised by "pg70_rs232_control" node:

schunk_pg70/reference
schunk_pg70/get_error
schunk_pg70/acknowledge_error
schunk_pg70/get_position
schunk_pg70/set_position
schunk_pg70/stop

These services are ROS user interface to Schunk PG70 gripper. Except "set_position" all of them use std_msgs::Empty requests, so you don't need to specify any values, just "rosservice call" as in the example below.

get_error service

To test the driver start by calling the get_error service (std_msgs::Empty request):

$ rosservice call /schunk_pg70/get_error

If no error occured - the service should respond:

error_code: 0

get_position

To get actual gripper position, call get_position service (std_msgs::Empty request):

$ rosservice call /schunk_pg70/get_position

Service respond with actual position value is in std_msgs::Float32 format:

actual_position: 20.000497818

set_position

To move the gripper to target position, call set_position service (std_msgs::float32 request):

$ rosservice call /schunk_pg70/set_position position velocity acceleration

where "position" is within a range of <0 - 69> mm, velocity <0-83>mm/s and acceleration <0-320>mm/s2. The gripper should respond with goal_position being accepted or not and move to goal position:

goal position accepted: True

reference

To reference gripper, call reference service (empty request). Referencing may also help in cases when you are not able to move the gripper although everything else looks okay:

$ rosservice call schunk_pg70/reference

Xacro model

To visualize Schunk PG70 gripper in Rviz use "pg70_visualize_standalone.launch" file.

$ roslaunch schunk_pg70 pg70_visualize_standalone.launch

Since fingers are mechanically interlocked, there is only one joint to control the movement both of them. Use joint_state_publisher GUI to test the movement:

pg70_standalone.jpg

Wiki: schunk_pg70 (last edited 2015-09-24 06:28:47 by FrantisekDurovsky)