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 load_apc_world():
"""Produces a world with only the Baxter, shelf, and ground plane in it."""
world = WorldModel()
#uncomment these lines and comment out the next 2 if you want to use the
#full Baxter model
#print "Loading full Baxter model (be patient, this will take a minute)..."
#world.loadElement(os.path.join(model_dir,"baxter.rob"))
print "Loading simplified Baxter model..."
world.loadElement(os.path.join(model_dir,baxter.klampt_model_name))
#print "Loading Kiva pod model..."
#world.loadElement(os.path.join(model_dir,"kiva_pod/meshes/pod_lowres.stl"))
print 'Loading north hall shelf...'
world.loadElement(os.path.join(model_dir,"north_shelf/mesh/shelf.stl"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
#shift the Baxter up a bit (95cm)
Rbase,tbase = world.robot(0).getLink(0).getParentTransform()
world.robot(0).getLink(0).setParentTransform(Rbase,(0,0,0.95))
world.robot(0).setConfig(world.robot(0).getConfig())
#translate pod to be in front of the robot, and rotate the pod by 90 degrees
reorient = ([1,0,0,0,0,1,0,-1,0],[0,0,0])
#kiva
#Trel = (so3.rotation((0,0,1),-math.pi/2),[1.3,0,0])
#north shelf
Trel = (so3.rotation((0,0,1),-3*math.pi/2),[1.,0,0])
Trel = se3.mul(Trel,(so3.rotation((1,0,0),-math.pi/2),[0,0,0]))
T = reorient
#world.terrain(0).geometry().transform(*Trel)
world.terrain(0).geometry().transform(*se3.mul(Trel,T))
#initialize the shelf xform for the visualizer and object
#xform initialization
global ground_truth_shelf_xform
ground_truth_shelf_xform = se3.mul(Trel,T)
#####################################test#################################################
# world.loadElement('../klampt_models/kiva_pod/meshes/pod_lowres.stl')
# reorient = ([1,0,0,0,0,1,0,-1,0],[0,0,0.01])
# #kiva
# Trel = (so3.rotation((0,0,1),-math.pi/2),[.3,0,0])
# T = reorient
# world.terrain(2).geometry().transform(*se3.mul(Trel,T))
##########################################################################################
return world
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 __init__(self):
GLRealtimeProgram.__init__(self, "GLRotationTest")
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print "Angle between"
print so3.matrix(Ra)
print "and"
print so3.matrix(Rb)
print "is", so3.distance(Ra, Rb)
self.Ta = (Ra, (-1, 0, 1))
self.Tb = (Rb, (1, 0, 1))
self.T = self.Ta
self.clearColor = [1, 1, 1, 0]
def __init__(self):
GLRealtimeProgram.__init__(self,"GLRotationTest")
Ra = so3.rotation((0,1,0),math.pi*-0.9)
Rb = so3.rotation((0,1,0),math.pi*0.9)
print "Angle between"
print so3.matrix(Ra)
print "and"
print so3.matrix(Rb)
print "is",so3.distance(Ra,Rb)
self.Ta = (Ra,(-1,0,1))
self.Tb = (Rb,(1,0,1))
self.T = self.Ta
self.clearColor = [1,1,1,0]
def load_apc_world():
"""Produces a world with only the Baxter, shelf, and ground plane in it."""
world = robotsim.WorldModel()
#uncomment these lines and comment out the next 2 if you want to use the
#full Baxter model
# print "Loading full Baxter model (be patient, this will take a minute)..."
# world.loadElement(os.path.join(model_dir,"baxter.rob"))
print "Loading simplified Baxter model..."
world.loadElement(os.path.join(model_dir,"baxter_with_parallel_gripper_col.rob"))
print "Loading Kiva pod model..."
world.loadElement(os.path.join(model_dir,"kiva_pod/meshes/pod_lowres.stl"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
#shift the Baxter up a bit (95cm)
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 pod to be in front of the robot, and rotate the pod by 90 degrees
reorient = ([1,0,0,0,0,1,0,-1,0],[0,0,0.01])
Trel = (so3.rotation((0,0,1),-math.pi/2),[1.4,0,0])
T = reorient
world.terrain(0).geometry().transform(*se3.mul(Trel,T))
# print se3.mul(Trel,T)
#initialize the shelf xform for the visualizer and object
#xform initialization
global ground_truth_shelf_xform
ground_truth_shelf_xform = se3.mul(Trel,T)
return world
def move_to_order_bin(self,object):
"""Sets the robot's configuration so the gripper is over the order bin
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
arms = { 'right': (self.right_gripper_link, right_gripper_center_xform[1]),
'left': (self.left_gripper_link, left_gripper_center_xform[1])
}
# use the active limb
try:
(link, gripper_center) = arms[self.active_limb]
except KeyError:
return False
# move the gripper to order bin top and point opposite the world's z axis
goal = ik.objective(link, R=so3.rotation([1,0,0], math.pi), t=self.knowledge.order_bin_top())
# try with 50 attempts
# randomize after each failed attempt
for attempts in range(50):
# solve the inverse kinematics with 5 cm accuracy (the bin is large)
if ik.solve(goal, tol=0.05):
self.config = goal.robot.getConfig()
return True
else:
# randomize this limb
randomize_links(goal.robot, arm_link_names[self.active_limb])
return False
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 main():
"""The main loop that loads the models and starts the OpenGL visualizer"""
world = WorldModel()
#uncomment these lines and comment out the next 2 if you want to use the
#full Baxter model
#print "Loading full Baxter model (be patient, this will take a minute)..."
#world.loadElement(os.path.join(model_dir,"baxter.rob"))
print "Loading simplified Baxter model..."
world.loadElement(os.path.join(model_dir,"baxter_col.rob"))
print "Loading Kiva pod model..."
world.loadElement(os.path.join(model_dir,"kiva_pod/model.obj"))
print "Loading plane model..."
world.loadElement(os.path.join(model_dir,"plane.env"))
#shift the Baxter up a bit (95cm)
Rbase,tbase = world.robot(0).getLink(0).getParentTransform()
world.robot(0).getLink(0).setParentTransform(Rbase,(0,0,0.95))
world.robot(0).setConfig(world.robot(0).getConfig())
#translate pod to be in front of the robot, and rotate the pod by 90 degrees
Trel = (so3.rotation((0,0,1),-math.pi/2),[1.2,0,0])
T = world.rigidObject(0).getTransform()
world.rigidObject(0).setTransform(*se3.mul(Trel,T))
#load the resting configuration from klampt_models/baxter_rest.config
global 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)
#run the visualizer
visualizer = MyGLViewer(world)
visualizer.run()
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 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 spawn_item_test(self):
self.world.loadElement('../klampt_models/kiva_pod/meshes/pod_lowres.stl')
reorient = ([1,0,0,0,0,1,0,-1,0],[0,0,0.01])
#kiva
Trel = (so3.rotation((0,0,1),-math.pi/2),[.3,0,0])
T = reorient
self.world.terrain(2).geometry().transform(*se3.mul(Trel,T))
self.planner.updateWorld(self.world)
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 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 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 setupWorld():
world = WorldModel()
print "Loading full Baxter model (be patient, this will take a minute)..."
world.loadElement(os.path.join(KLAMPT_MODELS_DIR, "baxter.rob"))
# print "Loading simplified Baxter model..."
# world.loadElement(os.path.join(KLAMPT_MODELS_DIR,"baxter_col.rob"))
print "Loading Kiva pod model..."
world.loadElement(os.path.join(KLAMPT_MODELS_DIR, "kiva_pod/model.obj"))
print "Loading plane model..."
world.loadElement(os.path.join(KLAMPT_MODELS_DIR, "plane.env"))
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())
Trel = (so3.rotation((0, 0, 1), -math.pi / 2), SHELF_MODEL_XFORM)
T = world.rigidObject(0).getTransform()
world.rigidObject(0).setTransform(*se3.mul(Trel, T))
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
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.
"""
print
print 'Trying to move to ' + bin_name
world_goal_xyz = self.knowledge.bin_vantage_point(bin_name)
#transform from world to shelf by rotating 180 degrees
rot = so3.rotation([0,0,1],math.pi);
rot_t = [rot,[0,0,0]];
T = se3.mul(rot_t,self.knowledge.shelf_xform);
cnt = 0;
while(cnt < 1000):
#pick a random starting config for the left link and try to solve the ik problem
config = self.robot.getConfig()
for ind in self.left_arm_indices:
config[ind] = random.uniform(0,3);
self.robot.setConfig(config);
goal = ik.objective(self.left_camera_link,R=T[0],t=world_goal_xyz)
if ik.solve(goal):
self.active_limb = 'Left'
self.config = self.robot.getConfig()
return True
cnt = cnt + 1
return False
data = json.load(open(path))
setattr(kb, k, data)
r = PerceptionInterface(kb)
c = ControlInterface(robot, kb)
p = PlanningInterface(kb)
kb.shelf_xform = r.localizeShelf()
plan = p.planMoveToInitialPose()
c.execute(plan[0], sleep)
kb.robot_state = RobotState(robot)
sleep(5)
plan = p.planMoveToVantagePoint('bin_A', 'bin_A_center')
c.execute(plan[0], sleep)
kb.robot_state = RobotState(robot)
sleep(5)
kb.object_xforms['crayola_64_ct'] = (so3.rotation([0,0,1], 3.1415/4+3.1415), (1.0, 0.15, 0.70))
plan = p.planGraspObjectInBin('bin_A', 'crayola_64_ct')
c.execute(plan[0], sleep)
# print 'going to control interface'
# print 'made control interface'
print robot.isMoving()
print 'waiting...'
sleep(100)
elif cmd == 'move' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], [ x, y, z ])
elif cmd == 'mover' and len(args) <= 3:
(x, y, z) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (xform[0], vectorops.add(xform[1], [ x, y, z ]))
elif cmd == 'rot' and len(args) <= 3:
(rx, ry, rz) = map(lambda a: float(a) * 3.141592/180, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (
reduce(so3.mul, [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
)
elif cmd == 'rotl' and len(args) <= 3:
(rx, ry, rz) = map(float, args) + ([0] * (3 - len(args)))
xform = kb.object_xforms[kb.target_object]
kb.object_xforms[kb.target_object] = (
reduce(so3.mul, [ xform[0] ] + [ so3.rotation(v, a) for (v, a) in zip([ [1,0,0], [0,1,0], [0,0,1] ], [ rx, ry, rz ]) ]),
xform[1]
)
elif cmd == 'check':
while not master.manager.control.update().done: sleep(0.1)
while not master.manager.control.update().done: sleep(0.1)
robot.setConfig(kb.robot_state.sensed_config)
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
# ),
# [ 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)
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)
[ 0, 0, 1 ]]),
[ 1.03, -0.055, -0.9046 ]
)
# transform the shelf point cloud
shelf_cloud = shelf_cloud.dot(shelf_xform[0]) + shelf_xform[1]
# load the camera point cloud
mark = time()
camera_cloud_color = pcd.parse(open(CAMERA_CLOUD_PATH))[1]
camera_cloud = numpy.array([ p[:3] for p in camera_cloud_color ])
logging.info('loaded {} camera points in {:.3f}s'.format(camera_cloud.shape[0], time() - mark))
# load the camera xform
gripper_xform = json.load(open(CAMERA_XFORM_PATH))[CAMERA_XFORM_INDEX]['xforms']['left_gripper']
camera_xform = se3.mul(gripper_xform, ( so3.rotation([0,0,1], -3.141592/2), [ -0.10, 0, 0 ] ))
# transform the camera point cloud
camera_cloud = camera_cloud.dot(numpy.array(camera_xform[0]).reshape((3, 3))) + camera_xform[1]
# load the object model point cloud
mark = time()
model_cloud_color = pcd.parse(open(OBJECT_MODEL_PATH))[1]
model_cloud = numpy.array([ p[:3] for p in model_cloud_color ])
logging.info('loaded {} model points in {:.3f}s'.format(model_cloud.shape[0], time() - mark))
# set up the bin bounds
bin_bounds = json.load(open(SHELF_DIMS_PATH))[BIN_NAME]
# for (i, p) in enumerate(camera_cloud_color):
# p[:3] = camera_cloud[i]
type="RigidTransform",
description="Left gripper center",
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",
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 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()
# NOTE: world.loadRigidObject(~) works too because the shelf model is a rigid object
# and loadElement automatically detects the object type
print "\n<Loading Kiva pod model...>"
world.loadElement(os.path.join(model_dir,"kiva_pod/model.obj"))
# Load the floor plane
print "\n<Loading plane model...>"
world.loadElement(os.path.join(model_dir,"plane.env"))
# Shift the Baxter up a bit (95cm)
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)