コード例 #1
0
    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
コード例 #2
0
ファイル: ex1.py プロジェクト: RGrant92/Klampt
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))
コード例 #3
0
ファイル: hw2_mtd13.py プロジェクト: ChenyuLInx/ece490-s2016
    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
コード例 #4
0
 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)
コード例 #5
0
ファイル: armRecorder.py プロジェクト: duke-iml/ece490-s2016
    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]
コード例 #6
0
    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()
コード例 #7
0
 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)
コード例 #8
0
 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
コード例 #9
0
ファイル: sensor.py プロジェクト: branchvincent/robo-catcher
 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)
コード例 #10
0
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
コード例 #11
0
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
コード例 #12
0
ファイル: apc.py プロジェクト: duke-iml/ece490-s2016
# 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])))
コード例 #13
0
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)
コード例 #14
0
    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()
コード例 #15
0
        #    ),
        #    [ 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)

コード例 #16
0
ファイル: apc.py プロジェクト: ChenyuLInx/ece490-s2016
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])
コード例 #17
0
ファイル: apc.py プロジェクト: ChenyuLInx/ece490-s2016
#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)
コード例 #18
0
ファイル: baxter_scoop.py プロジェクト: duke-iml/ece490-s2016
    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,
コード例 #19
0
    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)

コード例 #20
0
    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()
コード例 #21
0
 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()
コード例 #22
0
    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..."
コード例 #23
0
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
コード例 #24
0
ファイル: apc.py プロジェクト: ChenyuLInx/ece490-s2016
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])))