Example #1
0
    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
Example #2
0
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)
Example #3
0
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)
Example #4
0
    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
Example #6
0
 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)
Example #8
0
 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)