def calibrateCamera(self):
print self.calibratedCameraXform
calibrateR = self.calibratedCameraXform[0]
calibrateT = self.calibratedCameraXform[1]
try:
input_var = raw_input("Enter joint and angle to change to separated by a comma: ").split(",")
# translational transformation
if input_var[0] == "x":
calibrateT[0] = calibrateT[0] + float(input_var[1])
elif input_var[0] == "y":
calibrateT[1] = calibrateT[1] + float(input_var[1])
elif input_var[0] == "z":
calibrateT[2] = calibrateT[2] + float(input_var[1])
# rotational transformations
elif input_var[0] == "xr":
calibrateR = so3.mul(calibrateR, so3.rotation([1, 0, 0], float(input_var[1])))
elif input_var[0] == "yr":
calibrateR = so3.mul(calibrateR, so3.rotation([0, 1, 0], float(input_var[1])))
elif input_var[0] == "zr":
calibrateR = so3.mul(calibrateR, so3.rotation([0, 0, 1], float(input_var[1])))
time.sleep(0.1)
self.calibratedCameraXform = (calibrateR, calibrateT)
except:
print "input error\n"
print "printing camera calibration"
print self.calibratedCameraXform
def euler_angle_to_rotation(ea,convention='zyx'):
"""Converts an euler angle representation to a rotation matrix.
Can use arbitrary axes specified by the convention
arguments (default is 'zyx', or roll-pitch-yaw euler angles). Any
3-letter combination of 'x', 'y', and 'z' are accepted.
"""
axis_names_to_vectors = dict([('x',(1,0,0)),('y',(0,1,0)),('z',(0,0,1))])
axis0,axis1,axis2=convention
R0 = so3.rotation(axis_names_to_vectors[axis0],ea[0])
R1 = so3.rotation(axis_names_to_vectors[axis1],ea[1])
R2 = so3.rotation(axis_names_to_vectors[axis2],ea[2])
return so3.mul(R0,so3.mul(R1,R2))
def move_camera_to_bin(self,bin_name):
"""Sets the robot's configuration to a viewpoint that
observes bin_name. Will also change self.active_limb to the
appropriate limb.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
# get the bin position information
front_center = self.knowledge.bin_front_center(bin_name)
vantage_point = self.knowledge.bin_vantage_point(bin_name)
for link, side in [ (self.left_camera_link, 'left'), (self.right_camera_link, 'right') ]:
# move the camera to vantage_point such that it views opposite the shelf's z axis
#goal = ik.objective(link, local=[ [0,0,0], [0,0,vantage_point_offset] ], world=[ vantage_point, front_center ])
#goal = ik.objective(link, R=so3.identity(), t=vantage_point)
goal = ik.objective(link, R=so3.mul(so3.rotation([0,0,1],math.pi), self.knowledge.shelf_xform[0]), t=vantage_point)
# reset the robot to the starting config
goal.robot.setConfig(self.config)
# try with 50 attempts
# randomize after each failed attempt
for attempts in range(50):
# solve the inverse kinematics with 1 cm accuracy
if ik.solve(goal, tol=0.01):
self.active_limb = side
self.config = goal.robot.getConfig()
return True
else:
# randomize this limb
randomize_links(goal.robot, arm_link_names[side])
return False
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(so3.inv(deviceToBaxterBaseTransform[0]),Twidget[0]),Twidget[1])
T = (so3.mul(Twidget[0],
deviceToBaxterBaseTransform[0]), Twidget[1])
# T = Twidget
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 calibrateCamera(self, index=0):
global CAMERA_TRANSFORM
global REAL_CAMERA
if REAL_CAMERA:
if len(CAMERA_TRANSFORM) < index:
return False
while(True):
try:
input_var = raw_input("Camera: Enter joint and angle to change to separated by a comma: ").split(',');
#translational transformation
calibrateR = CAMERA_TRANSFORM[index][0]
calibrateT = CAMERA_TRANSFORM[index][1]
if(input_var[0] == "x" ):
calibrateT[0] = calibrateT[0] + float(input_var[1])
elif(input_var[0] == "y" ):
calibrateT[1] = calibrateT[1] + float(input_var[1])
elif(input_var[0] == "z" ):
calibrateT[2] = calibrateT[2] + float(input_var[1])
#rotational transformations
elif(input_var[0] == "xr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([1, 0, 0], float(input_var[1])))
elif(input_var[0] == "yr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 1, 0], float(input_var[1])))
elif(input_var[0] == "zr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 0, 1], float(input_var[1])))
elif(input_var[0] == "q"):
break
except:
print "input error\n"
#print error.strerror
time.sleep(0.1);
CAMERA_TRANSFORM[index] = (calibrateR, calibrateT)
self.takePicture(index)
print 'local camera ', index, ' transform is ', CAMERA_TRANSFORM[index]
def calibrateShelf(self):
calibratedShelfXform = self.world.rigidObject(0).getTransform()
print calibratedShelfXform
calibrateR = calibratedShelfXform[0];
calibrateT = calibratedShelfXform[1];
try:
input_var = raw_input("Enter joint and angle to change to separated by a comma: ").split(',');
#translational transformation
if(input_var[0] == "x" ):
calibrateT[0] = calibrateT[0] + float(input_var[1])
elif(input_var[0] == "y" ):
calibrateT[1] = calibrateT[1] + float(input_var[1])
elif(input_var[0] == "z" ):
calibrateT[2] = calibrateT[2] + float(input_var[1])
#rotational transformations
elif(input_var[0] == "xr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([1, 0, 0], float(input_var[1])))
elif(input_var[0] == "yr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 1, 0], float(input_var[1])))
elif(input_var[0] == "zr" ):
calibrateR = so3.mul(calibrateR, so3.rotation([0, 0, 1], float(input_var[1])))
time.sleep(0.1);
self.world.rigidObject(0).setTransform(calibrateR, calibrateT)
print self.world.rigidObject(0).getTransform()
except:
print "input error\n"
print "printing shelf calibration"
print self.world.rigidObject(0).getTransform()
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 grasp_xforms(self,object):
#you may want to implement this. Returns a list of world transformations
#for the grasps defined for the given object (an ItemInBin instance).
#The visualizer will draw these transforms if 'draw_grasps' is turned to True.
#TODO:
item_xform = object.xform
grasps = object.info.grasps
R_i = item_xform[0]
t_i = item_xform[1]
result = []
for g in grasps:
R_g = g.grasp_xform[0]
t_g = g.grasp_xform[1]
t_g = so3.apply(R_i, t_g)
t_net = [t_i[0]+t_g[0], t_i[1]+t_g[1], t_i[2]+t_g[2]]
R_net = so3.mul(R_i, R_g)
result.append((R_net, t_net))
return result
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 load_world():
world = robotsim.WorldModel()
print "Loading reflex hands"
# world.loadElement(os.path.join(model_dir,"reflex.rob"))
world.loadElement(os.path.join(model_dir,"reflex_col_with_moving_base.rob"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
R,t = world.robot(0).link(0).getParentTransform()
R_reorient = so3.rotation([1,0,0], math.pi/2)
offset = (0,0.15,0.1)
R = so3.mul(R,R_reorient)
t = vectorops.add(t, offset)
world.robot(0).link(0).setParentTransform(R,t)
for i in range(world.robot(0).numLinks()):
world.robot(0).link(i).geometry().setCollisionMargin(0)
return world
def load_item_geometry(bmin,bmax,geometry_ptr = None):
"""Loads the geometry of the given item and returns it. If geometry_ptr
is provided, then it is assumed to be a Geometry3D object and the object
geometry is loaded into it."""
if geometry_ptr == None:
geometry_ptr = Geometry3D()
# fn = model_dir + "cylinder.tri"
fn = model_dir + "cube.tri"
# fn = model_dir + "/objects/teapot/pots.tri"
# bmin = [0,0,0]
# bmax = [1.2,1.5,1.25]
# fn = model_dir + "items/rollodex_mesh_collection_jumbo_pencil_cup/textured_meshes/optimized_poisson_textured_mesh.ply"
if not geometry_ptr.loadFile(fn):
print "Error loading cube file",fn
exit(1)
center = vectorops.mul(vectorops.add(bmin,bmax),0.5)
scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
translate = vectorops.sub(bmin,center)
geometry_ptr.transform(so3.mul(scale,so3.rotation([0,0,1],math.pi/180*0)),translate)
geometry_ptr.setCurrentTransform(so3.identity(),[0,0,0])
return geometry_ptr
# shelf.geometryFile = 'box'
# shelf.grasps.append(ItemGrasp((Zto_Y,[0,0,0])))
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.08]
tall_item.bmax = [0.03,0.05,0.08]
tall_item.geometryFile = 'box'
# append these ItemGrasp objects to tall_item.grasps list
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.04])))
from klampt import so3
import json
from math import pi
import numpy
bbbs = {
'bin_A' : ([-0.41,1.55,0],[-0.158,1.78,0.42]),
'bin_B' : ([-0.149,1.55,0],[0.149,1.78,0.42]),
'bin_C' : ([0.158,1.55,0],[0.41,1.78,0.42]),
'bin_D' : ([-0.41,1.32,0],[-0.158,1.52,0.42]),
'bin_E' : ([-0.149,1.32,0],[0.149,1.52,0.42]),
'bin_F' : ([0.158,1.32,0],[0.41,1.52,0.42]),
'bin_G' : ([-0.41,1.09,0],[-0.158,1.29,0.42]),
'bin_H' : ([-0.149,1.09,0],[0.149,1.29,0.42]),
'bin_I' : ([0.158,1.09,0],[0.41,1.29,0.42]),
'bin_J' : ([-0.41,0.82,0],[-0.158,1.06,0.42]),
'bin_K' : ([-0.149,0.82,0],[0.149,1.06,0.42]),
'bin_L' : ([0.158,0.82,0],[0.41,1.06,0.42]),
}
for (k, bs) in bbbs.items():
bs = [ so3.apply(so3.mul(so3.rotation([0,0,1], 3.141592), so3.rotation([1,0,0], 3.141592/2)), b) for b in bs ]
bmin = [ min(*x) for x in zip(*bs) ]
bmax = [ max(*x) for x in zip(*bs) ]
bbbs[k] = (bmin, bmax)
print k, bbbs[k]
json.dump(bbbs, open('kb/shelf_dims.json', 'w'), indent=4)
json.dump(bbbs, open('kb/bin_bounds.json', 'w'), indent=4)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
print c, "pressed"
if c.lower() == 'q':
motion.robot.shutdown()
self.close()
elif c.lower() == 'h':
self.print_help()
elif c.lower() == 'o':
self.useRotation = not self.useRotation
print "Using rotation?", self.useRotation
elif c.lower() == 'p':
self.localMotion = not self.localMotion
print "Local motion?", self.localMotion
elif c.lower() == 'l':
print "Switching to left arm"
self.driveArm = 'l'
self.driveRot, self.drivePos = motion.robot.left_ee.commandedTransform(
)
elif c.lower() == 'r':
print "Switching to right arm"
self.driveArm = 'r'
self.driveRot, self.drivePos = motion.robot.right_ee.commandedTransform(
)
elif c == ' ':
motion.robot.stopMotion()
elif c == 'X':
self.drivePos[0] += self.increment
self.updateCommand()
elif c == 'x':
self.drivePos[0] -= self.increment
self.updateCommand()
elif c == 'Y':
self.drivePos[1] += self.increment
self.updateCommand()
elif c == 'y':
self.drivePos[1] -= self.increment
self.updateCommand()
elif c == 'Z':
self.drivePos[2] += self.increment
self.updateCommand()
elif c == 'z':
self.drivePos[2] -= self.increment
self.updateCommand()
elif c == '!':
R = so3.rotation([1, 0, 0], self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == '1':
R = so3.rotation([1, 0, 0], -self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == '@':
R = so3.rotation([0, 1, 0], self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == '2':
R = so3.rotation([0, 1, 0], -self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == '#':
R = so3.rotation([0, 0, 1], self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == '3':
R = so3.rotation([0, 0, 1], -self.increment)
self.driveRot = so3.mul(R, self.driveRot)
self.updateCommand()
elif c == ',' or c == '<':
self.baseCommand[2] += self.incrementang
self.updateBaseCommand()
elif c == '.' or c == '>':
self.baseCommand[2] -= self.incrementang
self.updateBaseCommand()
self.refresh()
# ),
# [ 0.50, 0.50, center_vp[1][2] ]
#)
# for the right bins using the right hand
#elif b[4] in 'CFIL':
#wps[b] = (
# so3.mul(
# so3.rotation([0,0,1], -pi/4),
# so3.rotation([0,1,0], pi/2)
# ),
# [ -0.50, 0.50, center_vp[1][2] ]
#)
#else:
# print 'skipping invalid bin', b
# continue
wps[b] = (
so3.mul(
so3.rotation([0,0,1],-pi/2),
so3.rotation([0,1,0], pi/2)
),
[ bbc[0], bbs[b][1][1] + 0.25, bbc[2] ]
)
print b, wps[b][1]
json.dump(wps, open('withdraw_point_xforms.json', 'w'), indent=4)
def load_fake_items():
"""Loads the fake items from the homework assignments"""
global item_info
#set up some items and some potential grasps
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
tall_item.geometryFile = 'box'
XtoY = so3.from_axis_angle(([0,0,1],math.pi*0.5))
Xto_Y = so3.from_axis_angle(([0,0,1],-math.pi*0.5))
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.03,0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.03,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.03,0.04])))
small_item = ItemInfo('small_item')
small_item.bmin = [-0.01,-0.04,-0.01]
small_item.bmax = [0.01,0.04,0.01]
small_item.geometryFile = 'box'
small_item.grasps.append(ItemGrasp((so3.identity(),[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((so3.identity(),[0,0.0,0])))
small_item.grasps.append(ItemGrasp((so3.identity(),[0,0.02,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((XtoZ,[0,0.02,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((Xto_Z,[0,0.02,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,-0.02,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,0.0,0])))
small_item.grasps.append(ItemGrasp((XZflip,[0,0.02,0])))
med_item = ItemInfo('med_item')
med_item.bmin = [-0.03,-0.03,-0.03]
med_item.bmax = [0.03,0.03,0.03]
med_item.geometryFile = 'box'
#med_item.grasps.append(ItemGrasp((so3.identity(),[0,0,0])))
med_item.grasps.append(ItemGrasp((so3.mul(XtoY,ZtoY),[0,0,0])))
#med_item.grasps.append(ItemGrasp((Xto_Y,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XYflip,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XtoZ,[0,0,0])))
#med_item.grasps.append(ItemGrasp((Xto_Z,[0,0,0])))
#med_item.grasps.append(ItemGrasp((XZflip,[0,0,0])))
#med_item.grasps.append(ItemGrasp((ZtoY,[0,0,0])))
#med_item.grasps.append(ItemGrasp((Zto_Y,[0,0,0])))
#med_item.grasps.append(ItemGrasp((YZflip,[0,0,0])))
#gripper local rotational axis is x, perturb +/- along that axis
for g in med_item.grasps[:]:
angle = 15.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],angle))),g.grasp_xform[1])))
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
angle = 30.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],angle))),g.grasp_xform[1])))
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
angle = 45.0/180.0*math.pi
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],so3.from_axis_angle(([1,0,0],-angle))),g.grasp_xform[1])))
#flip 180 about z axis
for g in med_item.grasps[:]:
med_item.grasps.append(ItemGrasp((so3.mul(g.grasp_xform[0],XYflip),g.grasp_xform[1])))
item_info = dict((i.name,i) for i in [tall_item, small_item, med_item])
#set up some items and some potential grasps
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
YZflip_90 = so3.from_axis_angle(([1,0,0],math.pi*0.5))
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,0])
tall_item.grasps.append(x)
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,-0.02]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,-0.02])
tall_item.grasps.append(x)
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,-0.04]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,-0.04])
tall_item.grasps.append(x)
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0.02]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,0.02])
tall_item.grasps.append(x)
x = ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0.04]))
x.pregrasp = (so3.mul(ZtoY,XYflip),[0,0.12,0.04])
tall_item.grasps.append(x)
world="klampt_models/" + klampt_model_name,
frame=left_gripper_link_name,
)
right_gripper_center_xform = resource.get(
"right_gripper_center.xform",
type="RigidTransform",
description="Right gripper center",
world="klampt_models/" + klampt_model_name,
frame=right_gripper_link_name,
)
resource.setDirectory("resources/baxter_scoop")
# local transforms of the cameras in the camera link
left_camera_center_xform = (so3.rotation([0, 0, 1], -3.141592 / 2), [-0.11, -0.05, -0.01])
right_camera_center_xform = (
so3.mul(so3.rotation([0, 1, 0], -3.141692 / 12), so3.rotation([0, 0, 1], -3.141592 / 2)),
[-0.03, -0.05, 0.05],
)
left_camera_center_xform = resource.get(
"left_camera_center.xform",
type="RigidTransform",
description="Left camera focal point",
default=left_camera_center_xform,
world="klampt_models/" + klampt_model_name,
frame=left_camera_link_name,
)
right_camera_center_xform = resource.get(
"right_camera_center.xform",
type="RigidTransform",
description="Right camera focal point",
default=right_camera_center_xform,
elif b[4] in 'CFIL':
x = -0.3
#if b[4] == 'H':
# y += 0.05
y += 0.4
if b[4] in 'ABC':
z += 1.50
elif b[4] in 'DEF':
#z += 1.50 - 6*0.0254
z += 1.50 - 9*0.0254
elif b[4] in 'GHI':
z += 1.50 - 18*0.0254
elif b[4] in 'JKL':
z += 1.50 - 29*0.0254
rdowntilt = downtilt*pi / 180
rhighdowntilt = (downtilt+10)*pi/180.0
vps[b+'_center'] = ( so3.mul(so3.from_axis_angle(([-1,0,0],-pi/2-rdowntilt)),
so3.from_axis_angle(([0,0,1],pi))),
[ x, y, z ])
vps[b+'_high'] = ( so3.mul(so3.from_axis_angle(([-1,0,0],-pi/2-rhighdowntilt)),
so3.from_axis_angle(([0,0,1],pi))),
[ x, y, z + 0.08 ])
print b, vps[b+'_center'][1], vps[b+'_high'][1]
json.dump(vps, open('vantage_point_xforms.json', 'w'), indent=4)
Rbase,tbase = world.robot(0).link(0).getParentTransform()
world.robot(0).link(0).setParentTransform(Rbase,(0,0,0.95))
world.robot(0).setConfig(world.robot(0).getConfig())
# Translate the shelf to be in front of the robot, and rotate it by 90 degrees
Trel = (so3.rotation((0,0,1),-math.pi/2),[1.2,0,0]) # desired shelf's xform
T = world.rigidObject(0).getTransform() # shelf model's xform
world.rigidObject(0).setTransform(*se3.mul(Trel,T)) # combine the two xforms
# Up until this point, the baxter model is extending its arms
# Load the resting configuration from klampt_models/baxter_rest.config
f = open(model_dir+'baxter_rest.config','r')
baxter_rest_config = loader.readVector(f.readline())
f.close()
world.robot(0).setConfig(baxter_rest_config)
# Add initial joint values to additional joints
n = world.robot(0).numLinks()
if len(baxter_rest_config) < n:
baxter_rest_config += [0.0]*(n-len(baxter_rest_config))
world.robot(0).setConfig(baxter_rest_config)
# Orient bin boxes correctly w.r.t. the shelf
ground_truth_shelf_xform = world.rigidObject(0).getTransform()
R = so3.mul(apc.Xto_Z,ground_truth_shelf_xform[0])
ground_truth_shelf_xform = (R, [1.2,0,0])
# run the visualizer
visualizer = MyGLViewer(world)
visualizer.run()
def keyboardfunc(self,c,x,y):
#Put your keyboard handler here
#the current example toggles simulation / movie mode
print c,"pressed"
if c.lower()=='q':
motion.robot.shutdown()
self.close()
elif c.lower()=='h':
self.print_help()
elif c.lower()=='o':
self.useRotation = not self.useRotation
print "Using rotation?",self.useRotation
elif c.lower()=='o':
self.localMotion = not self.localMotion
print "Local motion?",self.localMotion
elif c.lower()=='l':
print "Switching to left arm"
self.driveArm = 'l'
self.driveRot,self.drivePos = motion.robot.left_ee.commandedTransform()
elif c.lower()=='r':
print "Switching to right arm"
self.driveArm = 'r'
self.driveRot,self.drivePos = motion.robot.right_ee.commandedTransform()
elif c==' ':
motion.robot.stopMotion()
elif c=='X':
self.drivePos[0] += self.increment
self.updateCommand()
elif c=='x':
self.drivePos[0] -= self.increment
self.updateCommand()
elif c=='Y':
self.drivePos[1] += self.increment
self.updateCommand()
elif c=='y':
self.drivePos[1] -= self.increment
self.updateCommand()
elif c=='Z':
self.drivePos[2] += self.increment
self.updateCommand()
elif c=='z':
self.drivePos[2] -= self.increment
self.updateCommand()
elif c=='!':
R = so3.rotation([1,0,0],self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c=='1':
R = so3.rotation([1,0,0],-self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c=='@':
R = so3.rotation([0,1,0],self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c=='2':
R = so3.rotation([0,1,0],-self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c=='#':
R = so3.rotation([0,0,1],self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c=='3':
R = so3.rotation([0,0,1],-self.increment)
self.driveRot = so3.mul(R,self.driveRot)
self.updateCommand()
elif c==',' or c=='<':
self.baseCommand[2] += self.incrementang
self.updateBaseCommand()
elif c=='.' or c=='>':
self.baseCommand[2] -= self.incrementang
self.updateBaseCommand()
elif c=='p':
print "Baxter Total Config:"
print motion.robot.getKlamptSensedPosition()
print "=============================="
print "Arm Configuration ", self.driveArm
if( self.driveArm == 'r' ):
print motion.robot.right_limb.sensedPosition()[:7]
elif (self.driveArm == 'l'):
print motion.robot.left_limb.sensedPosition()[:7]
self.refresh()
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..."
import numpy, sys, os
from subprocess import call
import os.path
import subprocess
import uuid
if False:
T = numpy.eye(4)
from klampt import so3
if sys.argv[2] == 'crayola_64_ct':
T[:3,:3] = numpy.array(
so3.mul(
so3.rotation([1,0,0], 3.1415),
so3.rotation([0,0,1], 3.1415/4+3.1415)
)
).reshape((3,3))
T[:3,3] = (0.995, 0.27, 0.84)
elif sys.argv[2] == 'kong_duck_dog_toy':
T[:3,:3] = numpy.array(
# so3.mul(
# so3.rotation([1,0,0], 3.1415),
so3.rotation([0,0,1], -3.1415/4)
# )
).reshape((3,3))
T[:3,3] = (0.94, -0.05, 0.70)
else:
raise SystemExit
# print xform
tall_item = ItemInfo('tall_item')
tall_item.bmin = [-0.03,-0.05,-0.1]
tall_item.bmax = [0.03,0.05,0.1]
XtoZ = so3.from_axis_angle(([0,1,0],math.pi*0.5))
Xto_Z = so3.from_axis_angle(([0,1,0],-math.pi*0.5))
ZtoY = so3.from_axis_angle(([1,0,0],math.pi*0.5))
Zto_Y = so3.from_axis_angle(([1,0,0],-math.pi*0.5))
XYflip = so3.from_axis_angle(([0,0,1],math.pi))
XZflip = so3.from_axis_angle(([0,1,0],math.pi))
YZflip = so3.from_axis_angle(([1,0,0],math.pi))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0.02])))
tall_item.grasps.append(ItemGrasp((Zto_Y,[0,-0.04,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(Zto_Y,XYflip),[0,-0.04,0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.04,0])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.04,0.02])))
tall_item.grasps.append(ItemGrasp((ZtoY,[0,0.04,0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,-0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,-0.04])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0.02])))
tall_item.grasps.append(ItemGrasp((so3.mul(ZtoY,XYflip),[0,0.04,0.04])))
