Exemplo n.º 1
0
    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.
        """

        possibleGrasps = self.knowledge.grasp_xforms(object)
        oldConfig =  self.robot.getConfig()
        grasp = 0
        for grasp in possibleGrasps: # loop until we find a working grasp
            # try left arm first
            self.robot.setConfig(oldConfig)
            goal = ik.objective(self.left_gripper_link, R=grasp[0], t=grasp[1])
            if ik.solve(goal):
                worked = True
                self.active_limb = "Left"
                self.config = self.robot.getConfig()
                return True
            else:
                # left arm failed, try right arm
                self.robot.setConfig(oldConfig)
                goal = ik.objective(self.right_gripper_link, R=grasp[0], t=grasp[1])
                if ik.solve(goal):
                    worked = True
                    self.active_limb = "Right"
                    self.config = self.robot.getConfig()
                    return True

        self.robot.setConfig(oldConfig)
        print "IK solver failed to find a grasp."
        return False
Exemplo n.º 2
0
    def move_camera_to_bin(self,bin_name):
        """Starts a motion so the camera has a viewpoint that
        observes bin_name.  Will also change self.active_limb to the
        appropriate limb.

        If successful, sends the motion to the low-level controller and
        returns True.
        
        Otherwise, does not modify the low-level controller and returns False.
        """
        world_offset = self.knowledge.bin_vantage_point(bin_name)
        
        #place +z in the +x axis, y in the +z axis, and x in the -y axis
        left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        qcmd = self.controller.getCommandedConfig()
        old_config = [c for c in qcmd]
        for i in range(100):
            if random.random() < 0.5:
                if i == 0:
                    self.robot.setConfig(qcmd)
                else:
                    self.randomize_limb_position('left')
                result = ik.solve(left_goal)
                if result:
                    # TODO: plan a path, DONE
                    # change print_detail to True if you want to know what parts are colliding
                    if self.planner.is_in_self_collision(print_detail=False):
                        #print "collided"
                        continue
                    #try:
                    #    self.planner.plan(old_config, self.robot.getConfig())
                    #except:
                    #    continue
                    #print "Left planning done"
                    self.controller.setMilestone(self.robot.getConfig())
                    self.active_limb = 'left'
                    return True
            else:
                if i == 0:
                    self.robot.setConfig(qcmd)
                else:
                    self.randomize_limb_position('right')
                result = ik.solve(right_goal)
                if result:
                    # TODO: plan a path, DONE
                    # change print_detail to True if you want to know what parts are colliding
                    if self.planner.is_in_self_collision(print_detail=False):
                        #print "collided"
                        continue
                    #try:
                    #    self.planner.plan(old_config, self.robot.getConfig())
                    #except:
                    #    print "Right Failed"
                    #    continue
                    #print "Right planning done"
                    self.controller.setMilestone(self.robot.getConfig())
                    self.active_limb = 'right'
                    return True
        return False
Exemplo n.º 3
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        #TODO: remove me
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        qmin,qmax = self.robot.getJointLimits()
        for i in range(100):
            if self.active_limb == 'left':
                q = baxter_rest_config[:]
                for j in self.left_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                #goal = ik.objective(self.left_gripper_link,local=[vectorops.sub(left_gripper_center_xform[1],[0,0,0.1]),left_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
                if ik.solve(goal,tol=0.1):
                    self.config = self.robot.getConfig()
                    return True
                self.config = self.robot.getConfig()
            else:
                q = baxter_rest_config[:]
                for j in self.right_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                #goal = ik.objective(self.right_gripper_link,local=[vectorops.sub(right_gripper_center_xform[1],[0,0,0.1]),right_gripper_center_xform[1]],world=[vectorops.add(target,[0,0,0.1]),target])
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
                if ik.solve(goal,tol=0.1):
                    self.config = self.robot.getConfig()
                    return True
                self.config = self.robot.getConfig()
        #TODO: put my code here
        return False
Exemplo n.º 4
0
    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 attempt_random_config(self):
     """Attempts to configure arms in a random configuration"""
     
     goal = ik.objective(self.left_gripper_link, local=[0, 0, 0], world=[random.random(), random.random(), random.random()])
     ik.solve(goal)
     
     goal = ik.objective(self.right_gripper_link, local=[0, 0, 0], world=[random.random(), random.random(), random.random()])
     ik.solve(goal)
     
     self.config = self.robot.getConfig()
Exemplo n.º 6
0
    def move_camera_to_bin(self,bin_name):
        """Starts a motion so the camera has a viewpoint that
        observes bin_name.  Will also change self.active_limb to the
        appropriate limb.

        If successful, sends the motion to the low-level controller and
        returns True.
        
        Otherwise, does not modify the low-level controller and returns False.
        """
        world_offset = self.knowledge.bin_vantage_point(bin_name)
        
        #place +z in the +x axis, y in the +z axis, and x in the -y axis
        left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        qcmd = self.controller.getCommandedConfig()

        s_temp = bin_name
        ss = s_temp.split('_')
        if ord(ss[1]) <= 70:
            if ord(ss[1]) % 3 == 2:
                ss[0] = 'left'
            else:
                ss[0] = 'right'
        else:
            if ord(ss[1]) % 3 == 1:
                ss[0] = 'right'
            else:
                ss[0] = 'left'

        for i in range(100):
        #if random.random() < 0.5:
            if i == 0:
                self.robot.setConfig(qcmd)
            else:
                self.randomize_limb_position(ss[0])
            if ss[0] == 'left':
                if ik.solve(left_goal):
                    #TODO: plan a path
                    no_collision = LimbPlanner.check_collision_free(self.planner, ss[0])
                    if no_collision:
                        self.controller.setMilestone(self.robot.getConfig())
                        self.active_limb = ss[0]
                        return True
            else:
                if ik.solve(right_goal):
                    #TODO: plan a path
                    no_collision = LimbPlanner.check_collision_free(self.planner, ss[0])
                    if no_collision:
                        self.controller.setMilestone(self.robot.getConfig())
                        self.active_limb = ss[0]
                        return True
        return False
        #else:
        """
Exemplo n.º 7
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        #TODO: put my code here
        if self.active_limb=="LEFT":
            # left arm IK
            (_,t) = order_bin_xform
            R = [1,0,0,0,-1,0,0,0,-1]
            t = [t[0], t[1], 0.7]
            goal = ik.objective(self.left_gripper_link, R=R, t=t)
            res = ik.solve(goal)
            if res:
                self.config = self.robot.getConfig()
                print "Placed to order bin successfully"
                return True
            else:
                for _ in xrange(100):
                    self.robot.setConfig(self.random_init())
                    goal = ik.objective(self.left_gripper_link, R=R, t=t)
                    res = ik.solve(goal)
                    if res:
                        self.config = self.robot.getConfig()
                        print "Placed to order bin successfully"
                        return True
                print "Placing to order bin failed"
                return False
        elif self.active_limb=="RIGHT":
            # right arm IK
            (_,t) = order_bin_xform
            R = [1,0,0,0,-1,0,0,0,-1]
            t = [t[0], t[1], 0.7]
            goal = ik.objective(self.right_gripper_link, R=R, t=t)
            res = ik.solve(goal)
            if res:
                self.config = self.robot.getConfig()
                print "Placed to order bin successfully"
                return True
            else:
                for _ in xrange(100):
                    self.robot.setConfig(self.random_init())
                    goal = ik.objective(self.right_gripper_link, R=R, t=t)
                    res = ik.solve(goal)
                    if res:
                        self.config = self.robot.getConfig()
                        print "Placed to order bin successfully"
                        return True
                print "Placing to order bin failed"
                return False
        else:
            print "Active limb does not find"
            return False
Exemplo n.º 8
0
    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
Exemplo n.º 9
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """

        # find a point a little (0.5 meters) above the order bin)
        aboveOrderBinLocation = se3.apply(order_bin_xform, [0, 0, 0])
        aboveOrderBinLocation[2] = aboveOrderBinLocation[2] + 0.5

        oldConfig = self.robot.getConfig()

        # figure out what gripper is holding the object
        activeGripper = self.left_gripper_link
        if self.active_limb == "Right":
            activeGripper = self.right_gripper_link

        goal = ik.objective(activeGripper, local=[0, 0, 0], world=aboveOrderBinLocation)
        if ik.solve(goal):
            self.config = self.robot.getConfig();
            return True


        #self.robot.setConfig(oldConfig)
        print "IK solver failed to find a a point above the order bin."
        return False
Exemplo n.º 10
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        
        arms = { 'right': (self.right_gripper_link, right_gripper_center_xform[1]), 
                 'left':  (self.left_gripper_link, left_gripper_center_xform[1])
               }

        # use the active limb
        try:
            (link, gripper_center) = arms[self.active_limb]
        except KeyError:
            return False
        
        # move the gripper to order bin top and point opposite the world's z axis
        goal = ik.objective(link, R=so3.rotation([1,0,0], math.pi), t=self.knowledge.order_bin_top())
        # try with 50 attempts
        # randomize after each failed attempt        
        for attempts in range(50):
            # solve the inverse kinematics with 5 cm accuracy (the bin is large)
            if ik.solve(goal, tol=0.05):
                self.config = goal.robot.getConfig()
                return True
            else:
                # randomize this limb
                randomize_links(goal.robot, arm_link_names[self.active_limb])

        return False
Exemplo n.º 11
0
    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
Exemplo n.º 12
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        #TODO: put my code here
		#gohere
	'''
	bin_name = object.bin_name
	pointInFrontOfBin = self.knowledge.bin_front_center(bin_name)
	goal = ik.objective(self.active_limb,R=[0,1,0,0,0,1,1,0,0],t=pointInFrontOfBin)
	self.config = self.robot.getConfig()
	return ik.solve(goal)
	'''
	self.robot.setConfig(self.originalConfig)
	self.config = self.originalConfig

	orderBinLocalPoint = [(order_bin_bounds[0][i] + order_bin_bounds[1][i])/2 for i in range(3)]
	orderBinLocalPoint[2] = .7
	gripper = self.active_limb
	orderBinWorldPoint = se3.apply(order_bin_xform,orderBinLocalPoint)
	goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=orderBinWorldPoint)
	iterations = 0
	while iterations!=1000:
		iterations+=1
		if ik.solve(goal,tol=.1):
			self.config = self.robot.getConfig()
			return True
		print "having trouble putting in order bin"
		pointAroundBinWorldPoint = [orderBinWorldPoint[i] + random.random()*.1 for i in range(3)]
		goal = ik.objective(gripper,R=[0,1,0,1,0,0,0,0,-1],t=pointAroundBinWorldPoint)
		self.robot.setConfig(self.originalConfig)
		self.config = self.originalConfig
Exemplo n.º 13
0
    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.
        """
    #TODO: put my code here
	graspToWorldTransforms = self.knowledge.grasp_xforms(object)
	grasps_in_world_coords = [se3.apply(transform,[0,0,0]) for transform in graspToWorldTransforms]
	if self.active_limb == self.left_camera_link:
		gripper = self.left_gripper_link
		print "left gripper"
	if self.active_limb == self.right_camera_link:
		gripper = self.left_gripper_link
		print "right gripper"
	goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
	
	iterations = 0
	while iterations!= 1000:
		iterations += 1
		for goal in goals:
			if ik.solve(goal,tol=.01):
				self.config = self.robot.getConfig()
				return True
		print "having trouble grasping"
		if iterations % 500 == 0:
			a = 5
		goals = [ik.objective(gripper,R=graspToWorldTransforms[i][0],t=grasps_in_world_coords[i]) for i in range(len(graspToWorldTransforms))]
	return False
Exemplo n.º 14
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
        print
        print 'Trying to move over order bin'
        world_goal_xyz = se3.apply(order_bin_xform,[0,0,0.75])
        
        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,local=([0,0,0]),world=(world_goal_xyz))

            if ik.solve(goal,tol = 0.05): #lower tol because the bin is huge
                self.active_limb = 'Left'
                self.config = self.robot.getConfig()
                return True

            cnt = cnt + 1



        
        return False
Exemplo n.º 15
0
    def move_to_grasp_object(self,object):
        #Select appropriate limb and link
        self.active_limb = self.limb_selector("gripper")

        #Extract the contents of the bin
        binContents = self.knowledge.bin_contents[self.current_bin]

        #Extract the list of possible grasp points
        graspList = binContents[0].info.grasps

        #Loop through grasp list and attempt to solve inverse kinematics
        for graspIndex in range(0, len(graspList)):
            #Compute transform from grasp point to bin to world
            graspTransform = graspList[graspIndex].grasp_xform
            binCoordinate = se3.apply(graspTransform,[0,0,0])
            worldCoordinate = se3.apply(binContents[0].xform, binCoordinate)

            #Compute objective
            objective = ik.objective(self.active_limb, local=[0,0,0], world=worldCoordinate)

            #Attempt to solve inverse kinematics
            if ik.solve(objective):
                self.config = self.robot.getConfig()
                return True
            else:
                return False
Exemplo n.º 16
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, sends the motion to the low-level controller and
        returns True.

        Otherwise, does not modify the low-level controller and returns False.
        """
        left_target = se3.apply(order_bin_xform,[0.0,0.2,order_bin_bounds[1][2]+0.1])
        right_target = se3.apply(order_bin_xform,[0.0,-0.2,order_bin_bounds[1][2]+0.1])
        qcmd = self.controller.getCommandedConfig()
        for i in range(100):
            if self.active_limb == 'left':
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=left_target)
            else:
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=right_target)
            #set IK solver initial configuration
            if i==0:
                self.robot.setConfig(qcmd)
            else:
                self.randomize_limb_position(self.active_limb)
            #solve
            if ik.solve(goal,tol=0.1):

                if self.planner.check_collision_free('left'):
                    self.controller.setMilestone(self.robot.getConfig())
                    self.active_limb = 'left'
                    return True

        return False
    def right_arm_ik_near_seed(self, right_target, ignore_elbow_up_constraint=True):
        """Solves IK to move the right arm to the specified
            right_target ([x, y, z] in world space)
        """
        self.load_real_robot_state()
        self.set_model_right_arm(eval("Q_IK_SEED_" + self.current_bin))

        oldRSquared = -1
        q_ik = None

        qmin, qmax = self.robotModel.getJointLimits()
        for i in range(1000):
            point2_local = vectorops.add(VACUUM_POINT_XFORM[1], [0.5, 0, 0])
            point2_world = vectorops.add(right_target, [0, 0, -0.5])
            goal1 = ik.objective(self.robotModel.link("right_wrist"), local=VACUUM_POINT_XFORM[1], world=right_target)
            goal2 = ik.objective(self.robotModel.link("right_wrist"), local=point2_local, world=point2_world)
            if ik.solve([goal1, goal2], tol=0.0001) and (self.elbow_up() or ignore_elbow_up_constraint):
                q_vals = [self.robotModel.getConfig()[v] for v in self.right_arm_indices]
                rSquared = vectorops.norm(vectorops.sub(q_vals, eval("Q_IK_SEED_" + self.current_bin)))
                if oldRSquared < 0 or oldRSquared > rSquared:
                    oldRSquared = rSquared
                    q_ik = q_vals
        print FAIL_COLOR + "right_arm_ik failed for " + str(right_target) + END_COLOR
        if not (self.elbow_up() or ignore_elbow_up_constraint):
            print FAIL_COLOR + str([self.robotModel.getConfig()[v] for v in self.right_arm_indices]) + END_COLOR
            print FAIL_COLOR + "IK found but elbow wasn't up" + END_COLOR

        if oldRSquared >= 0:
            self.set_model_right_arm(q_ik)
            return True
        return False
Exemplo n.º 18
0
    def move_camera_to_bin(self,bin):
        # Camera rotation and translation relative to world frame
        R_camera = [0,0,-1, 1,0,0, 0,1,0]
        t_camera = knowledge.bin_vantage_point(bin)

        # Setup ik objectives for both arms
        left_goal  = ik.objective(self.left_camera_link,  R=R_camera, t=t_camera)
        right_goal = ik.objective(self.right_camera_link, R=R_camera, t=t_camera)

        # Joint Limits
        qmin,qmax = self.robot.getJointLimits()

        for i in range(100):
            # initialize to the resting pose
            # NOTE: that this (without [:]) doesn't work because the list
            #       must be copied by value, not reference.
            #       See python shallow copy vs. deep copy for more details
            # q = baxter_rest_config
            q = baxter_rest_config[:]

            # use left hand
            if random.random() < 0.5:
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.left_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(left_goal):
                    self.config = self.robot.getConfig()
                    self.active_limb = 'left'
                    return True

            # use right hand
            else:
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.right_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(right_goal):
                    self.config = self.robot.getConfig()
                    self.active_limb = 'right'
                    return True
        return False
Exemplo n.º 19
0
    def move_to_order_bin(self,object):
        # apply the order_bin_xform to the given point
        left_target_point  = se3.apply(order_bin_xform, [0.0, 0.2, order_bin_bounds[1][2]+0.1])
        right_target_point = se3.apply(order_bin_xform, [0.0, -0.2, order_bin_bounds[1][2]+0.1])

        # Setup ik objectives for both arms
        left_goal  = ik.objective(self.left_gripper_link,  local=left_gripper_center_xform[1],  world=left_target_point)
        right_goal = ik.objective(self.right_gripper_link, local=right_gripper_center_xform[1], world=right_target_point)

        qmin, qmax = self.robot.getJointLimits()

        for i in range(100):
            # initialize to the resting pose
            # NOTE: that this (without [:]) doesn't work because the list
            #       must be copied by value, not reference.
            #       See python shallow copy vs. deep copy for more details
            #       ( q = baxter_rest_config ) doesn't work!
            q = baxter_rest_config[:]

            # use left hand
            if self.active_limb == 'left':
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.left_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(left_goal):
                    self.config = self.robot.getConfig()
                    return True

            # use right hand
            else:
                # randomly initialize each joint in the arm within joint limits
                for jointIndex in self.right_arm_indices:
                    q[jointIndex] = random.uniform(qmin[jointIndex], qmax[jointIndex])
                self.robot.setConfig(q)

                # attempt to solve ik objective
                if ik.solve(right_goal):
                    self.config = self.robot.getConfig()
                    return True
        self.config = self.robot.getConfig()
        return False
Exemplo n.º 20
0
    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.
        """
        
        # Store current config in case we need to reset
        originalConfig = self.robot.getConfig()

        randomAttempts = 0        
        
        while randomAttempts < self.maxRandomAttempts:
            # Try goal with left camera first
            leftGoal = ik.objective(self.left_camera_link, local=[[0, 0, 0], [0, 0, -1]], world=[self.knowledge.bin_vantage_point(bin_name), [self.knowledge.bin_vantage_point(bin_name)[0] - 1, self.knowledge.bin_vantage_point(bin_name)[1], self.knowledge.bin_vantage_point(bin_name)[2]]])
            
            if ik.solve(leftGoal):
                self.active_limb = "left"
                self.config = self.robot.getConfig()
                return True
            else:
                # Restore previous config
                self.robot.setConfig(originalConfig)
                
                # Try goal with right camera
                rightGoal = ik.objective(self.right_camera_link, local=[[0, 0, 0], [0, 0, -1]], world=[self.knowledge.bin_vantage_point(bin_name), [self.knowledge.bin_vantage_point(bin_name)[0] - 1, self.knowledge.bin_vantage_point(bin_name)[1], self.knowledge.bin_vantage_point(bin_name)[2]]])
                
                if ik.solve(rightGoal):      
                    self.active_limb = "right"
                    self.config = self.robot.getConfig()
                    return True
                    
            # Reset from a random position
            self.attempt_random_config()
            randomAttempts += 1
        
        # Restore previous config
        self.robot.setConfig(originalConfig)
       
        return False
Exemplo n.º 21
0
    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.
        """
        
        # Store current config in case we need to reset
        originalConfig = self.robot.getConfig()
        
        randomAttempts = 0
        
        while randomAttempts < self.maxRandomAttempts:
            # Loop through all potential grasps trying to solve with the active limb
            for grasp in self.knowledge.grasp_xforms(object):
                objectGraspLocation = se3.apply(object.xform, grasp[1])
                
                # Construct world points for axes alignment
                objectGraspXPoint = se3.apply(object.xform, se3.apply(grasp, [1, 0, 0]))
                objectGraspYPoint = se3.apply(object.xform, se3.apply(grasp, [0, 1, 0]))
                objectGraspZPoint = se3.apply(object.xform, se3.apply(grasp, [0, 0, 1]))
                
                if self.active_limb == "left":
                    # Construct local points for axes alignment
                    localXPoint = [left_gripper_center_xform[1][0] + 1, left_gripper_center_xform[1][1], left_gripper_center_xform[1][2]]
                    localYPoint = [left_gripper_center_xform[1][0], left_gripper_center_xform[1][1] + 1, left_gripper_center_xform[1][2]]
                    localZPoint = [left_gripper_center_xform[1][0], left_gripper_center_xform[1][1], left_gripper_center_xform[1][2] + 1]   
                    
                    goal = ik.objective(self.left_gripper_link, local=[left_gripper_center_xform[1], localXPoint, localYPoint, localZPoint], world=[objectGraspLocation, objectGraspXPoint, objectGraspYPoint, objectGraspZPoint])
                else:
                    # Construct local points for axes alignment
                    localXPoint = [right_gripper_center_xform[1][0] + 1, right_gripper_center_xform[1][1], right_gripper_center_xform[1][2]]
                    localYPoint = [right_gripper_center_xform[1][0], right_gripper_center_xform[1][1] + 1, right_gripper_center_xform[1][2]]
                    localZPoint = [right_gripper_center_xform[1][0], right_gripper_center_xform[1][1], right_gripper_center_xform[1][2] + 1]    
                
                    goal = ik.objective(self.right_gripper_link, local=[right_gripper_center_xform[1], localXPoint, localYPoint, localZPoint], world=[objectGraspLocation, objectGraspXPoint, objectGraspYPoint, objectGraspZPoint])
            
                if ik.solve(goal):
                    self.config = self.robot.getConfig()
                    return True
                else:
                    # Restore previous config
                    self.robot.setConfig(originalConfig)
                    
                    # Reset from a random position
                    self.attempt_random_config()
                    randomAttempts += 1
                    
        # Restore previous config
        self.robot.setConfig(originalConfig)
        
        return False
Exemplo n.º 22
0
 def prob(self):
     self.active_limb = self.left_camera_link
     for goal in range(1,150):
         rand_pos = [random.randrange(0,2),random.randrange(0,2),random.randrange(0,2)]
         objective = ik.objective(self.active_limb, local=[0,0,0], world=rand_pos)
         if ik.solve(objective):
             self.config = self.robot.getConfig()
             break
         if goal == 150:
             return True
     return False
Exemplo n.º 23
0
    def move_camera_to_bin(self,bin_name):
        """Starts a motion so the camera has a viewpoint that
        observes bin_name.  Will also change self.active_limb to the
        appropriate limb.

        If successful, sends the motion to the low-level controller and
        returns True.
        
        Otherwise, does not modify the low-level controller and returns False.
        """
        world_offset = self.knowledge.bin_vantage_point(bin_name)
        start_config = self.robot.getConfig()
        
        #place +z in the +x axis, y in the +z axis, and x in the -y axis
        left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        qcmd = self.controller.getCommandedConfig()
        for i in range(100):
            if random.random() < 0.5:
                if i == 0:
                    self.robot.setConfig(qcmd)
                else:
                    self.randomize_limb_position('left')
                if ik.solve(left_goal):
                    if self.planner.check_collision_free('left'):
                        self.controller.setMilestone(self.robot.getConfig())
                        #self.controller.setMilestone(self.planner.plan(start_config, self.robot.getConfig()))
                        self.active_limb = 'left'
                        return True
            else:
                if i == 0:
                    self.robot.setConfig(qcmd)
                else:
                    self.randomize_limb_position('right')
                if ik.solve(right_goal):
                    if self.planner.check_collision_free('right'):
                        self.controller.setMilestone(self.robot.getConfig())
                        #self.controller.setMilestone(self.planner.plan(start_config, self.robot.getConfig()))
                        self.active_limb = 'right'
                        return True
        return False
Exemplo n.º 24
0
 def iksolve(self, config, kEE, pEE, mq):
     goal = ik.objective(kEE,local=self.hand_position, world=config)
     q = self.setKlamptPos()
     pEE.positionCommand(pEE.configFromKlampt(q))
     if ik.solve(goal):
         print "success!"
         q = self.kRobot.getConfig()
         mq.setRamp(pEE.configFromKlampt(q))
         while(mq.moving()):
             time.sleep(0.1)
     else:
         print "failed. Residual:", ik.solver(goal).getResidual()
Exemplo n.º 25
0
    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.
        """
        world_offset = self.knowledge.bin_vantage_point(bin_name)
        
        #place +z in the +x axis, y in the +z axis, and x in the -y axis
        left_goal = ik.objective(self.left_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        right_goal = ik.objective(self.right_camera_link,R=[0,0,-1,1,0,0,0,1,0],t=world_offset)
        qmin,qmax = self.robot.getJointLimits()
        for i in range(100):
            if random.random() < 0.5:
                q = baxter_rest_config[:]
                for j in self.left_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                self.robot.setConfig(q)
                if ik.solve(left_goal):
                    self.config = self.robot.getConfig()
                    self.active_limb = 'left'
                    return True
            else:
                q = baxter_rest_config[:]
                for j in self.right_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                self.robot.setConfig(q)
                if ik.solve(right_goal):
                    self.config = self.robot.getConfig()
                    self.active_limb = 'right'
                    return True
        return False
        #TODO: put my code here
        #Hint:
        #goal = ik.objective(self.left_camera_link,R=[local-to-world rotation],t=[local-to-world translation])
        #if ik.solve(goal):
        #   the robot model's configuration is changed... but you still have to update the state of the controller.
        return False
Exemplo n.º 26
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """
	self.robot.setConfig(baxter_rest_config)
	target = se3.apply(order_bin_xform, [0,0,.7])
	if(ik.solve(ik.objective(self.active_limb,R=[0,1,0,1,0,0,0,0,-1],t=target))):
		self.config = self.robot.getConfig()
		return True
	self.config = self.robot.getConfig()
	return False
Exemplo n.º 27
0
    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.
        """
        #TODO: remove me
        grasps = self.knowledge.grasp_xforms(object)
        qmin,qmax = self.robot.getJointLimits()
        for i in range(100):
            if self.active_limb == 'left':
                q = baxter_rest_config[:]
                for j in self.left_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                g = random.choice(grasps)
                #Tg = se3.mul(g,se3.inv(left_gripper_center_xform))
                #goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
                goal = ik.objective(self.left_gripper_link,local=left_gripper_center_xform[1],world=g[1])
                if ik.solve(goal):
                    self.config = self.robot.getConfig()
                    return True
            else:
                q = baxter_rest_config[:]
                for j in self.right_arm_indices:
                    q[j] = random.uniform(qmin[j],qmax[j])
                g = random.choice(grasps)
                #Tg = se3.mul(g,se3.inv(right_gripper_center_xform))
                #goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
                goal = ik.objective(self.right_gripper_link,local=right_gripper_center_xform[1],world=g[1])
                if ik.solve(goal):
                    self.config = self.robot.getConfig()
                    return True
                
        #TODO: put my code here
        return False
Exemplo n.º 28
0
    def move_to_order_bin(self,object):
        """Sets the robot's configuration so the gripper is over the order bin

        If successful, changes self.config and returns True.
        Otherwise, does not change self.config and returns False.
        """

        self.active_limb = self.left_camera_link
        order_vantage = self.knowledge.order_vantage()
        objective = ik.objective(self.active_limb, local=[0,0,0], world=order_vantage)
        if ik.solve(objective):
            self.config = self.robot.getConfig()
            return True
        else:
            return False
Exemplo n.º 29
0
    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.
        """
        # get list of possible grasps
	poss = self.knowledge.grasp_xforms(object=object)
	target = [se3.apply(transform,[0,0,0]) for transform in poss]
	for i in range(len(target)):
		self.robot.setConfig(baxter_rest_config)
		if(ik.solve(ik.objective(self.active_limb,R=poss[i][0],t=target[i]))):
			self.config=self.robot.getConfig()
			return True
	return False
Exemplo n.º 30
0
    def move_camera_to_bin(self,bin_name):
        #Select appropriate limb and link
        self.active_limb = self.limb_selector("camera")

        #Calculate camera vantage point
        vantagePoint = self.knowledge.bin_vantage_point(bin_name)

        #Generate objective
        objective = ik.objective(self.active_limb, R=[0, -1, 0, 0, 0, -1, 1, 0, 0], t=vantagePoint)

        #Attempt to solve the inverse kinematics
        if ik.solve(objective):
            self.config = self.robot.getConfig()
            return True
        else:
            return False