def display(self):
if self.state == None: return
glEnable(GL_LIGHTING)
self.statelock.acquire()
for i,d in enumerate(self.state):
Twidget = d['widgetTransform']
T = (so3.mul(Twidget[0],so3.inv(deviceToWidgetTransform[0])),Twidget[1])
width = 0.05*d['rotationScale']
length = 0.1*d['positionScale']
gldraw.xform_widget(T,length,width)
self.statelock.release()
if self.robot:
#draw robot
qcmd = self.qcmdGetter.get()
if qcmd:
self.robot.setConfig(qcmd)
self.robot.drawGL(True)
else:
self.robot.drawGL(True)
qdest = self.qdestGetter.get()
if qdest:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[0,1,0,0.5])
self.robot.setConfig(qdest)
self.robot.drawGL(False)
glDisable(GL_BLEND)
glDisable(GL_LIGHTING)
def move_gripper_upright(self,limb,moveVector,collisionchecker = None):
vertical = [0,0,0.1]
if self.active_limb == 'left':
gripperlink = self.left_gripper_link
else:
gripperlink = self.right_gripper_link
qcur = self.robot.getConfig()
vertical_in_gripper_frame = so3.apply(so3.inv(gripperlink.getTransform()[0]),vertical)
centerpos = se3.mul(gripperlink.getTransform(),left_gripper_center_xform)[1]
move_target = vectorops.add(centerpos,moveVector)
movegoal = ik.objective(gripperlink,local=[left_gripper_center_xform[1],vectorops.add(left_gripper_center_xform[1],vertical_in_gripper_frame)],world=[move_target,vectorops.add(move_target,vertical)])
sortedSolutions = self.get_ik_solutions([movegoal],[self.active_limb],qcur,validity_checker=collisionchecker,maxResults=1)
if len(sortedSolutions) == 0:
print "No upright-movement config found"
return False
self.robot.setConfig(sortedSolutions[0][0])
return True
def emulate(self, sim):
"""Given a Simulator instance, emulates the sensor.
Result is a CameraColorDetectorOutput structure."""
global objectColors
xscale = math.tan(math.radians(self.fov * 0.5)) * self.w / 2
blobs = []
for i in range(sim.world.numRigidObjects()):
o = sim.world.rigidObject(i)
body = sim.body(o)
Tb = body.getTransform()
plocal = se3.apply(se3.inv(self.Tsensor), Tb[1])
x, y, z = plocal
if z < self.dmin or z > self.dmax:
continue
c = objectColors[i % len(objectColors)]
o.geometry().setCurrentTransform(
so3.mul(so3.inv(self.Tsensor[0]), Tb[0]), [0] * 3)
bmin, bmax = o.geometry().getBB()
err = self.pixelError
dscale = xscale / z
xim = dscale * x + self.w / 2
yim = dscale * y + self.h / 2
xmin = int(xim - dscale * (bmax[0] - bmin[0]) * 0.5 +
random.uniform(-err, err))
ymin = int(yim - dscale * (bmax[1] - bmin[1]) * 0.5 +
random.uniform(-err, err))
xmax = int(xim + dscale * (bmax[0] - bmin[0]) * 0.5 +
random.uniform(-err, err))
ymax = int(yim + dscale * (bmax[1] - bmin[1]) * 0.5 +
random.uniform(-err, err))
if xmin < 0: xmin = 0
if ymin < 0: ymin = 0
if xmax >= self.w: xmax = self.w - 1
if ymax >= self.h: ymax = self.h - 1
if ymax <= ymin: continue
if xmax <= xmin: continue
#print "Actual x,y,z",x,y,z
#print "blob color",c[0:3],"dimensions",xmin,xmax,"x",ymin,ymax
blob = CameraBlob(c[0:3], 0.5 * (xmin + xmax), 0.5 * (ymin + ymax),
xmax - xmin, ymax - ymin)
blobs.append(blob)
return CameraColorDetectorOutput(blobs)
def onMessage(self, msg):
global deviceState, defaultDeviceState
#print "Getting haptic message"
#print msg
if msg['type'] != 'MultiHapticState':
print "Strange message type", msg['type']
return
if len(deviceState) == 0:
print "Adding", len(
msg['devices']) - len(deviceState), "haptic devices"
while len(deviceState) < len(msg['devices']):
deviceState.append(defaultDeviceState.copy())
if len(msg['devices']) != len(deviceState):
print "Incorrect number of devices in message:", len(
msg['devices'])
return
#change overall state depending on button 2
if 'events' in msg:
for e in msg['events']:
#print e['type']
if e['type'] == 'b2_down':
dstate = deviceState[e['device']]
toggleMode(dstate)
print 'Changing device', e['device'], 'to state', dstate[
'mode']
for i in range(len(deviceState)):
dmsg = msg['devices'][i]
dstate = deviceState[i]
if dmsg['button1'] == 1:
#drag widget if button 1 is down
oldButtonDown = dstate['buttonDown']
dstate['buttonDown'] = True
#moving
if dstate['mode'] == 'normal':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
if self.widgetGetters == None:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
else:
Twidget = self.widgetGetters[i]()
if Twidget != None:
dstate['moveStartWidgetTransform'] = Twidget
else:
dstate['moveStartWidgetTransform'] = dstate[
'widgetTransform']
#================================================================================================
# #perform transformation of widget
# deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position'])
# #compute relative motion
# Tdev = se3.mul(deviceToWidgetTransform,deviceTransform)
# TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform'])
# relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1]))
# #scale relative motion
# Rrel,trel = relDeviceTransform
# wrel = so3.moment(Rrel)
# wrel = vectorops.mul(wrel,dstate['rotationScale'])
# trel = vectorops.mul(trel,dstate['positionScale'])
# #print "Moving",trel,"rotating",wrel
# relDeviceTransform = (so3.from_moment(wrel),trel)
# #perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
#================================================================================================
#- perform transformation of widget
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
# delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1])
# print "haptic moves: ", delta_device
delta_device_R_matrix = so3.mul(
deviceTransform[0],
so3.inv(dstate['moveStartDeviceTransform'][0]))
# delta_device_R = so3.moment(delta_device_R_matrix)
# norm_delta_R = vectorops.norm(delta_device_R)
# if norm_delta_R != 0:
# vec_delta_R = vectorops.div(delta_device_R, norm_delta_R)
# else:
# vec_delta_R = [0,0,0]
#
# print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R
#- compute relative motion
Tdev = se3.mul(deviceToBaxterBaseTransform,
deviceTransform)
TdevStart = se3.mul(deviceToBaxterBaseTransform,
dstate['moveStartDeviceTransform'])
# delta_widget = vectorops.sub(Tdev[1],TdevStart[1])
# print "Baxter moves: ", delta_widget
# delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0]))
# delta_widget_R = so3.moment(delta_widget_R_matrix)
# norm_widget_R = vectorops.norm(delta_widget_R)
# if norm_widget_R != 0:
# vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R)
# else:
# vec_widget_R = [0,0,0]
# print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R
relDeviceTransform = (so3.mul(Tdev[0],
so3.inv(TdevStart[0])),
vectorops.sub(Tdev[1], TdevStart[1]))
#- scale relative motion
Rrel, trel = relDeviceTransform
wrel = so3.moment(Rrel)
wrel = vectorops.mul(wrel, dstate['rotationScale'])
trel = vectorops.mul(trel, dstate['positionScale'])
#- print "Moving",trel,"rotating",wrel
relDeviceTransform = (so3.from_moment(wrel), trel)
#- perform transform
# dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
dstate['widgetTransform'] = (
so3.mul(dstate['moveStartWidgetTransform'][0],
relDeviceTransform[0]),
vectorops.add(dstate['moveStartWidgetTransform'][1],
relDeviceTransform[1]))
#================================================================================================
elif dstate['mode'] == 'scaling':
if not oldButtonDown:
dstate['moveStartDeviceTransform'] = (so3.from_moment(
dmsg['rotationMoment']), dmsg['position'])
#perform scaling
deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
dmsg['position'])
oldDeviceTransform = dstate['moveStartDeviceTransform']
znew = deviceTransform[1][1]
zold = oldDeviceTransform[1][1]
posscalerange = [1e-2, 20]
rotscalerange = [0.5, 2]
newposscale = min(
max(
dstate['positionScale'] * math.exp(
(znew - zold) * 10), posscalerange[0]),
posscalerange[1])
newrotscale = min(
max(
dstate['rotationScale'] * math.exp(
(znew - zold) * 4), rotscalerange[0]),
rotscalerange[1])
if any(newposscale == v for v in posscalerange) or any(
newrotscale == v for v in rotscalerange):
pass #dont change the scale
else:
dstate['positionScale'] = newposscale
dstate['rotationScale'] = newrotscale
print "Setting position scale to", dstate[
'positionScale'], "rotation scale to", dstate[
'rotationScale']
#update start device transform
dstate['moveStartDeviceTransform'] = deviceTransform
elif dstate['mode'] == 'gripper':
print "Gripper mode not available"
dstate['mode'] = 'normal'
dstate['buttonDown'] = False
else:
dstate['buttonDown'] = False
if self.viewer: self.viewer.update(deviceState)
else: print "No viewer..."