def interpolate_rotation(R1,R2,u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1,m2,u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu,angle)
return so3.rotation(axis,angle)
def interpolate_rotation(R1, R2, u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def saveStep(self,extra=[]):
sim = self.sim
world = sim.world
sim.updateWorld()
values = []
values.append(sim.getTime())
for i in xrange(world.numRobots()):
robot = world.robot(i)
values += robot.getCom()
values += robot.getConfig()
values += sim.getActualTorques(i)
"""
j = 0
while True:
s = self.sim.controller(i).getSensor(j)
if len(s.name())==0:
break
meas = s.measurements()
values += meas
j += 1
"""
for i in xrange(world.numRigidObjects()):
obj = world.rigidObject(i)
T = obj.getTransform()
values += se3.apply(T,obj.getMass().getCom())
values += T[1]
values += so3.moment(T)
values += sim.body(obj).getVelocity()[1]
values += sim.body(obj).getVelocity()[0]
if self.f_contact:
for i,id in enumerate(self.colliding):
for j in range(i+1,len(self.colliding)):
id2 = self.colliding[j]
if sim.hadContact(id,id2):
clist = sim.getContacts(id,id2);
f = sim.contactForce(id,id2)
m = sim.contactTorque(id,id2)
pavg = [0.0]*3
navg = [0.0]*3
for c in clist:
pavg = vectorops.add(pavg,c[0:3])
navg = vectorops.add(navg,c[3:6])
if len(clist) > 0:
pavg = vectorops.div(pavg,len(clist))
navg = vectorops.div(navg,len(clist))
body1 = world.getName(id)
body2 = world.getName(id2)
values = [sim.getTime(),body1,body2,len(clist)]
values += pavg
values += navg
values += f
values += m
self.f_contact.write(','.join(str(v) for v in values))
self.f_contact.write('\n')
if extra:
values += extra
self.f.write(','.join([str(v) for v in values]))
self.f.write('\n')
def saveStep(self, extra=[]):
sim = self.sim
world = sim.world
sim.updateWorld()
values = []
values.append(sim.getTime())
for i in xrange(world.numRobots()):
robot = world.robot(i)
values += robot.getCom()
values += robot.getConfig()
values += robot.getVelocity()
values += sim.getActualTorques(i)
j = 0
while True:
s = self.sim.controller(i).sensor(j)
if s is None or len(s.name()) == 0:
break
meas = s.getMeasurements()
values += meas
j += 1
for i in xrange(world.numRigidObjects()):
obj = world.rigidObject(i)
T = obj.getTransform()
values += se3.apply(T, obj.getMass().getCom())
values += T[1]
values += so3.moment(T[0])
values += sim.body(obj).getVelocity()[1]
values += sim.body(obj).getVelocity()[0]
if self.f_contact:
for i, id in enumerate(self.colliding):
for j in range(i + 1, len(self.colliding)):
id2 = self.colliding[j]
if sim.hadContact(id, id2):
clist = sim.getContacts(id, id2)
f = sim.contactForce(id, id2)
m = sim.contactTorque(id, id2)
pavg = [0.0] * 3
navg = [0.0] * 3
for c in clist:
pavg = vectorops.add(pavg, c[0:3])
navg = vectorops.add(navg, c[3:6])
if len(clist) > 0:
pavg = vectorops.div(pavg, len(clist))
navg = vectorops.div(navg, len(clist))
body1 = world.getName(id)
body2 = world.getName(id2)
cvalues = [sim.getTime(), body1, body2, len(clist)]
cvalues += pavg
cvalues += navg
cvalues += f
cvalues += m
self.f_contact.write(','.join(str(v) for v in cvalues))
self.f_contact.write('\n')
if extra:
values += extra
if not (self.f is None):
self.f.write(','.join([str(v) for v in values]))
self.f.write('\n')
def onUpdate(self):
global deviceState
if len(deviceState)==0: return
cobj = {'type':'multi_ik','safe':1,'components':[]}
for i,dstate in enumerate(deviceState):
if not dstate['buttonDown']: continue
ikgoal = {}
ikgoal['link'] = endEffectorIndices[i]
Rlocal,tlocal = endEffectorLocalTransforms[i]
ikgoal['localPosition'] = tlocal
ikgoal['endPosition'] = dstate['widgetTransform'][1]
ikgoal['endRotation'] = so3.moment(so3.mul(Rlocal,dstate['widgetTransform'][0]))
ikgoal['posConstraint'] = 'fixed'
ikgoal['rotConstraint'] = 'fixed'
cobj['components'].append(ikgoal)
if cobj != self.lastData:
self.sendMessage({'type':'set','path':self.topic,'data':cobj})
self.lastData = cobj
return Service.onUpdate(self)
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..."
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