File size: 2,836 Bytes
7399b6f | 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 | """Bimanual L-shaped relay push: each arm drives one leg, and neither can do the other's.
The route turns a corner on purpose. The right arm's shoulder is at y=-0.2 and the left arm's at
y=+0.2, so an arm can only push comfortably AWAY from its own shoulder: the right arm can drive
the block in +x, the left arm can drive it in -y. Getting the block from the start to the pad
needs both, in that order.
A straight-line relay does not work here, and not for a tuning reason: pushing along -y means
standing on the block's +y side, which the right arm can only reach by stretching over the block
itself -- the approach swipes it off the table.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import push
from .. import conditions as C
@register_task("push_relay")
class PushRelayTask(YamTaskEnv):
title = "RELAY-PUSH THE BLOCK AROUND THE CORNER"
tags = ["bimanual", "push", "no-grasp", "coordination", "primitive"]
instruction = {"default": "Push the block onto the pad, using both arms in turn."}
bimanual = True
# Both legs run DIAGONALLY (+x and -y together), which is the direction each arm naturally
# pushes: the left shoulder is at (-0.2,+0.2) and the right at (-0.2,-0.2), so a hand stands
# on the near side of the block and drives it away and across. A leg 1 out at y=+0.12 with
# the RIGHT arm never reached the block at all -- it sat at 0.20 m of error for 220 steps.
block_spawn = (-0.15, 0.12) # deep in the left arm's band
spawn_jitter = (0.018, 0.015)
corner_xy = (0.02, -0.02) # end of the left arm's diagonal leg
target_xy = (0.18, -0.10) # end of the right arm's diagonal leg; left arm cannot reach
target_size = 0.11
gripper_effort, gripper_damping = 78, 88
def _load_scene(self):
self.scene.build_marker({"name": "corner", "xy": self.corner_xy, "size": 0.085,
"color": (0.55, 0.55, 0.58), "xy_jitter": (0.012, 0.012)})
self.scene.build_marker({"name": "target", "xy": self.target_xy, "size": self.target_size,
"color": (0.90, 0.68, 0.15), "xy_jitter": (0.012, 0.012)})
self._placed = [self.scene.place_object(
{"name": "cube_y", "xy": self.block_spawn, "xy_jitter": self.spawn_jitter})]
def solve(self):
return push.relay(self, obj="cube_y", legs=[
{"arm": "left", "target": "corner"}, # diagonal, away from the left shoulder
{"arm": "right", "target": "target"}, # diagonal, away from the right shoulder
])
def evaluate(self):
return self.check(
C.labelled("block reached the pad", C.object_moved_to("cube_y", "target", tol=0.07)),
C.labelled("block never left the table", C.object_level("cube_y", 40.0)))
|