def cup_from_front():
yumi.go_to_simple(0.2, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_simple(0.55, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def grab():
yumi.gripper_effort(yumi.LEFT, -10)
#yumi.go_to_simple(0.4, 0.2, 0.03, 0, 3.14, 0, yumi.LEFT)
#yumi.go_to_simple(0.4, 0.2, 0.03, 3.14, 0, 0, yumi.LEFT)
yumi.go_to_simple(0.4, 0.2, 0.3, 0, 3, 0, yumi.LEFT)
yumi.go_to_simple(0.4, 0.2, 0.03, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 20)
def cup_from_top():
yumi.go_to_simple(0.37, 0.2, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.37, 0.2, 0.25, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_simple(0.67, 0.2, 0.25, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def open_grippers(arm):
"""Opens the grippers.
Opens the grippers with an effort of -15 and then relaxes the effort to 0.
:param arm: The side to be opened (moveit_utils LEFT or RIGHT)
:type arm: int
:returns: Nothing
:rtype: None
"""
yumi.gripper_effort(arm, -15.0)
yumi.gripper_effort(arm, 0.0)
def close_grippers(arm):
"""Closes the grippers.
Closes the grippers with an effort of 15 and then relaxes the effort to 0.
:param arm: The side to be closed (moveit_utils LEFT or RIGHT)
:type arm: int
:returns: Nothing
:rtype: None
"""
yumi.gripper_effort(arm, 15.0)
yumi.gripper_effort(arm, 0.0)
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 test_200518():
yumi.go_to_simple(0.25, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.15, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_joints(giving_point_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.15, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_joints(giving_point_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def test_action_with_traverse():
yumi.group_l.set_max_velocity_scaling_factor(1)
yumi.go_to_simple(0.3, 0.1, 0.3, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.traverse_path([[0.3, 0.1, 0.3, 0, 3.14, 0], [0.25, 0.0, 0.15, 0, 3.14, 0]], yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.traverse_path([[0.6, 0.1, 0.2, 0, 3.14, 0]], yumi.LEFT)
def test_200513():
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_simple(0.25, 0.0, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.1, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.25, 0.2, 0.2, 0, 3.14, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
yumi.go_to_simple(0.6, 0.0, 0.2, 1.57, 0, 1.57, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -10)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
def check_grip():
yumi.go_to_simple(0.3, 0.3, 0.3, 0, 1.57, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, -20)
yumi.gripper_effort(yumi.LEFT, 10)
def pick_up():
yumi.gripper_effort(yumi.LEFT, -20)
yumi.go_to_simple(0.5, 0.2, 0.08, 3.14, 0, 0, yumi.LEFT)
yumi.gripper_effort(yumi.LEFT, 20)
yumi.go_to_simple(0.3, 0.2, 0.3, 1.57, 0, 0, yumi.LEFT)
