def check_collision_free_with_object(self,limb,object,grasp):
"""Checks whether the given limb, holding an object at the given
grasp, is collision free at the robot's current configuration"""
objToGripperXForm = se3.mul(baxter.left_gripper_center_xform,se3.inv(grasp))
#assume robot configuration is updated
if limb=='left':
gripperLink = self.robot.getLink(baxter.left_gripper_link_name)
else:
gripperLink = self.robot.getLink(baxter.right_gripper_link_name)
if not isinstance(object,list):
if object==None:
object = []
else:
object = [object]
Tgripper = gripperLink.getTransform()
Tobj = se3.mul(Tgripper,objToGripperXForm)
for o in object:
o.setTransform(*Tobj)
excludeids = [ o.getID() for o in object]
if not self.check_collision_free(limb,exclude=excludeids):
#print 'Limb is not collision free'
return False
for t in xrange(self.world.numRigidObjects()):
other = self.world.rigidObject(t)
if(other.getID() not in excludeids):
if any(other.geometry().collides(o.geometry()) for o in object):
#visualization.debug(self.world)
#print "Held object-shelf collision"
return False
return True
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 drawGL(self,q): x = self.getSensedValue(q) xdes = self.xdes if self.baseLinkNo >= 0: Tb = self.baseLink.getTransform() if self.taskType == "position": x = se3.apply(Tb,x) xdes = se3.apply(Tb,xdes) elif self.taskType == "po": x = se3.mul(Tb,x) xdes = se3.mul(Tb,xdes) glPointSize(6) glEnable(GL_POINT_SMOOTH) glBegin(GL_POINTS) if self.taskType == "position": glColor3f(1,0,0) #red glVertex3fv(xdes) glColor3f(1,0.5,0) #orange glVertex3fv(x) elif self.taskType == "po": glColor3f(1,0,0) #red glVertex3fv(xdes[1]) glColor3f(1,0.5,0) #orange glVertex3fv(x[1]) glEnd()
def drawGL(self, q):
x = self.getSensedValue(q)
xdes = self.xdes
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
if self.taskType == "position":
x = se3.apply(Tb, x)
xdes = se3.apply(Tb, xdes)
elif self.taskType == "po":
x = se3.mul(Tb, x)
xdes = se3.mul(Tb, xdes)
glPointSize(6)
glEnable(GL_POINT_SMOOTH)
glBegin(GL_POINTS)
if self.taskType == "position":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes)
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x)
elif self.taskType == "po":
glColor3f(1, 0, 0) #red
glVertex3fv(xdes[1])
glColor3f(1, 0.5, 0) #orange
glVertex3fv(x[1])
glEnd()
def grasp_xforms(self,object):
if object.xform == None: return None
res = []
for g in object.info.grasps:
grasp_xform_world = se3.mul(object.xform,g.grasp_xform)
pregrasp_xform_world = se3.mul(object.xform,g.pregrasp)
res.append((g,grasp_xform_world,pregrasp_xform_world))
return res
def __init__(self):
threading.Thread.__init__(self)
self.writer = HapticCartesianTaskService(system_state_addr)
self.reader = HapticWidgetUIService(haptic_service_addr)
getter1 = TopicListener(system_state_addr,'.robot.endEffectors.0.dest_xform')
getter2 = TopicListener(system_state_addr,'.robot.endEffectors.1.dest_xform')
self.reader.widgetGetters = [lambda :se3.mul(getter1.get(),endEffectorLocalTransforms[0]),
lambda :se3.mul(getter2.get(),endEffectorLocalTransforms[1])]
def move_to_grasp_object(self,object):
# return True
"""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.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
#TODO: put my code here
if self.active_limb=="LEFT":
first_gripper = self.left_gripper_link
second_gripper = self.right_gripper_link
first_offset = left_gripper_center_xform
second_offset = right_gripper_center_xform
first_limb = "LEFT"
second_limb = "RIGHT"
elif self.active_limb=="RIGHT":
first_gripper = self.right_gripper_link
second_gripper = self.left_gripper_link
first_offset = right_gripper_center_xform
second_offset = left_gripper_center_xform
first_limb = "RIGHT"
second_limb = "LEFT"
else:
print "Grasping... Unrecognized active limb"
return False
item_to_world = object.xform
for grasp in object.info.grasps:
gripper_to_item = grasp.grasp_xform
for _ in xrange(100):
self.robot.setConfig(self.random_init())
(R_net, t_net) = se3.mul(item_to_world, se3.mul(gripper_to_item, se3.inv(first_offset)))
goal = ik.objective(first_gripper, R=R_net, t=t_net)
res = ik.solve(goal)
if res==True:
self.config = self.robot.getConfig()
#print "Grasp succeeded"
self.active_limb=first_limb
return True
else:
pass
#self.config = self.robot.getConfig()
self.robot.setConfig(self.random_init())
(R_net, t_net) = se3.mul(item_to_world, se3.mul(gripper_to_item, se3.inv(second_offset)))
goal = ik.objective(second_gripper, R=R_net, t=t_net)
res = ik.solve(goal)
if res==True:
self.config = self.robot.getConfig()
#print "Grasp succeeded"
self.active_limb=second_limb
return True
else:
#self.config = self.robot.getConfig()
pass
'''
def graspedObjectTransform(robot, hand, qrobot0, Tobj0, qrobot):
"""Given initial robot configuration qrobot0 and object transform Tobj0,
returns the object transformation corresponding to new configuration
qrobot assuming the object is rigidly attached to the hand"""
robot.setConfig(qrobot0)
Thand0 = robot.link(hand.link).getTransform()
Tgrasp = se3.mul(se3.inv(Thand0), Tobj0)
robot.setConfig(qrobot)
Thand = robot.link(hand.link).getTransform()
return se3.mul(Thand, Tgrasp)
def graspedObjectTransform(robot,hand,qrobot0,Tobj0,qrobot):
"""Given initial robot configuration qrobot0 and object transform Tobj0,
returns the object transformation corresponding to new configuration
qrobot assuming the object is rigidly attached to the hand"""
robot.setConfig(qrobot0)
Thand0 = robot.link(hand.link).getTransform()
Tgrasp = se3.mul(se3.inv(Thand0),Tobj0)
robot.setConfig(qrobot)
Thand = robot.link(hand.link).getTransform()
return se3.mul(Thand,Tgrasp)
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 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.
"""
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
qcmd = self.controller.getCommandedConfig()
#phase 1: init IK from the commanded config, search among grasps
for (grasp,gxform) in grasps:
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
self.robot.setConfig(qcmd)
if ik.solve(goal):
#self.controller.setMilestone(self.robot.getConfig())
#self.active_grasp = grasp
no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb) ###
if no_collision:
self.active_grasp = grasp
self.controller.setMilestone(self.robot.getConfig())
return True
#Phase 2: that didn't work, now try random sampling
for i in range(100):
#pick a config at random
self.randomize_limb_position(self.active_limb)
#pick a grasp at random
(grasp,gxform) = random.choice(grasps)
if self.active_limb == 'left':
Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
if ik.solve(goal):
# self.active_grasp = grasp
#TODO: plan a path
no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb)
if no_collision:
self.active_grasp = grasp
self.controller.setMilestone(self.robot.getConfig())
return True
return False
def __init__(self, knowledge_base):
PerceptionInterface.__init__(self, knowledge_base)
world = WorldModel()
robot = world.loadRobot(os.path.join('klampt_models', baxter.klampt_model_name))
robot.setConfig(self.knowledge_base.robot_state.sensed_config)
self.base_xform = robot.getLink('{}_gripper'.format(self.knowledge_base.active_limb)).getTransform()
if self.knowledge_base.active_limb == 'left':
self.camera_xform = se3.mul(self.base_xform, self.knowledge_base.left_camera_offset_xform)
else:
self.camera_xform = se3.mul(self.base_xform, self.knowledge_base.right_camera_offset_xform)
def set_in_bin_xform(self,shelf_xform,ux,uy,theta):
"""A utility convenience function for setting up virtual shelves.
Sets self.xform assuming the object is resting in the bin
with its center's translational parameters (ux,uy) in the
range [0,1]x[0,1] and orientation theta about the z axis.
ux goes from left to right and uy goes from front to back."""
global bin_bounds
bmin,bmax = bin_bounds[self.bin_name]
tlocal = [bmin[0]+ux*(bmax[0]-bmin[0]),bmin[1]-self.info.bmin[2],bmax[2]+uy*(bmin[2]-bmax[2])]
Rlocal = so3.from_axis_angle(([0,1,0],theta))
toShelfCoords = (so3.from_axis_angle(([1,0,0],math.pi/2)),[0,0,0])
self.xform = se3.mul(shelf_xform,se3.mul((Rlocal,tlocal),toShelfCoords))
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 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:
objGrasps = object.info.grasps
return [se3.mul(object.xform,grasp.grasp_xform) for grasp in objGrasps]
def objectTransform(self, x):
"""Given an arm configuration x, returns the object transformation"""
q = self.q0[:]
for i, xi in zip(self.hand.armIndices, x):
q[i] = xi
self.robot.setConfig(q)
Thand = self.robot.link(self.hand.link).getTransform()
return se3.mul(Thand, self.Tgrasp)
def objectTransform(self,x):
"""Given an arm configuration x, returns the object transformation"""
q = self.q0[:]
for i,xi in zip(self.hand.armIndices,x):
q[i] = xi
self.robot.setConfig(q)
Thand = self.robot.link(self.hand.link).getTransform()
return se3.mul(Thand,self.Tgrasp)
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 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.
If successful, changes self.config and returns True.
Otherwise, does not change self.config and returns False.
"""
print
print 'Trying to grasp ' + object.info.name
grasp_transforms = self.knowledge.grasp_xforms(object);
#try all of the grasps and stop if one is successful
for grasp_t in grasp_transforms:
#get the world point goal which is offset from the grasp_t
rot_xform = se3.mul(grasp_t,se3.inv(left_gripper_center_xform))
#rot = so3.rotation([0,0,1],math.pi);
#rot_t = [rot,[0,0,0]];
world_goal_xyz = rot_xform[1];
cnt = 0;
while(cnt < 100):
#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_gripper_link,R=se3.mul(grasp_t,se3.identity())[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
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.
if not object.is_localized():
return None
# compose the object and graph transforms
return [ se3.mul(object.xform, g.grasp_xform) for g in object.info.grasps ]
def localizeSpecificObject_old(self, bin, object):
(xform, cloud) = self._localize_task('SpecificObject', [ bin, object ])
# apply the camera transform
xform = se3.mul(self.camera_xform, xform)
cloud = map(lambda p: se3.apply(self.camera_xform, p[:3]) + [ p[3] ], cloud)
debug_cloud(cloud, [ self.base_xform, self.camera_xform, xform ])
return (xform, cloud)
def preGraspAction(self,object):
grasps = self.knowledge.grasp_xforms(object)
qmin,qmax = self.robot.getJointLimits()
qcmd = self.controller.getCommandedConfig()
isPreGrasp = False
#pregrasp - allign the gripper with the grasp
for i in range(1000):
#pick a config at random
self.randomize_limb_position(self.active_limb)
#pick a grasp at random
(grasp,gxform,pregrasp) = random.choice(grasps)
if self.active_limb == 'left':
Tg = se3.mul(pregrasp,se3.inv(left_gripper_center_xform))
goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
else:
Tg = se3.mul(pregrasp,se3.inv(right_gripper_center_xform))
goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
if ik.solve(goal):
if(self.check_arms_for_collisions(True)):
#TODO: plan a path
finalConfig = self.robot.getConfig()
#self.controller.setMilestone(self.robot.getConfig())
if self.active_limb == 'left':
path = self.planner.plan(qcmd,finalConfig)
else:
path = self.planner.plan(qcmd,finalConfig,order=['right','left'])
if path == None:
continue
else:
isPreGrasp = True
for pp in path:
self.controller.setMilestone(pp)
self.waitForMove()
break;
if(not isPreGrasp):
return False
return True
def icp(object,scene): #return se3.identity() """Computes a rotation and translation of the object to the scene using the ICP algorithm. Returns the transform (R,t) in Klamp't se3 format. """ kd = KDTree(scene) print "kd tree done" #TODO: implement me # Sample both maps to make the closest point computations faster # Compute the minimum distance between the points # Reject the outliers, for example, using a threshold # Compute the R and t matrices. The procedure can be similar as the one for the # 2D images. #ret = se3.identity() #return ret #ret[1][2]+=0.2 #ret[1][1]+=0.3 #return ret sampled = random.sample(object, 1000) threshold = 0.75 net = se3.identity() #net =[net[0], (0.2,0.05,0)] #return net sampled = [se3.apply(net, o) for o in sampled] #print object[0] for _ in xrange(10): print "start finding correspondances" correspondings = [] for i in sampled: try: (dist, idx) = kd.query(i) except: print i raise Exception() correspondings.append((i, scene[idx], dist)) correspondings.sort(key=lambda x: x[2]) correspondings = correspondings[0:int(len(correspondings)*threshold)] #print "\n".join(map(str,correspondings)) print "done finding correspondances" object_points = [i[0] for i in correspondings] scene_points = [i[1] for i in correspondings] (R,t) = one_round_icp(scene_points, object_points, 'list') #(R,t) = one_round_icp(object_points, scene_points, 'matrix') sampled = [se3.apply((R,t),i) for i in sampled] net = se3.mul((R,t), net) #print R,t #print net return net
def update(self):
self.objXform = self.object.getTransform()
self.robotXform = self.robot.link(6).getTransform()
self.relativeXform = se3.mul(se3.inv(self.objXform), self.robotXform)
# for sanity check (checked that the relative transform output is correct)
# print "obj", self.objXform[1], "robot", self.robotXform[1], "relative", self.relativeXform[1]
self.robotConfig = []
for i in self.validLinks:
self.robotConfig.append(self.robot.getConfig()[i])
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.
l = [x.grasp_xform for x in object.info.grasps]
r = [se3.mul(object.xform,x) for x in l]
#if len(r) > 11:
# return [r[11]]
#else:
# return None
return r
def grasp_xforms(self,object):
if object.xform == None: return None
res = []
for g in object.info.grasps:
# NOTE: g.grasp_xform: the transformation of the gripper fingers
# relative to the object's local frame
# object.xform: the transformation of the object center
# relative to the world frame
# grasp_xform_world: the transformation of the gripper fingers
# relative to the world frame
grasp_xform_world = se3.mul(object.xform,g.grasp_xform)
res.append((g,grasp_xform_world))
return res
def grasp_xforms(self,object):
#TODO: remove me
if object.xform == None: return None
res = []
for g in object.info.grasps:
grasp_xform_world = se3.mul(object.xform,g.grasp_xform)
res.append(grasp_xform_world)
return res
#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:
return None
def getCameraToWorldXform(linkName=None, index=0):
'''
get current transformation (R, t) of camera in world frame.
'''
global CAMERA_TRANSFORM, SIMWORLD
if len(CAMERA_TRANSFORM) < index:
print 'Error, camera index does not exist'
return ([0,0,0,0,0,0,0,0,0],[0,0,0])
if linkName != None:
return se3.mul(SIMWORLD.robot(0).link(linkName).getTransform(), CAMERA_TRANSFORM[index])
else:
return CAMERA_TRANSFORM[index]
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.
possibleGrasps = object.info.grasps
objectToWorldXform = object.xform
if objectToWorldXform == None:
return None
possibleGraspsWorld = list()
for grasp in possibleGrasps:
possibleGraspsWorld.append(se3.mul(objectToWorldXform, grasp.grasp_xform))
return possibleGraspsWorld
def check_collision_free_with_object(self,limb,objectGeom,grasp):
"""Checks whether the given limb, holding an object at the given
grasp, is collision free at the robot's current configuration"""
if not self.check_collision_free(limb):
return False
objToGripperXForm = se3.mul(left_gripper_center_xform,se3.inv(grasp.grasp_xform))
#assume robot configuration is updated
if limb=='left':
gripperLink = self.robot.link(left_gripper_link_name)
else:
gripperLink = self.robot.link(right_gripper_link_name)
Tgripper = gripperLink.getTransform();
Tobj = se3.mul(Tgripper,objToGripperXForm)
objectGeom.setCurrentTransform(*Tobj)
for t in xrange(self.world.numTerrains()):
if self.world.terrain(t).geometry().collides(objectGeom):
# print "Held object-shelf collision"
return False
for o in self.dynamic_objects:
if o.info.geometry.collides(objectGeom):
# print "Held object-object collision"
return False
return True
def __init__(self,globals,hand,object):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
self.object = object
#setup initial object-robot transform
Thand0 = self.robot.link(hand.link).getTransform()
Tobj0 = object.getTransform()
self.Tgrasp = se3.mul(se3.inv(Thand0),Tobj0)
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
def __init__(self, globals, hand, object):
CSpace.__init__(self)
self.globals = globals
self.robot = globals.robot
self.hand = hand
self.object = object
#setup initial object-robot transform
Thand0 = self.robot.link(hand.link).getTransform()
Tobj0 = object.getTransform()
self.Tgrasp = se3.mul(se3.inv(Thand0), Tobj0)
#initial whole-body configuratoin
self.q0 = self.robot.getConfig()
#setup CSpace sampling range
qlimits = zip(*self.robot.getJointLimits())
self.bound = [qlimits[i] for i in self.hand.armIndices]
#setup CSpace edge checking epsilon
self.eps = 1e-2
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 getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv, T)
if self.taskType == 'po':
x = (T[0], se3.apply(T, self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T, self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType " + self.taskType)
return x
def getSensedValue(self, q):
"""Get link x, which is rotation matrix and/or translation
"""
self.robot.setConfig(q)
T = self.link.getTransform()
#check if relative transform task, modify T to local transform
if self.baseLinkNo >= 0:
Tb = self.baseLink.getTransform()
Tbinv = se3.inv(Tb)
T = se3.mul(Tbinv,T)
if self.taskType == 'po':
x = (T[0],se3.apply(T,self.localPosition))
elif self.taskType == 'position':
x = se3.apply(T,self.localPosition)
elif self.taskType == 'orientation':
x = T[0]
else:
raise ValueError("Invalid taskType "+self.taskType)
return x
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
"""
import sys
import random
robot_fn = "../Klampt/data/robots/baxter_col.rob"
world = WorldModel()
if not world.readFile(robot_fn):
exit(1)
robot = world.robot(0)
camera_obs_link = "head_camera"
marker_obs_link = "left_gripper"
lc = robot.link(camera_obs_link)
lm = robot.link(marker_obs_link)
pc = robot.link(lc.getParent())
pm = robot.link(lm.getParent())
Tc0 = se3.mul(se3.inv(pc.getTransform()),lc.getTransform())
Tm0 = se3.mul(se3.inv(pm.getTransform()),lm.getTransform())
print "True camera transform",Tc0
print "True marker transform",Tm0
print
camera_link = pc.getName()
marker_link = pm.getName()
#generate synthetic data, corrupted with joint encoder and sensor measurement errors
qmin,qmax = robot.getJointLimits()
numObs = 10
jointEncoderError = 1e-5
sensorErrorRads = 1e-2
sensorErrorMeters = 2e-3
trueCalibrationConfigs = []
calibrationConfigs = []
def calibrate_xform_camera(camera_link_transforms,
marker_link_transforms,
marker_observations,
marker_ids,
observation_relative_errors=None,
camera_initial_guess=None,
marker_initial_guess=None,
regularizationFactor=0,
maxIters=100,
tolerance=1e-7):
"""Single camera calibration function for a camera and markers on some
set of rigid bodies.
Given body transforms and a list of estimated calibration marker observations
in the camera frame, estimates both the camera transform relative to the
camera link as well as the marker transforms relative to their links.
M: is the set of m markers. By default there is at most one marker per link.
Markers can either be point markers (e.g., a mocap ball), or transform
markers (e.g., an AR tag or checkerboard pattern).
O: is the set of n observations, consisting of a reading (Tc_i,Tm_i,o_i,l_i)
where Tc_i is the camera link's transform, Tm_i is the marker link's
transform, o_i is the reading which consists of either a point or transform
estimate in the camera frame, and l_i is the ID of the marker (by default,
just its link)
Output: a tuple (err,Tc,marker_dict) where err is the norm of the
reconstruction residual, Tc is the estimated camera transform relative to the
camera's link, and marker_dict is a dict mapping each marker id to its
estimated position or transform on the marker's link.
Arguments:
- camera_link: an integer index or a RobotModelLink instance on which
the camera lies.
- calibration_configs: a list of the RobotModel configurations q_1,...,q_n
that generated the marker_observations list.
- marker_observations: a list of estimated positions or transformations
of calibration markers o_1,...,o_n, given in the camera's reference
frame (z forward, x right, y down).
If o_i is a 3-vector, the marker is considered to be a point marker.
If a se3 element (R,t) is given, the marker is considered to be
a transform marker. You may not mix point and transform observations
for a single marker ID.
- marker_ids: a list of marker ID #'s l_1,...,l_n corresponding to
each observation, e.g., the link index on which each marker lies.
- observation_relative_errors: if you have an idea of the magnitude of
each observation error, it can be placed into this list. Must be
a list of n floats, 3-lists (point markers), or 6-lists (transform
markers). By default, errors will be set proportionally to the
observed distance between the camera and marker.
- camera_initial_guess: if not None, an initial guess for the camera transform
- marker_initial_guess: if not None, a dictionary containing initial guesses
for the marker transforms
- regularizationFactor: if nonzero, the optimization penalizes deviation
of the estimated camera transform and marker transforms from zero
proportionally to this factor.
- maxIters: maximum number of iterations for optimization.
- tolerance: optimization convergence tolerance. Stops when the change of
estimates falls below this threshold
"""
if len(camera_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as camera transforms")
if len(marker_link_transforms) != len(marker_ids):
raise ValueError("Must provide the same number of marker IDs as marker transforms")
if len(marker_observations) != len(marker_ids):
raise ValueError("Must provide the same number of marker observations as marker transforms")
#get all unique marker ids
marker_id_list = list(set(marker_ids))
#detect marker types
marker_types = dict((v,None) for v in marker_id_list)
for i,(obs,id) in enumerate(zip(marker_observations,marker_ids)):
if len(obs)==3:
if marker_types[id] == 't':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 'p'
elif len(obs)==2 and len(obs[0])==9 and len(obs[1])==3:
if marker_types[id] == 'p':
raise ValueError("Provided both point and transform observations for observation #%d, id %s\n"%(i,str(id)))
marker_types[id] = 't'
else:
raise ValueError("Invalid observation for observation #%d, id %s\n"%(i,str(id)))
n = len(marker_observations)
m = len(marker_id_list)
#get all the observation weights
observation_weights = []
if observation_relative_errors is None:
#default weights: proportional to distance
for obs in marker_observations:
if len(obs) == 3:
observation_weights.append(1.0/vectorops.norm(obs))
else:
observation_weights.append(1.0/vectorops.norm(obs[1]))
observation_weights = [1.0]*len(observation_weights)
else:
if len(observation_relative_errors) != n:
raise ValueError("Invalid length of observation errors")
for err in observation_relative_errors:
if hasattr(err,'__iter__'):
observation_weights.append([1.0/v for v in err])
else:
observation_weights.append(1.0/err)
#initial guesses
if camera_initial_guess == None:
camera_initial_guess = se3.identity()
if any(v == 't' for v in marker_types.itervalues()):
#estimate camera rotation from point estimates because rotations are more prone to initialization failures
point_observations = []
marker_point_rel = []
for i,obs in enumerate(marker_observations):
if len(obs)==2:
point_observations.append(obs[1])
else:
point_observations.append(obs)
marker_point_rel.append(se3.mul(se3.inv(camera_link_transforms[i]),marker_link_transforms[i])[1])
camera_initial_guess = (point_fit_rotation_3d(point_observations,marker_point_rel),[0.0]*3)
print "Estimated camera rotation from points:",camera_initial_guess
if marker_initial_guess == None:
marker_initial_guess = dict((l,(se3.identity() if marker_types[l]=='t' else [0.0]*3)) for l in marker_id_list)
else:
marker_initial_guess = marker_initial_guess.copy()
for l in marker_id_list:
if l not in marker_initial_guess:
marker_initial_guess[l] = (se3.identity() if marker_types[l]=='t' else [0.0]*3)
camera_transform = camera_initial_guess
marker_transforms = marker_initial_guess.copy()
if DO_VISUALIZATION:
rgroup = coordinates.addGroup("calibration")
rgroup.addFrame("camera link",worldCoordinates=camera_link_transforms[-1])
rgroup.addFrame("marker link",worldCoordinates=marker_link_transforms[-1])
rgroup.addFrame("camera estimate",parent="camera link",relativeCoordinates=camera_transform)
rgroup.addFrame("marker estimate",parent="marker link",relativeCoordinates=marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.addFrame("obs"+str(i)+" estimate",parent="camera estimate",relativeCoordinates=obs)
vis.add("coordinates",rgroup)
vis.dialog()
point_observations_only = all(marker_types[marker] == 'p' for marker in marker_id_list)
xform_observations_only = all(marker_types[marker] == 't' for marker in marker_id_list)
if not point_observations_only and not xform_observations_only:
raise NotImplementedError("Can't calibrate camera from mixed point/transform markers yet")
for iters in range(maxIters):
#attempt to minimize the error on the following over all observations i
#camera_link_transform(q_i)*camera_transform*observation_i = marker_link_transform(l_i,q_i)*marker_transform(l_i)
#first, we'll assume the camera transform is fixed and then optimize the marker transforms.
#then, we'll assume the marker transforms are fixed and then optimize the camera transform.
#finally we'll check the error to detect convergence
#1. Estimate marker transforms from current camera transform
new_marker_transforms = dict((l,(TransformStats(zero=marker_initial_guess[l],prior=regularizationFactor) if marker_types[l]=='t' else VectorStats(value,zero=[0.0]*3,prior=RegularizationFactor))) for l in marker_id_list)
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tmlink),se3.mul(Tclink,camera_transform))
if marker_types[marker] == 't':
estimate = se3.mul(Trel,obs)
else:
estimate = se3.apply(Trel,obs)
new_marker_transforms[marker].add(estimate,observation_weights[i])
print "ITERATION",iters
#print " ESTIMATED MARKER TRANSFORMS:",dict((k,v.average) for (k,v) in new_marker_transforms.iteritems())
#2. Estimate camera transform from current marker transforms
new_camera_transform = TransformStats(zero=camera_initial_guess,prior=regularizationFactor)
if point_observations_only:
#TODO: weighted point fitting
relative_points = []
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
pRel = se3.apply(se3.inv(Tclink),se3.apply(Tmlink,new_marker_transforms[marker].average))
relative_points.append(pRel)
new_camera_transform.add(point_fit_xform_3d(marker_observations,relative_points),sum(observation_weights))
else:
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Trel = se3.mul(se3.inv(Tclink),se3.mul(Tmlink,new_marker_transforms[marker].average))
estimate = se3.mul(Trel,se3.inv(obs))
new_camera_transform.add(estimate,observation_weights[i])
#print " ESTIMATED CAMERA TRANSFORMS:",new_camera_transform.average
#3. compute difference between last and current estimates
diff = 0.0
diff += vectorops.normSquared(se3.error(camera_transform,new_camera_transform.average))
for marker in marker_id_list:
if marker_types[marker]=='t':
diff += vectorops.normSquared(se3.error(marker_transforms[marker],new_marker_transforms[marker].average))
else:
diff += vectorops.distanceSquared(marker_transforms[marker],new_marker_transforms[marker].average)
camera_transform = new_camera_transform.average
for marker in marker_id_list:
marker_transforms[marker] = new_marker_transforms[marker].average
if math.sqrt(diff) < tolerance:
#converged!
print "Converged with diff %g on iteration %d"%(math.sqrt(diff),iters)
break
print " ESTIMATE CHANGE:",math.sqrt(diff)
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
print " OBSERVATION ERROR:",math.sqrt(error)
#raw_input()
if DO_VISUALIZATION:
rgroup.setFrameCoordinates("camera estimate",camera_transform)
rgroup.setFrameCoordinates("marker estimate",marker_transforms.values()[0])
for i,obs in enumerate(marker_observations):
rgroup.setFrameCoordinates("obs"+str(i)+" estimate",obs)
vis.add("coordinates",rgroup)
vis.dialog()
if math.sqrt(diff) >= tolerance:
print "Max iters reached"
error = 0.0
for i in xrange(n):
marker = marker_ids[i]
Tclink = camera_link_transforms[i]
Tmlink = marker_link_transforms[i]
obs = marker_observations[i]
Tc = se3.mul(Tclink,camera_transform)
if marker_types[marker] == 't':
Tm = se3.mul(Tmlink,marker_transforms[marker])
error += vectorops.normSquared(se3.error(se3.mul(Tc,obs),Tm))
else:
Tm = se3.apply(Tmlink,marker_transforms[marker])
error += vectorops.distanceSquared(se3.apply(Tc,obs),Tm)
return (math.sqrt(error),camera_transform,marker_transforms)
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..."
'''Reference points''' frwrd = [0,0,-1,0,1,0,1,0,0] # rotation relative to global/baxter fram to point hands/z forward ext = [0.91,-0.41,1.46] '''Transforms''' T_target = [frwrd,ext] T_EEtoPPU = [[-1,0,0,0,0,1,0,1,0], [0,0,0]] # '''Correction loop''' # while True: # T_EE = robot.right_ee.sensedTransform() # R_PPU = so3.mul(T_EEtoPPU[0],T_EE[0]) # roundEachAndPrint(R_PPU,3) while True: T_bestEE = se3.mul(se3.inv(T_EEtoPPU),T_target) T_EEcmd = T_bestEE robot.right_ee.moveTo(T_EEcmd[0],T_EEcmd[1]) # while True: # transform = robot.right_ee.sensedTransform() # position = transform[1] # rotation = transform[0] # roundEachAndPrint(rotation,3)
