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
