def move_and_grasp(arm, pose_ee, grip_effort):
try:
yumi.traverse_path([pose_ee], arm, 10)
except Exception:
if (arm == yumi.LEFT):
yumi.plan_and_move(yumi.group_l, yumi.create_pose_euler(pose_ee[0], pose_ee[1], pose_ee[2], pose_ee[3], pose_ee[4], pose_ee[5]))
elif (arm == yumi.RIGHT):
yumi.plan_and_move(yumi.group_r, yumi.create_pose_euler(pose_ee[0], pose_ee[1], pose_ee[2], pose_ee[3], pose_ee[4], pose_ee[5]))
if (grip_effort <= 20 and grip_effort >= -20):
yumi.gripper_effort(arm, grip_effort)
else:
print("The gripper effort values should be in the range [-20, 20]")
def handle_move_simple(req):
print "arm - move_simple - request received! Executing..."
result = yumi.move_simple(req.arm, yumi.create_pose_euler(req.x, req.y, req.z, req.roll, req.pitch, req.yaw))
if (result==1):
"arm - move_simple - request executed !"
else:
"arm - move_simple - execution failed !"
return ArmInterfaceResponse(result)
def __init__(self):
self._tfsub = tf.TransformListener()
self.state_lis = rospy.Subscriber('/state', String,
self.state_callback)
yumi.init_Moveit()
while not rospy.is_shutdown():
start = time.time()
if (self.need_adj == True):
try:
(trans_adr,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adr', rospy.Time(0))
(trans_adrr,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adrr', rospy.Time(0))
(trans_adl,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adl', rospy.Time(0))
(trans_adll,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adll', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
yoffset = yoff - (gripperoff) * np.cos(pi / 4)
x_r = trans_adr[0] + xoff
y_r = trans_adr[1] + yoffset
y_rr = trans_adrr[1] + yoffset
x_l = trans_adl[0] + xoff
y_l = trans_adl[1] - yoffset
y_ll = trans_adll[1] - yoffset
self.need_adj = False
yumi.reset_arm_home(BOTH)
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.25, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.25, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_ll, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_rr, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.25, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.25, pi / 4, pi, pi))
yumi.reset_arm_home(BOTH)
yumi.reset_arm_cal(BOTH)
rospy.sleep(3)
else:
try:
(trans,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/shoe_hole',
rospy.Time(0))
(trans_norm,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/pre_put',
rospy.Time(0))
(trans_pick,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/pick', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
x = trans[0]
y = trans[1]
z = trans[2]
xn = trans_norm[0]
yn = trans_norm[1]
zn = trans_norm[2]
xp = trans_pick[0]
yp = trans_pick[1]
zp = trans_pick[2]
if (0 < xn <= x and zn >= z > 0 and 0 < x < xp):
a = np.arctan2((y - yn), (zn - z))
b = np.arctan2((x - xn), (zn - z))
c = np.arctan2((yp - y), (xp - x))
if (-0.5 * pi <= a <= 0.5 * pi and 0 <= b <= 0.5 * pi
and -0.5 * pi <= c <= 0.5 * pi):
zoffset = gripperoff * np.cos(a) * np.cos(b) + zoff
yoffset = yoff - (gripperoff) * np.sin(a)
xoffset = xoff - (gripperoff) * np.sin(b)
b = b + pi
c = 0.5 * pi + c - 0.10
#set up 6D pose----------------
x = x + xoffset
y = y + yoffset
z = z + zoffset
xn = xn + xoffset
yn = yn + yoffset
zn = zn + zoffset
xp = xp + xoff
yp = yp + yoff
zp = zp + zoff + gripperoff
print(x, y, z, xn, yn, zn, a, b)
if (np.isnan(a) == False and np.isnan(b) == False):
#'''
pose_norm = [xn, yn, zn, a, b, pi]
pose = [x, y, z, a, b, pi]
yumi.reset_arm(RIGHT)
yumi.move_and_grasp(yumi.RIGHT, pose_norm, 10.0)
yumi.move_and_grasp(yumi.RIGHT, pose, -10.0)
yumi.move_and_grasp(yumi.RIGHT, pose_norm, -10.0)
yumi.reset_arm(RIGHT)
yumi.reset_arm_cal(RIGHT)
#'''
pose_pick = [xp, yp, 0.18 + 0.08, 0, pi, c]
pose_grab = [xp, yp, 0.18, 0, pi, c]
yumi.reset_arm_home(LEFT)
yumi.move_and_grasp(yumi.LEFT, pose_pick, -10.0)
yumi.move_and_grasp(yumi.LEFT, pose_grab, 10.0)
yumi.move_and_grasp(yumi.LEFT, pose_pick, 10.0)
yumi.reset_arm_home(LEFT)
yumi.reset_arm_cal(LEFT)
#'''
print time.time() - start
rospy.on_shutdown(done)