File size: 2,495 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 | """Constrained pull: draw a wooden bar straight out of a slotted block (drawer-like motion).
The bar starts most of the way inside a through-channel with only its end sticking out. The
channel caps it top and sides, so the only motion that gets it free is a straight pull along
+x -- lift it or turn it and it binds. That constraint is the whole point of the task.
"""
from ..registry import register_task
from ..envs import YamTaskEnv
from ..solvers import pull
from .. import conditions as C
@register_task("bar_pull")
class BarPullTask(YamTaskEnv):
title = "PULL THE BAR OUT OF THE BLOCK"
tags = ["constrained-motion", "pull", "primitive"]
instruction = {"default": "Pull the wooden bar out of the block."}
sleeve_xy = (-0.06, 0.02)
sleeve_length = 0.10
bar_length = 0.15 # must match the CuboidCfg registered in the env
pull_distance = 0.14
# a light clamp: the bar is a small dry cuboid, and squeezing hard wedges it in the channel
gripper_effort, gripper_damping = 62, 85
def _load_scene(self):
s = self.scene.build_sleeve({"name": "sleeve", "xy": self.sleeve_xy,
"length": self.sleeve_length, "width": 0.046, "height": 0.046,
"xy_jitter": (0.015, 0.020)})
# slide the bar in so it is flush at the -x end: the overhang at +x is what we grab
self._overhang = self.bar_length-self.sleeve_length
self._placed = [self.scene.place_object({
"name": "bar",
"xy": (s["xy"][0]+self._overhang/2.0, s["xy"][1]),
# vary how deep it starts in the slot
"xy_jitter": (0.010, 0.0),
# ride on the channel floor, not the table
"z_offset": s["channel_z"]-0.45+0.002,
})]
def solve(self):
# grab the MIDDLE OF THE EXPOSED STUB, not the centroid: the centroid is buried in the
# housing, and the very tip leaves too little bar between the jaws
grip_x = self.bar_length/2.0-self._overhang/2.0
return pull.solve(self, obj="bar", direction=(1.0, 0.0), distance=self.pull_distance,
grasp_offset=(grip_x, 0.0), max_gap=0.05)
def evaluate(self):
return self.check(
C.labelled("bar drawn clear of the block",
C.object_clear_of("bar", "sleeve", extent=self.bar_length/2.0, margin=0.02)),
C.labelled("bar stayed flat", C.object_level("bar", 25.0)))
|