microscribe.py

Go to the documentation of this file.

#! /usr/bin/python
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

# Author: Stuart Glaser

import usb
import time
from math import pi

import roslib
roslib.load_manifest('microscribe')
import rospy
from sensor_msgs.msg import *
from joy.msg import *

import atexit
atexit.register(rospy.signal_shutdown, 'exit')

rospy.init_node('microscribe', disable_signals=True)

def dump_list():
    for bus in usb.busses():
        print "%s" % bus
        for dev in bus.devices:
            print "  ", dev.iManufacturer, dev.iProduct, dev.idProduct, dev.idVendor

def get_microscribes():
    ms = []
    for bus in usb.busses():
        for dev in bus.devices:
            if dev.idVendor == 0x0563 and dev.idProduct == 0x0050:
                ms.append(dev)
    return ms

def get_microscribe(serial_number = None):
    ms = get_microscribes()
    if serial_number is None:
        return ms[0]
    for dev in ms:
        h = dev.open()
        try:
            if h.getString(4, 100) == serial_number:
                return dev
            if dev.iSerialNumber == serial_number:
                return dev
        except usb.USBError:
            print "Difficulty opening %s" % dev

#dump_list()

serial_number = None
try:
    serial_number = rospy.get_param(rospy.get_name() + '/serial_number')
except:
    pass
ms = get_microscribe(serial_number)
#print "Got microscribe:", ms.iSerialNumber

h = ms.open()
try:
    h.detachKernelDriver(0)
except:
    print "Didn't detach kernel driver"
h.claimInterface(0)
print "Opened microscribe:"
for i in range(1,6):
    print "    %s" % h.getString(i, 40)

def p_datum(datum):
    for i, d in enumerate(datum):
        if i in [2, 14, 16, 18]:
            print "   ",
        #elif i in [2, 4, 16, 18]:
        elif i in [4, 6, 8, 10, 12]:
            print "",
        print "%02x" % d,

    # Buttons
    buttons = datum[1]
    buttons_str = ""
    if buttons & 1:
        buttons_str += 'G'
    else:
        buttons_str += ' '
    if buttons & 2:
        buttons_str += 'W'
    else:
        buttons_str += ' '
    print "  [%s]" % buttons_str,

    # Time
    time = datum[16] + datum[17] * 0x100
    print " %5i" % time,

    # Joints
    joint_d = datum[2:14]
    enc = []
    for i in range(6):
        enc.append(joint_d[2*i] + joint_d[2*i+1] * 0x100)
    print " ", enc,
    
    print
    
    
def lis():
    while True:
        try:
            datum = h.bulkRead(0x81, 100)
            p_datum(datum)
        except usb.USBError:
            time.sleep(0.1)

TICKS = [16000, 16000, 8000, 4000, 4000, 2000]
REF = [2.756, pi, 8.0, 0, 6.4, 0]


pub_joints = rospy.Publisher(rospy.get_name() + '/joint_states', JointState)
pub_buttons = rospy.Publisher(rospy.get_name() + '/buttons', Joy)

def pub_loop():
    while True:
        try:
            datum = h.bulkRead(0x81, 100)

            enc = []
            for i in range(6):
                enc.append(datum[2+2*i] + datum[2+2*i+1] * 0x100)
            if enc[0] > 50000:
                enc[0] = enc[0] - 0x10000

            q = [2*pi * float(e) / float(T) - R for e, T, R in zip(enc, TICKS, REF)]

            #print "ENC:", ["%i" % ee for ee in enc]
            #print "Q:", ["%6.3f" % qq for qq in q]

            msg = JointState()
            msg.header.stamp = rospy.get_rostime()
            msg.name = ['base_joint', 'shoulder_joint', 'elbow_joint',
                        'wrist_roll_joint', 'wrist_flex_joint', 'stylus_joint']
            msg.position = q
            msg.velocity = [0] * 6
            msg.effort = [0] * 6
            pub_joints.publish(msg)

            but = Joy()
            but.buttons = [0, 0]
            if datum[1] & 1:
                but.buttons[0] = 1
            if datum[1] & 2:
                but.buttons[1] = 1
            pub_buttons.publish(but)
        except usb.USBError:
            time.sleep(0.1)

pub_loop()
'''
handle.bulkRead(0x81, 30, 100)




Bus 001 Device 012: ID 0563:0050 Immersion Corp. 
Device Descriptor:
  bLength                18
  bDescriptorType         1
  bcdUSB               2.00
  bDeviceClass            0 (Defined at Interface level)
  bDeviceSubClass         0 
  bDeviceProtocol         0 
  bMaxPacketSize0         8
  idVendor           0x0563 Immersion Corp.
  idProduct          0x0050 
  bcdDevice            3.17
  iManufacturer           1 
  iProduct                2 
  iSerial                 4 
  bNumConfigurations      1
  Configuration Descriptor:
    bLength                 9
    bDescriptorType         2
    wTotalLength           34
    bNumInterfaces          1
    bConfigurationValue     1
    iConfiguration          0 
    bmAttributes         0x80
      (Bus Powered)
    MaxPower              500mA
    Interface Descriptor:
      bLength                 9
      bDescriptorType         4
      bInterfaceNumber        0
      bAlternateSetting       0
      bNumEndpoints           1
      bInterfaceClass         3 Human Interface Device
      bInterfaceSubClass      0 No Subclass
      bInterfaceProtocol      0 None
      iInterface              0 
      ** UNRECOGNIZED:  09 21 00 01 00 01 22 a6 01
      Endpoint Descriptor:
        bLength                 7
        bDescriptorType         5
        bEndpointAddress     0x81  EP 1 IN
        bmAttributes            3
          Transfer Type            Interrupt
          Synch Type               None
          Usage Type               Data
        wMaxPacketSize     0x0020  1x 32 bytes
        bInterval               1


00 02 Constant
01 00 Buttons

02 55 Changes with pan
03 d7 ^^

04 3d Tilt?
05 18
06 09 Elbow?
07 1a
08 00 Cant?
09 10
10 e2 Flex?
11 0c
12 83 Roll?
13 07

14 00 Constant
15 00 ^^

16 31 Time in ms
17 10 ^^

18 00 Constant


Pan over 90 degrees:
57969, 62023,

Tilt over 90 degrees:
78, 4058,

Elbow over 90 degrees:
10198, 8250,

Twist over 360 degrees:
2164, 6145,

Flex over 90 degrees:
2546, 3599,

Roll over 180 degrees:
131, 1120

Ticks per revolution
Pan    16216  (16000)
Tilt   15920  (16000)
Elbow  7792   ( 8000)
Twist  3981   ( 4000)
Flex   4212   ( 4000)
Roll   1978   ( 2000)


'''