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