def bin_vantage_point(self,bin_name):
#TODO: remove me
world_center = self.bin_front_center(bin_name)
#20cm offset
world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
#you may want to implement this. Returns the world position of the
#vantage point for viewing the bin. The visualizer will draw these points if
#'draw_bins' is turned to True.
#TODO:
return None
def point_fit_xform_3d(apts,bpts):
"""Finds a 3D rigid transform that maps the list of points apts to the
list of points bpts. Return value is a klampt.se3 element that
minimizes the sum of squared errors ||T*ai-bi||^2.
"""
assert len(apts)==len(bpts)
ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
arel = [vectorops.sub(a,ca) for a in apts]
brel = [vectorops.sub(b,cb) for b in bpts]
R = point_fit_rotation_3d(arel,brel)
#R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
t = so3.sub(cb,so3.apply(R,ca))
return (R,t)
def point_fit_xform_3d(apts,bpts):
"""Finds a 3D rigid transform that maps the list of points apts to the
list of points bpts. Return value is a klampt.se3 element that
minimizes the sum of squared errors ||T*ai-bi||^2.
"""
assert len(apts)==len(bpts)
ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
arel = [vectorops.sub(a,ca) for a in apts]
brel = [vectorops.sub(b,cb) for b in bpts]
R = point_fit_rotation_3d(arel,brel)
#R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
t = so3.sub(cb,so3.apply(R,ca))
return (R,t)
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 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 bin_vantage_point(self,bin_name):
world_center = self.bin_front_center(bin_name)
# Vantage point has 20cm offset from bin center
world_offset = so3.apply(ground_truth_shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
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 bin_vantage_point(self,bin_name):
world_center = self.bin_front_center(bin_name)
#20cm offset
world_offset = so3.apply(self.shelf_xform[0],[0,0,0.2])
return vectorops.add(world_center,world_offset)
