def executeSmooth(self, fold_traj): if fold_traj == False: return False if fold_traj.ignore_smooth: return False if pi / 4 <= abs(fold_traj.tilts[0]) <= 3 * pi / 4: return False scale = 1 if self.is_backwards(fold_traj): scale = -1 StanceUtils.open_gripper("b") self.fold_stance("b") GRIPPER_LENGTH = 0.195 center = fold_traj.smooth_center left_start = self.torso_pt(center) right_start = self.torso_pt(center) left_start.point.y += 0.07 left_start.point.x += scale * 0.1 left_start.point.z += GRIPPER_LENGTH + 0.01 right_start.point.y -= 0.07 right_start.point.x += scale * 0.1 right_start.point.z += GRIPPER_LENGTH + 0.01 left_end = self.torso_pt(fold_traj.smooth_edges[0]) left_end.point.z += GRIPPER_LENGTH + 0.01 left_end.point.x += scale * 0.08 right_end = self.torso_pt(fold_traj.smooth_edges[1]) right_end.point.z += GRIPPER_LENGTH + 0.01 right_end.point.x += scale * 0.08 left_tilt = 0 right_tilt = 0 #Start moving target_left = GripTarget(point=left_start, arm="l", yaw=left_tilt, grip=True, pitch=-pi / 2, roll=0) target_right = GripTarget(point=right_start, arm="r", yaw=right_tilt, grip=True, pitch=-pi / 2, roll=0) self.sendTarget(5.0, target_left, target_right) target_left = GripTarget(point=left_end, arm="l", yaw=left_tilt, grip=True, pitch=-pi / 2, roll=0) target_right = GripTarget(point=right_end, arm="r", yaw=right_tilt, grip=True, pitch=-pi / 2, roll=0) self.sendTarget(5.0, target_left, target_right) self.fold_stance("b") return True
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 executeSmooth(self,fold_traj): if fold_traj == False: return False if fold_traj.ignore_smooth: return False if pi/4 <= abs(fold_traj.tilts[0]) <= 3*pi/4: return False scale = 1 if self.is_backwards(fold_traj): scale = -1 StanceUtils.open_gripper("b") self.fold_stance("b") GRIPPER_LENGTH = 0.195 center = fold_traj.smooth_center left_start = self.torso_pt(center) right_start = self.torso_pt(center) left_start.point.y += 0.07 left_start.point.x += scale*0.1 left_start.point.z += GRIPPER_LENGTH + 0.01 right_start.point.y -= 0.07 right_start.point.x += scale*0.1 right_start.point.z += GRIPPER_LENGTH + 0.01 left_end = self.torso_pt(fold_traj.smooth_edges[0]) left_end.point.z += GRIPPER_LENGTH + 0.01 left_end.point.x += scale*0.08 right_end = self.torso_pt(fold_traj.smooth_edges[1]) right_end.point.z += GRIPPER_LENGTH + 0.01 right_end.point.x += scale*0.08 left_tilt = 0 right_tilt = 0 #Start moving target_left =GripTarget(point=left_start,arm="l",yaw=left_tilt,grip=True,pitch=-pi/2,roll=0) target_right = GripTarget(point=right_start,arm="r",yaw=right_tilt,grip=True,pitch=-pi/2,roll=0) self.sendTarget(5.0,target_left,target_right) target_left = GripTarget(point=left_end,arm="l",yaw=left_tilt,grip=True,pitch=-pi/2,roll=0) target_right = GripTarget(point=right_end,arm="r",yaw=right_tilt,grip=True,pitch=-pi/2,roll=0) self.sendTarget(5.0,target_left,target_right) self.fold_stance("b") return True
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