Spaces:
Runtime error
Runtime error
File size: 3,775 Bytes
8fc2b4e |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 |
"""Motion primitives."""
import numpy as np
from cliport.utils import utils
class PickPlace():
"""Pick and place primitive."""
def __init__(self, height=0.32, speed=0.01):
self.height, self.speed = height, speed
def __call__(self, movej, movep, ee, pose0, pose1):
"""Execute pick and place primitive.
Args:
movej: function to move robot joints.
movep: function to move robot end effector pose.
ee: robot end effector.
pose0: SE(3) picking pose.
pose1: SE(3) placing pose.
Returns:
timeout: robot movement timed out if True.
"""
pick_pose, place_pose = pose0, pose1
# Execute picking primitive.
prepick_to_pick = ((0, 0, 0.32), (0, 0, 0, 1))
postpick_to_pick = ((0, 0, self.height), (0, 0, 0, 1))
prepick_pose = utils.multiply(pick_pose, prepick_to_pick)
postpick_pose = utils.multiply(pick_pose, postpick_to_pick)
timeout = movep(prepick_pose)
# Move towards pick pose until contact is detected.
delta = (np.float32([0, 0, -0.001]),
utils.eulerXYZ_to_quatXYZW((0, 0, 0)))
targ_pose = prepick_pose
while not ee.detect_contact(): # and target_pose[2] > 0:
targ_pose = utils.multiply(targ_pose, delta)
timeout |= movep(targ_pose)
if timeout:
return True
# Activate end effector, move up, and check picking success.
ee.activate()
timeout |= movep(postpick_pose, self.speed)
pick_success = ee.check_grasp()
# Execute placing primitive if pick is successful.
if pick_success:
preplace_to_place = ((0, 0, self.height), (0, 0, 0, 1))
postplace_to_place = ((0, 0, 0.32), (0, 0, 0, 1))
preplace_pose = utils.multiply(place_pose, preplace_to_place)
postplace_pose = utils.multiply(place_pose, postplace_to_place)
targ_pose = preplace_pose
while not ee.detect_contact():
targ_pose = utils.multiply(targ_pose, delta)
timeout |= movep(targ_pose, self.speed)
if timeout:
return True
ee.release()
timeout |= movep(postplace_pose)
# Move to prepick pose if pick is not successful.
else:
ee.release()
timeout |= movep(prepick_pose)
return timeout
def push(movej, movep, ee, pose0, pose1): # pylint: disable=unused-argument
"""Execute pushing primitive.
Args:
movej: function to move robot joints.
movep: function to move robot end effector pose.
ee: robot end effector.
pose0: SE(3) starting pose.
pose1: SE(3) ending pose.
Returns:
timeout: robot movement timed out if True.
"""
# Adjust push start and end positions.
pos0 = np.float32((pose0[0][0], pose0[0][1], 0.005))
pos1 = np.float32((pose1[0][0], pose1[0][1], 0.005))
vec = np.float32(pos1) - np.float32(pos0)
length = np.linalg.norm(vec)
vec = vec / length
pos0 -= vec * 0.02
pos1 -= vec * 0.05
# Align spatula against push direction.
theta = np.arctan2(vec[1], vec[0])
rot = utils.eulerXYZ_to_quatXYZW((0, 0, theta))
over0 = (pos0[0], pos0[1], 0.31)
over1 = (pos1[0], pos1[1], 0.31)
# Execute push.
timeout = movep((over0, rot))
timeout |= movep((pos0, rot))
n_push = np.int32(np.floor(np.linalg.norm(pos1 - pos0) / 0.01))
for _ in range(n_push):
target = pos0 + vec * n_push * 0.01
timeout |= movep((target, rot), speed=0.003)
timeout |= movep((pos1, rot), speed=0.003)
timeout |= movep((over1, rot))
return timeout
|