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
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
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
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()
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: """
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
def move_to_grasp_object(self,object): """Sets the robot's configuration so the gripper grasps object at one of its potential grasp locations. Might change self.active_limb to the appropriate limb. Must change self.active_grasp to the selected grasp. If successful, sends the motion to the low-level controller and returns True. Otherwise, does not modify the low-level controller and returns False. """ grasps = self.knowledge.grasp_xforms(object) qmin,qmax = self.robot.getJointLimits() qcmd = self.controller.getCommandedConfig() #phase 1: init IK from the commanded config, search among grasps for (grasp,gxform) in grasps: if self.active_limb == 'left': Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform)) goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1]) else: Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform)) goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1]) self.robot.setConfig(qcmd) if ik.solve(goal): #self.controller.setMilestone(self.robot.getConfig()) #self.active_grasp = grasp no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb) ### if no_collision: self.active_grasp = grasp self.controller.setMilestone(self.robot.getConfig()) return True #Phase 2: that didn't work, now try random sampling for i in range(100): #pick a config at random self.randomize_limb_position(self.active_limb) #pick a grasp at random (grasp,gxform) = random.choice(grasps) if self.active_limb == 'left': Tg = se3.mul(gxform,se3.inv(left_gripper_center_xform)) goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1]) else: Tg = se3.mul(gxform,se3.inv(right_gripper_center_xform)) goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1]) if ik.solve(goal): # self.active_grasp = grasp #TODO: plan a path no_collision = LimbPlanner.check_collision_free(self.planner, self.active_limb) if no_collision: self.active_grasp = grasp self.controller.setMilestone(self.robot.getConfig()) return True return False
def 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
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
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
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
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
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
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
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
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
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
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
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
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
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
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()
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
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
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
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
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
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