<s> import os <EOL> import vtkAll as vtk <EOL> import math <EOL> import time <EOL> import numpy as np <EOL> from director import transformUtils <EOL> from director import lcmUtils <EOL> from director . timercallback import TimerCallback <EOL> from director import objectmodel as om <EOL> from director import visualization as vis <EOL> from director import applogic as app <EOL> from director . debugVis import DebugData <EOL> from director import ioUtils <EOL> from director . simpletimer import SimpleTimer <EOL> from director . utime import getUtime <EOL> from director import robotstate <EOL> import drc as lcmdrc <EOL> import irobothand as lcmirobot <EOL> import robotiqhand as lcmrobotiq <EOL> class IRobotHandDriver ( object ) : <EOL> MOTOR_FINGER_0 = <NUM_LIT:0> <EOL> MOTOR_FINGER_1 = <NUM_LIT:1> <EOL> MOTOR_THUMB = <NUM_LIT:2> <EOL> def __init__ ( self , side ) : <EOL> assert side in ( '<STR_LIT:left>' , '<STR_LIT:right>' ) <EOL> self . side = side <EOL> self . enabledMotors = [ True , True , True , False ] <EOL> pass <EOL> def sendCalibrate ( self , withJig = True ) : <EOL> msg = lcmirobot . calibrate_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . in_jig = withJig <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendOpen ( self , percentage = <NUM_LIT> ) : <EOL> self . sendClose ( <NUM_LIT> - percentage ) <EOL> def sendClose ( self , percentage = <NUM_LIT> ) : <EOL> assert <NUM_LIT:0.0> <= percentage <= <NUM_LIT> <EOL> msg = lcmirobot . position_control_close_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . valid = [ True , True , True , False ] <EOL> msg . close_fraction = percentage / <NUM_LIT> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> class RobotiqHandDriver ( object ) : <EOL> def __init__ ( self , side ) : <EOL> assert side in ( '<STR_LIT:left>' , '<STR_LIT:right>' ) <EOL> self . side = side <EOL> pass <EOL> def sendCalibrate ( self ) : <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:0> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:0> <EOL> msg . mode = <NUM_LIT:0> <EOL> msg . position = <NUM_LIT:0> <EOL> msg . force = <NUM_LIT:0> <EOL> msg . velocity = <NUM_LIT:0> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> time . sleep ( <NUM_LIT:0.1> ) <EOL> msg . activate = <NUM_LIT:1> <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendDrop ( self ) : <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:1> <EOL> msg . do_move = <NUM_LIT:0> <EOL> msg . mode = <NUM_LIT:0> <EOL> msg . position = <NUM_LIT:0> <EOL> msg . force = <NUM_LIT:0> <EOL> msg . velocity = <NUM_LIT:0> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendOpen ( self , percentage = <NUM_LIT> ) : <EOL> self . sendClose ( <NUM_LIT> - percentage ) <EOL> def sendClose ( self , percentage = <NUM_LIT> ) : <EOL> assert <NUM_LIT:0.0> <= percentage <= <NUM_LIT> <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:1> <EOL> msg . position = int ( <NUM_LIT> * ( percentage / <NUM_LIT> ) ) <EOL> msg . force = <NUM_LIT> <EOL> msg . velocity = <NUM_LIT> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendCustom ( self , position , force , velocity , mode ) : <EOL> assert <NUM_LIT:0.0> <= position <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= force <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= velocity <= <NUM_LIT> <EOL> assert <NUM_LIT:0> <= int ( mode ) <= <NUM_LIT:4> <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:1> <EOL> msg . position = int ( <NUM_LIT> * ( position / <NUM_LIT> ) ) <EOL> msg . force = int ( <NUM_LIT> * ( force / <NUM_LIT> ) ) <EOL> msg . velocity = int ( <NUM_LIT> * ( velocity / <NUM_LIT> ) ) <EOL> msg . mode = int ( mode ) <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendRegrasp ( self , position , force , velocity , mode ) : <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> statusMsg = lcmUtils . captureMessage ( channel , lcmrobotiq . status_t ) <EOL> avgPosition = ( statusMsg . positionA + <EOL> statusMsg . positionB + <EOL> statusMsg . positionC ) / <NUM_LIT> <EOL> print avgPosition <EOL> newPosition = avgPosition / <NUM_LIT> * <NUM_LIT> <EOL> print newPosition <EOL> if newPosition > <NUM_LIT> : <EOL> self . sendCustom ( newPosition - <NUM_LIT> , force , velocity , mode ) <EOL> else : <EOL> self . sendCustom ( newPosition , force , velocity , mode ) <EOL> time . sleep ( <NUM_LIT> ) <EOL> self . sendCustom ( position , force , velocity , mode ) <EOL> def sendFingerControl ( self , positionA , positionB , positionC , force , velocity , scissor , mode ) : <EOL> assert <NUM_LIT:0.0> <= positionA <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= positionB <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= positionC <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= force <= <NUM_LIT> <EOL> assert <NUM_LIT:0.0> <= velocity <= <NUM_LIT> <EOL> assert <NUM_LIT:0> <= int ( mode ) <= <NUM_LIT:4> <EOL> if not scissor is None : <EOL> assert <NUM_LIT:0.0> <= scissor <= <NUM_LIT> <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:1> <EOL> msg . ifc = <NUM_LIT:1> <EOL> msg . positionA = int ( positionA ) <EOL> msg . positionB = int ( positionB ) <EOL> msg . positionC = int ( positionC ) <EOL> msg . force = int ( <NUM_LIT> * ( force / <NUM_LIT> ) ) <EOL> msg . velocity = int ( <NUM_LIT> * ( velocity / <NUM_LIT> ) ) <EOL> msg . mode = int ( mode ) <EOL> if not scissor is None : <EOL> msg . isc = <NUM_LIT:1> <EOL> msg . positionS = int ( scissor ) <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def setMode ( self , mode ) : <EOL> assert <NUM_LIT:0> <= int ( mode ) <= <NUM_LIT:4> <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:0> <EOL> msg . position = <NUM_LIT:0> <EOL> msg . force = <NUM_LIT:0> <EOL> msg . velocity = <NUM_LIT:0> <EOL> msg . mode = int ( mode ) <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendDeactivate ( self ) : <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:0> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:0> <EOL> msg . mode = <NUM_LIT:0> <EOL> msg . position = <NUM_LIT:0> <EOL> msg . force = <NUM_LIT:0> <EOL> msg . velocity = <NUM_LIT:0> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) <EOL> def sendActivate ( self ) : <EOL> msg = lcmrobotiq . command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . activate = <NUM_LIT:1> <EOL> msg . emergency_release = <NUM_LIT:0> <EOL> msg . do_move = <NUM_LIT:0> <EOL> msg . mode = <NUM_LIT:0> <EOL> msg . position = <NUM_LIT:0> <EOL> msg . force = <NUM_LIT:0> <EOL> msg . velocity = <NUM_LIT:0> <EOL> channel = '<STR_LIT>' % self . side . upper ( ) <EOL> lcmUtils . publish ( channel , msg ) </s>
