def random_rotation():
"""Returns a uniformly distributed rotation matrix."""
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return so3.from_moment(m)
def random_rotation():
"""Returns a uniformly distributed rotation matrix."""
q = [
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)
]
q = vectorops.unit(q)
theta = math.acos(q[3]) * 2.0
if abs(theta) < 1e-8:
m = [0, 0, 0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]), theta)
return so3.from_moment(m)
def display(self):
global currentTask, currentTaskLock
glEnable(GL_LIGHTING)
currentTaskLock.acquire()
task = currentTask
if task != None:
task = currentTask.copy()
currentTaskLock.release()
if task != None:
if task['type'] == 'multi_ik':
for ikgoal in task['components']:
T = (so3.from_moment(ikgoal['endRotation']),
ikgoal['endPosition'])
width = 0.02
length = 0.2
gldraw.xform_widget(T, length, width)
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..."
trueCalibrationConfigs = []
calibrationConfigs = []
trueObservations = []
observations = []
for obs in xrange(numObs):
q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]
#don't move head
for i in range(13):
q0[i] = 0
trueCalibrationConfigs.append(q0)
trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world)
for q0 in trueCalibrationConfigs:
robot.setConfig(q0)
obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform())
dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))]
dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)])
calibrationConfigs.append(vectorops.add(q0,dq))
observations.append(se3.mul(obs0,dobs))
trueObservations.append(obs0)
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration ground truth")
rgroup.addFrame("camera link",worldCoordinates=pc.getTransform())
rgroup.addFrame("marker link",worldCoordinates=pm.getTransform())
rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0)
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
for i,(obs,obs0) in enumerate(zip(observations,trueObservations)):
rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0)
rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs)
vis.add("world",world)
for i,q in enumerate(calibrationConfigs):
trueCalibrationConfigs = []
calibrationConfigs = []
trueObservations = []
observations = []
for obs in xrange(numObs):
q0 = [random.uniform(a,b) for (a,b) in zip(qmin,qmax)]
#don't move head
for i in range(13):
q0[i] = 0
trueCalibrationConfigs.append(q0)
trueCalibrationConfigs=resource.get("calibration.configs",default=trueCalibrationConfigs,type="Configs",description="Calibration configurations",world=world)
for q0 in trueCalibrationConfigs:
robot.setConfig(q0)
obs0 = se3.mul(se3.inv(lc.getTransform()),lm.getTransform())
dq = [random.uniform(-jointEncoderError,jointEncoderError) for i in range(len(q0))]
dobs = (so3.from_moment([random.uniform(-sensorErrorRads,sensorErrorRads) for i in range(3)]),[random.uniform(-sensorErrorMeters,sensorErrorMeters) for i in range(3)])
calibrationConfigs.append(vectorops.add(q0,dq))
observations.append(se3.mul(obs0,dobs))
trueObservations.append(obs0)
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration ground truth")
rgroup.addFrame("camera link",worldCoordinates=pc.getTransform())
rgroup.addFrame("marker link",worldCoordinates=pm.getTransform())
rgroup.addFrame("camera (ground truth)",parent="camera link",relativeCoordinates=Tc0)
rgroup.addFrame("marker (ground truth)",parent="marker link",relativeCoordinates=Tm0)
for i,(obs,obs0) in enumerate(zip(observations,trueObservations)):
rgroup.addFrame("obs"+str(i)+" (ground truth)",parent="camera (ground truth)",relativeCoordinates=obs0)
rgroup.addFrame("obs"+str(i)+" (from camera)",parent="camera (ground truth)",relativeCoordinates=obs)
visualization.add("world",world)
for i,q in enumerate(calibrationConfigs):
def move_to_grasp_object(self,object):
"""Sets the robot's configuration so the gripper grasps object at
one of its potential grasp locations. Might change self.active_limb
to the appropriate limb. Must change self.active_grasp to the
selected grasp.
If successful, sends the motion to the low-level controller and
returns True.
Otherwise, does not modify the low-level controller and returns False.
"""
self.waitForMove()
self.controller.commandGripper(self.active_limb,[1])
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
#get the end of the commandGripper motion
qcmd = self.controller.getCommandedConfig()
self.robot.setConfig(qcmd)
baxter.set_model_gripper_command(self.robot,self.active_limb,[1])
qcmd = self.robot.getConfig()
#solve an ik solution to one of the grasps
grasp_goals = []
pregrasp_goals = []
pregrasp_shift = [0,0,0.05]
for (grasp,gxform) in grasps:
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(baxter.left_gripper_center_xform))
Tpg = se3.mul(gxform,se3.inv((baxter.left_gripper_center_xform[0],vectorops.add(baxter.left_gripper_center_xform[1],pregrasp_shift))))
Tg = (so3.from_moment(so3.moment(Tg[0])),Tg[1])
Tpg = (so3.from_moment(so3.moment(Tpg[0])),Tpg[1])
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
pregoal = ik.objective(self.left_gripper_link,R=Tpg[0],t=Tpg[1])
else:
Tg = se3.mul(gxform,se3.inv(baxter.right_gripper_center_xform))
Tpg = se3.mul(gxform,se3.inv((baxter.right_gripper_center_xform[0],vectorops.add(baxter.right_gripper_center_xform[1],pregrasp_shift))))
#hack: weird matrices coming from ODE aren't rotations within tolerance, gives abort
Tg = (so3.from_moment(so3.moment(Tg[0])),Tg[1])
Tpg = (so3.from_moment(so3.moment(Tpg[0])),Tpg[1])
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
pregoal = ik.objective(self.right_gripper_link,R=Tpg[0],t=Tpg[1])
grasp_goals.append(goal)
pregrasp_goals.append(pregoal)
sortedSolutions = self.get_ik_solutions(pregrasp_goals,[self.active_limb]*len(grasp_goals),qcmd)
if len(sortedSolutions)==0: return False
#prototyping hack: move straight to target
if SKIP_PATH_PLANNING:
for pregrasp in sortedSolutions:
graspIndex = pregrasp[1]
gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],pregrasp[0],maxResults=1)
if len(gsolns)==0:
print "Couldn't find grasp config for pregrasp? Trying another"
else:
self.waitForMove()
self.sendPath([pregrasp[0],gsolns[0][0]])
self.active_grasp = grasps[graspIndex][0]
return True
print "Planning failed"
return False
for solution in sortedSolutions:
path = self.planner.plan(qcmd,solution[0])
graspIndex = solution[1]
if path != None:
#now solve for grasp
gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],path[-1],maxResults=1)
if len(gsolns)==0:
print "Couldn't find grasp config??"
else:
self.waitForMove()
self.sendPath(path+[gsolns[0][0]])
self.active_grasp = grasps[graspIndex][0]
return True
print "Planning failed"
return False
