def executeSingleGrip(self, fold_traj): print "hi from single grip" approach_point = fold_traj.approach_points[0] grip_point = fold_traj.grip_points[0] quarter_point = fold_traj.quarter_points[0] vertical_point = fold_traj.vertical_points[0] goal_point = fold_traj.goal_points[0] smooth_center = fold_traj.smooth_center smooth_edges = fold_traj.smooth_edges first_adjustment = False tilt = fold_traj.tilts[0] roll = -pi/2 #pi/2 pitch = pi/4 if fold_traj.red: tilt = self.last_traj.tilts[0]%(2*pi) rospy.loginfo("Tilt: %s"%tilt) #We do this so that we don't let go of the vertex during a red fold if not fold_traj.red: self.executeSmooth(self.last_traj) StanceUtils.open_gripper("b") self.fold_stance("b") arm = self.getArm(fold_traj) #Move the unused arm out of the way if arm=="l": StanceUtils.call_stance("viewing_right",5.0) else: StanceUtils.call_stance("viewing_left",5.0) (target,nothing) = self.gripPoints(point=approach_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red) pitch = target.pitch self.gripPoints(point=grip_point,arm=arm,tilt=tilt,grip=False,preferred_pitch = pi/4,preferred_roll=roll,red=fold_traj.red,grab=True) StanceUtils.close_gripper(arm) else: arm = self.last_arm StanceUtils.close_gripper(arm) self.gripPoints(point=vertical_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=pi/2,preferred_roll=roll,red=fold_traj.red) self.gripPoints(point=goal_point,grip=True,tilt=tilt,arm=arm,preferred_pitch=3*pi/5,preferred_roll=roll,red=fold_traj.red) self.last_traj = fold_traj self.last_arm = arm
def executeBiGrip(self, fold_traj): tilts = fold_traj.tilts approach_points = fold_traj.approach_points grip_points = fold_traj.grip_points quarter_points = fold_traj.quarter_points weight_points = fold_traj.weight_points vertical_points = fold_traj.vertical_points goal_points = fold_traj.goal_points rolls = [pi / 2, pi / 2] #pitches = [pi/4,pi/4] pitches = [pi / 4, pi / 4] if not fold_traj.red: self.executeSmooth(self.last_traj) self.fold_stance("b") arms = self.getArms(fold_traj) #Initially, both are in "approach" mode (target1, target2) = self.gripPoints(point=approach_points[0], grip=False, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 4, preferred_roll=rolls[0], point2=approach_points[1], grip2=False, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 4, preferred_roll2=rolls[1]) #pitches[0] = target1.pitch #Do the first pickup and nothing with the other arm self.gripPoints(point=grip_points[0], grip=False, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 4, preferred_roll=rolls[0], grab=True) StanceUtils.close_gripper(arms[0]) #Do the second pickup, with the first arm interpolated first_dx = vertical_points[0].point.x - grip_points[0].point.x first_dy = vertical_points[0].point.y - grip_points[0].point.y first_dz = vertical_points[0].point.z - grip_points[0].point.z second_dx = vertical_points[1].point.x - grip_points[1].point.x second_dy = vertical_points[1].point.y - grip_points[1].point.y second_dz = vertical_points[1].point.z - grip_points[1].point.z interp_dx = first_dx - second_dx interp_dy = first_dy - second_dy interp_dz = first_dz - second_dz interp_x = grip_points[0].point.x + interp_dx interp_y = grip_points[0].point.y + interp_dy interp_z = grip_points[0].point.z + interp_dz interp_pt = PointStamped() interp_pt.point.x = interp_x interp_pt.point.y = interp_y interp_pt.point.z = interp_z interp_pt.header.frame_id = grip_points[0].header.frame_id #Go to the second approach point (target2, nothing) = self.gripPoints(point=interp_pt, grip=True, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 4, preferred_roll=rolls[0], point2=approach_points[1], arm2=arms[1], grip2=False, tilt2=tilts[1], preferred_pitch2=pi / 4, preferred_roll2=rolls[1]) #pitches[1] = target2.pitch self.gripPoints(point=interp_pt, grip=True, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 4, preferred_roll=rolls[0], point2=grip_points[1], grip2=False, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 4, preferred_roll2=rolls[1]) self.gripPoints(point=grip_points[1], grip=False, tilt=tilts[1], arm=arms[1], preferred_pitch=pi / 4, preferred_roll=rolls[1], grab=True) StanceUtils.close_gripper(arms[1]) else: arms = self.getArms(fold_traj) #Bring both to middle self.gripPoints(point=vertical_points[0], grip=True, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 2, preferred_roll=rolls[0], point2=vertical_points[1], grip2=True, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 2, preferred_roll2=rolls[1]) #FIX pi/4 -> pi/2 #Set first one down first_dx = goal_points[0].point.x - vertical_points[0].point.x first_dy = goal_points[0].point.y - vertical_points[0].point.y first_dz = goal_points[0].point.z - vertical_points[0].point.z second_dx = goal_points[1].point.x - vertical_points[1].point.x second_dy = goal_points[1].point.y - vertical_points[1].point.y second_dz = goal_points[1].point.z - vertical_points[1].point.z interp_dx = first_dx - second_dx interp_dy = first_dy - second_dy interp_dz = first_dz - second_dz interp_x = goal_points[0].point.x - interp_dx interp_y = goal_points[0].point.y - interp_dy interp_z = goal_points[0].point.z - interp_dz interp_pt = PointStamped() interp_pt.point.x = interp_x interp_pt.point.y = interp_y interp_pt.point.z = interp_z interp_pt.header.frame_id = grip_points[0].header.frame_id self.gripPoints(point=interp_pt, grip=True, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 2, preferred_roll=rolls[0], point2=goal_points[1], grip2=True, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 2, preferred_roll2=rolls[1]) #Finally, set last one down self.gripPoints(point=goal_points[0], grip=True, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 2, preferred_roll=rolls[0], point2=goal_points[1], grip2=True, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 2, preferred_roll2=rolls[1]) GripUtils.open_grippers() goal_points[0].point.z += 0.03 goal_points[1].point.z += 0.03 self.gripPoints(point=goal_points[0], grip=False, tilt=tilts[0], arm=arms[0], preferred_pitch=pi / 2, preferred_roll=rolls[0], point2=goal_points[1], grip2=False, tilt2=tilts[1], arm2=arms[1], preferred_pitch2=pi / 2, preferred_roll2=rolls[1]) self.last_traj = fold_traj
def executeSingleGrip(self, fold_traj): approach_point = fold_traj.approach_points[0] grip_point = fold_traj.grip_points[0] quarter_point = fold_traj.quarter_points[0] vertical_point = fold_traj.vertical_points[0] goal_point = fold_traj.goal_points[0] smooth_center = fold_traj.smooth_center smooth_edges = fold_traj.smooth_edges first_adjustment = False tilt = fold_traj.tilts[0] roll = pi / 2 pitch = pi / 4 if fold_traj.red: tilt = self.last_traj.tilts[0] % (2 * pi) rospy.loginfo("Tilt: %s" % tilt) #We do this so that we don't let go of the vertex during a red fold if not fold_traj.red: self.executeSmooth(self.last_traj) StanceUtils.open_gripper("b") self.fold_stance("b") arm = self.getArm(fold_traj) #Move the unused arm out of the way if arm == "l": StanceUtils.call_stance("viewing_right", 5.0) else: StanceUtils.call_stance("viewing_left", 5.0) (target, nothing) = self.gripPoints(point=approach_point, arm=arm, tilt=tilt, grip=False, preferred_pitch=pi / 4, preferred_roll=roll, red=fold_traj.red) pitch = target.pitch self.gripPoints(point=grip_point, arm=arm, tilt=tilt, grip=False, preferred_pitch=pi / 4, preferred_roll=roll, red=fold_traj.red, grab=True) StanceUtils.close_gripper(arm) else: arm = self.last_arm StanceUtils.close_gripper(arm) self.gripPoints(point=vertical_point, grip=True, tilt=tilt, arm=arm, preferred_pitch=pi / 2, preferred_roll=roll, red=fold_traj.red) self.gripPoints(point=goal_point, grip=True, tilt=tilt, arm=arm, preferred_pitch=3 * pi / 5, preferred_roll=roll, red=fold_traj.red) self.last_traj = fold_traj self.last_arm = arm
def executeBiGrip(self, fold_traj): tilts = fold_traj.tilts approach_points = fold_traj.approach_points grip_points = fold_traj.grip_points quarter_points = fold_traj.quarter_points weight_points = fold_traj.weight_points vertical_points = fold_traj.vertical_points goal_points = fold_traj.goal_points print "hi from bigrip" rolls = [pi/2,pi/2] #rolls = [-pi/2,-pi/2] #rolls = [0,0] pitches = [pi/4,pi/4] #pitches = [0,0] if not fold_traj.red: self.executeSmooth(self.last_traj) self.fold_stance("b") arms = self.getArms(fold_traj) #Initially, both are in "approach" mode (target1,target2) = self.gripPoints(point=approach_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0], point2=approach_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1]) #pitches[0] = target1.pitch #Do the first pickup and nothing with the other arm self.gripPoints(point=grip_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=0 ,preferred_roll=rolls[0],grab=True) StanceUtils.close_gripper(arms[0]) #Do the second pickup, with the first arm interpolated first_dx = vertical_points[0].point.x - grip_points[0].point.x first_dy = vertical_points[0].point.y - grip_points[0].point.y first_dz = vertical_points[0].point.z - grip_points[0].point.z second_dx = vertical_points[1].point.x - grip_points[1].point.x second_dy = vertical_points[1].point.y - grip_points[1].point.y second_dz = vertical_points[1].point.z - grip_points[1].point.z interp_dx = first_dx - second_dx interp_dy = first_dy - second_dy interp_dz = first_dz - second_dz interp_x = grip_points[0].point.x + interp_dx interp_y = grip_points[0].point.y + interp_dy interp_z = grip_points[0].point.z + interp_dz interp_pt = PointStamped() interp_pt.point.x = interp_x interp_pt.point.y = interp_y interp_pt.point.z = interp_z interp_pt.header.frame_id = grip_points[0].header.frame_id #Go to the second approach point (target2,nothing) = self.gripPoints(point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0], point2=approach_points[1],arm2=arms[1],grip2=False,tilt2=tilts[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1]) #pitches[1] = target2.pitch self.gripPoints (point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/4,preferred_roll=rolls[0] ,point2=grip_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/4,preferred_roll2=rolls[1]) self.gripPoints(point=grip_points[1],grip=False,tilt=tilts[1],arm=arms[1],preferred_pitch=pi/4,preferred_roll=rolls[1],grab=True) StanceUtils.close_gripper(arms[1]) else: arms = self.getArms(fold_traj) #Bring both to middle self.gripPoints (point=vertical_points[0],grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0] ,point2=vertical_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1]) #FIX pi/4 -> pi/2 #Set first one down first_dx = goal_points[0].point.x - vertical_points[0].point.x first_dy = goal_points[0].point.y - vertical_points[0].point.y first_dz = goal_points[0].point.z - vertical_points[0].point.z second_dx = goal_points[1].point.x - vertical_points[1].point.x second_dy = goal_points[1].point.y - vertical_points[1].point.y second_dz = goal_points[1].point.z - vertical_points[1].point.z interp_dx = first_dx - second_dx interp_dy = first_dy - second_dy interp_dz = first_dz - second_dz interp_x = goal_points[0].point.x - interp_dx interp_y = goal_points[0].point.y - interp_dy interp_z = goal_points[0].point.z - interp_dz interp_pt = PointStamped() interp_pt.point.x = interp_x interp_pt.point.y = interp_y interp_pt.point.z = interp_z interp_pt.header.frame_id = grip_points[0].header.frame_id self.gripPoints (point=interp_pt,grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0] ,point2=goal_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1]) #Finally, set last one down self.gripPoints(point=goal_points[0],grip=True,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0] ,point2=goal_points[1],grip2=True,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1]) GripUtils.open_grippers() goal_points[0].point.z += 0.03 goal_points[1].point.z += 0.03 self.gripPoints(point=goal_points[0],grip=False,tilt=tilts[0],arm=arms[0],preferred_pitch=pi/2,preferred_roll=rolls[0] ,point2=goal_points[1],grip2=False,tilt2=tilts[1],arm2=arms[1],preferred_pitch2=pi/2,preferred_roll2=rolls[1]) self.last_traj = fold_traj