def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005):
duration = compute_duration(plan)
real_time = None if sec_per_step is None else (duration * sec_per_step / time_step)
print('Duration: {} | Step size: {} | Real time: {}'.format(duration, time_step, real_time))
colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots}
for i, t in enumerate(inclusive_range(0, duration, time_step)):
print('Step={} | t={}'.format(i, t))
for action in plan:
name, args, start, duration = action
if not (action.start <= t <= get_end(action)):
continue
if name == 'move':
robot, conf1, traj, conf2 = args
traj = [conf1.values, conf2.values]
fraction = (t - action.start) / action.duration
conf = get_value_at_time(traj, fraction)
body = problem.get_body(robot)
color = COLOR_FROM_NAME[robot]
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
set_base_conf(body, conf)
elif name == 'recharge':
robot, conf = args
body = problem.get_body(robot)
color = YELLOW
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
else:
raise ValueError(name)
if sec_per_step is None:
wait_if_gui('Continue?')
else:
wait_for_duration(sec_per_step)
def get_ik_gen_fn(robot,
brick_from_index,
obstacle_from_name,
max_attempts=10):
movable_joints = get_movable_joints(robot)
tool_link = link_from_name(robot, TOOL_NAME)
disabled_collisions = {
tuple(
link_from_name(robot, link) for link in pair
if has_link(robot, link))
for pair in DISABLED_COLLISIONS
}
sample_fn = get_sample_fn(robot, movable_joints)
def gen_fn(index, pose, grasp):
body = brick_from_index[index].body
#world_pose = get_link_pose(robot, tool_link)
#draw_pose(world_pose, length=0.04)
#set_pose(body, multiply(world_pose, grasp.attach))
#draw_pose(multiply(pose.value, invert(grasp.attach)), length=0.04)
#wait_for_interrupt()
set_pose(body, pose.value)
for _ in range(max_attempts):
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_pose = multiply(pose.value, invert(grasp.attach))
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if attach_conf is None:
continue
approach_pose = multiply(attach_pose, ([0, 0, -0.1], unit_quat()))
#approach_pose = multiply(pose.value, invert(grasp.approach))
approach_conf = inverse_kinematics(robot, tool_link, approach_pose)
if approach_conf is None:
continue
# TODO: retreat
path = plan_waypoints_joint_motion(
robot,
movable_joints, [approach_conf, attach_conf],
obstacles=obstacle_from_name.values(),
self_collisions=True,
disabled_collisions=disabled_collisions)
if path is None:
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
return gen_fn
def get_disabled_collisions(robot):
return {
tuple(
link_from_name(robot, link) for link in pair
if has_link(robot, link))
for pair in DISABLED_COLLISIONS
}
def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
# _, upper = robot_aabb
# radius = upper[0]
extent = get_aabb_extent(robot_aabb)
radius = max(extent[:2]) / 2.
#draw_aabb(robot_aabb)
center, (diameter, height) = approximate_as_cylinder(body)
distance = radius + diameter / 2. + epsilon
_, _, z = get_point(body) # Assuming already placed stably
for [scale] in unit_generator(d=1, use_halton=use_halton):
#theta = PI # 0 | PI
theta = random.uniform(*theta_interval)
position = np.append(distance * unit_from_theta(theta=theta),
[z]) # TODO: halton
yaw = scale * 2 * PI - PI
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose
if draw:
world_pose = multiply(get_link_pose(robot, link), grasp.value)
set_pose(body, world_pose)
handles = draw_pose(get_pose(body), length=1)
wait_for_user()
remove_handles(handles)
#continue
yield (grasp, )
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_interrupt()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_interrupt()
def test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_interrupt()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_interrupt()
def get_motion_fn(robot, brick_from_index, obstacle_from_name):
movable_joints = get_movable_joints(robot)
tool_link = link_from_name(robot, TOOL_NAME)
disabled_collisions = {
tuple(
link_from_name(robot, link) for link in pair
if has_link(robot, link))
for pair in DISABLED_COLLISIONS
}
def fn(conf1, conf2, fluents=[]):
#path = [conf1, conf2]
obstacles = list(obstacle_from_name.values())
attachments = []
for fact in fluents:
if fact[0] == 'atpose':
index, pose = fact[1:]
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles.append(body)
elif fact[0] == 'atgrasp':
index, grasp = fact[1:]
body = brick_from_index[index].body
attachments.append(
Attachment(robot, tool_link, grasp.attach, body))
else:
raise NotImplementedError(fact[0])
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=True,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
return fn
def fn(robot, conf, body, grasp):
conf.assign()
link = link_from_name(robot, BASE_LINK)
world_from_body = multiply(get_link_pose(robot, link), grasp.value)
pose = Pose(body, world_from_body)
pose.assign()
if any(pairwise_collision(body, obst) for obst in problem.obstacles):
return None
return (pose, )
def __init__(self,
robot,
surface,
detect=True,
camera_frame=HEAD_LINK_NAME):
self.robot = robot
self.surface = surface
self.camera_frame = camera_frame
self.link = link_from_name(robot, self.camera_frame)
self.detect = detect
def __init__(self,
robot,
body,
camera_frame=HEAD_LINK_NAME,
max_depth=MAX_KINECT_DISTANCE):
self.robot = robot
self.body = body
self.camera_frame = camera_frame
self.max_depth = max_depth
self.link = link_from_name(robot, camera_frame)
def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
#ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB)
#ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box'))
#print(ycb_path)
#load_pybullet(ycb_path)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False)
#set_euler(table, Euler(yaw=np.pi/2))
with HideOutput(False):
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
#robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf')
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
#robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table)
set_point(block1, Point(z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=+0.25, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, z=block_z))
blocks = [block1, block2, block3]
add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON),
Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED)
set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z))
wait_for_user()
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False)
grasp = y_grasp # fingers won't collide
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user('Finish?')
disconnect()
def test_ik(robot, node_order, node_points):
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for n in node_order:
point = node_points[n]
set_joint_positions(robot, movable_joints, sample_fn())
conf = inverse_kinematics(robot, link, (point, None))
if conf is not None:
set_joint_positions(robot, movable_joints, conf)
continue
link_point, link_quat = get_link_pose(robot, link)
#print(point, link_point)
#user_input('Continue?')
wait_for_interrupt()
def compute_motions(robot, fixed_obstacles, element_bodies, initial_conf,
trajectories):
# TODO: can just plan to initial and then shortcut
# TODO: backoff motion
# TODO: reoptimize for the sequence that have the smallest movements given this
# TODO: sample trajectories
# TODO: more appropriate distance based on displacement/volume
if trajectories is None:
return None
weights = np.array(JOINT_WEIGHTS)
resolutions = np.divide(0.005 * np.ones(weights.shape), weights)
movable_joints = get_movable_joints(robot)
disabled_collisions = {
tuple(link_from_name(robot, link) for link in pair)
for pair in DISABLED_COLLISIONS
}
printed_elements = []
current_conf = initial_conf
all_trajectories = []
for i, print_traj in enumerate(trajectories):
set_joint_positions(robot, movable_joints, current_conf)
goal_conf = print_traj.path[0]
obstacles = fixed_obstacles + [
element_bodies[e] for e in printed_elements
]
path = plan_joint_motion(robot,
movable_joints,
goal_conf,
obstacles=obstacles,
self_collisions=True,
disabled_collisions=disabled_collisions,
weights=weights,
resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
print('Failed to find a path!')
return None
motion_traj = MotionTrajectory(robot, movable_joints, path)
print('{}) {}'.format(i, motion_traj))
all_trajectories.append(motion_traj)
current_conf = print_traj.path[-1]
printed_elements.append(print_traj.element)
all_trajectories.append(print_traj)
# TODO: return to initial?
return all_trajectories
def parse_fluents(robot, brick_from_index, fluents, obstacles):
tool_link = link_from_name(robot, TOOL_NAME)
attachments = []
for fact in fluents:
if fact[0] == 'atpose':
index, pose = fact[1:]
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles.append(body)
elif fact[0] == 'atgrasp':
index, grasp = fact[1:]
body = brick_from_index[index].body
attachments.append(Attachment(robot, tool_link, grasp.attach,
body))
else:
raise NotImplementedError(fact[0])
return attachments
def get_target_point(conf):
# TODO: use full body aabb
robot = conf.body
link = link_from_name(robot, 'torso_lift_link')
#link = BASE_LINK
# TODO: center of mass instead?
# TODO: look such that cone bottom touches at bottom
# TODO: the target isn't the center which causes it to drift
with BodySaver(conf.body):
conf.step()
lower, upper = get_aabb(robot, link)
center = np.average([lower, upper], axis=0)
point = np.array(get_group_conf(conf.body, 'base'))
#point[2] = upper[2]
point[2] = center[2]
#center, _ = get_center_extent(conf.body)
return point
def compute_direction_path(robot, length, reverse, element_body, direction,
collision_fn):
step_size = 0.0025 # 0.005
#angle_step_size = np.pi / 128
angle_step_size = np.math.radians(0.25)
angle_deltas = [-angle_step_size, 0, angle_step_size]
#num_initial = 12
num_initial = 1
steps = np.append(np.arange(-length / 2, length / 2, step_size),
[length / 2])
#print('Length: {} | Steps: {}'.format(length, len(steps)))
#initial_angles = [wrap_angle(angle) for angle in np.linspace(0, 2*np.pi, num_initial, endpoint=False)]
initial_angles = [
wrap_angle(angle)
for angle in np.random.uniform(0, 2 * np.pi, num_initial)
]
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
set_joint_positions(robot, movable_joints, sample_fn())
link = link_from_name(robot, TOOL_NAME)
element_pose = get_pose(element_body)
current_angle, current_conf = optimize_angle(robot, link, element_pose,
steps[0], direction, reverse,
initial_angles, collision_fn)
if current_conf is None:
return None
# TODO: constrain maximum conf displacement
# TODO: alternating minimization for just position and also orientation
trajectory = [current_conf]
for translation in steps[1:]:
#set_joint_positions(robot, movable_joints, current_conf)
initial_angles = [
wrap_angle(current_angle + delta) for delta in angle_deltas
]
current_angle, current_conf = optimize_angle(robot, link, element_pose,
translation, direction,
reverse, initial_angles,
collision_fn)
if current_conf is None:
return None
trajectory.append(current_conf)
return trajectory
def gen(robot, body):
link = link_from_name(robot, BASE_LINK)
with BodySaver(robot):
set_base_conf(robot, np.zeros(3))
robot_pose = get_link_pose(robot, link)
robot_aabb = get_turtle_aabb(robot)
#draw_aabb(robot_aabb)
lower, upper = robot_aabb
center, (diameter, height) = approximate_as_cylinder(body)
_, _, z = get_point(body) # Assuming already placed stably
position = Point(x=upper[0] + diameter / 2., z=z)
for [scale] in halton_generator(d=1):
yaw = scale * 2 * np.pi - np.pi
quat = quat_from_euler(Euler(yaw=yaw))
body_pose = (position, quat)
robot_from_body = multiply(invert(robot_pose), body_pose)
grasp = Pose(body, robot_from_body)
yield (grasp, )
def display_trajectories(ground_nodes, trajectories, time_step=0.05):
if trajectories is None:
return
connect(use_gui=True)
floor, robot = load_world()
wait_for_interrupt()
movable_joints = get_movable_joints(robot)
#element_bodies = dict(zip(elements, create_elements(node_points, elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
connected = set(ground_nodes)
for trajectory in trajectories:
if isinstance(trajectory, PrintTrajectory):
print(trajectory, trajectory.n1 in connected, trajectory.n2
in connected, is_ground(trajectory.element, ground_nodes),
len(trajectory.path))
connected.add(trajectory.n2)
#wait_for_interrupt()
#set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for conf in trajectory.path:
set_joint_positions(robot, movable_joints, conf)
if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
get_link_pose(robot, link_from_name(robot, TOOL_NAME)))
if last_point is not None:
color = (0, 0, 1) if is_ground(trajectory.element,
ground_nodes) else (1, 0, 0)
handles.append(
add_line(last_point, current_point, color=color))
last_point = current_point
wait_for_duration(time_step)
#wait_for_interrupt()
#user_input('Finish?')
wait_for_interrupt()
disconnect()
def __init__(self, robot, surface):
self.robot = robot
self.surface = surface
self.link = link_from_name(robot, HEAD_LINK_NAME)
def get_turtle_aabb(robot):
return get_subtree_aabb(robot, link_from_name(robot, BASE_LINK))
def __init__(self, robot, arm, body):
self.robot = robot
self.arm = arm
self.body = body
self.link = link_from_name(self.robot, PR2_TOOL_FRAMES[self.arm])
def get_print_gen_fn(robot, obstacles, node_points, element_bodies,
ground_nodes):
max_attempts = 150
max_trajectories = 10
check_collisions = True
# 50 doesn't seem to be enough
movable_joints = get_movable_joints(robot)
disabled_collisions = {
tuple(link_from_name(robot, link) for link in pair)
for pair in DISABLED_COLLISIONS
}
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(
get_supported_orders(element_bodies, node_points))
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
def gen_fn(node1, element, fluents=[]):
# TODO: sample collisions while making the trajectory
reverse = (node1 != element[0])
element_body = element_bodies[element]
n1, n2 = reversed(element) if reverse else element
delta = node_points[n2] - node_points[n1]
# if delta[2] < 0:
# continue
length = np.linalg.norm(delta) # 5cm
#supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)}
supporters = []
retrace_supporters(element, incoming_supporters, supporters)
elements_order = [
e for e in element_bodies
if (e != element) and (e not in supporters)
]
bodies_order = [element_bodies[e] for e in elements_order]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles + [element_bodies[e] for e in supporters],
attachments=[],
self_collisions=True,
disabled_collisions=disabled_collisions,
use_limits=True)
trajectories = []
for num in irange(max_trajectories):
for attempt in range(max_attempts):
path = sample_print_path(robot, length, reverse, element_body,
collision_fn)
if path is None:
continue
if check_collisions:
collisions = check_trajectory_collision(
robot, path, bodies_order)
colliding = {
e
for k, e in enumerate(elements_order)
if (element != e) and collisions[k]
}
else:
colliding = set()
if (node_neighbors[n1] <= colliding) and not any(
n in ground_nodes for n in element):
continue
print_traj = PrintTrajectory(robot, movable_joints, path,
element, reverse, colliding)
trajectories.append(print_traj)
if print_traj not in trajectories:
continue
print('{}) {}->{} ({}) | {} | {} | {}'.format(
num, n1, n2, len(supporters), attempt, len(trajectories),
sorted(len(t.colliding) for t in trajectories)))
yield print_traj,
if not colliding:
return
else:
print('{}) {}->{} ({}) | {} | Failure!'.format(
num, len(supporters), n1, n2, max_attempts))
break
return gen_fn
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z + 1),
target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1,
tool_pose=Pose(euler=Euler(yaw=np.pi / 2)),
top_offset=0.02,
grasp_length=0.03,
under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def __init__(self, robot, body):
self.robot = robot
self.body = body
self.link = link_from_name(robot, HEAD_LINK_NAME)
def get_ik_gen_fn(robot,
brick_from_index,
obstacle_from_name,
max_attempts=25):
movable_joints = get_movable_joints(robot)
tool_link = link_from_name(robot, TOOL_NAME)
disabled_collisions = get_disabled_collisions(robot)
sample_fn = get_sample_fn(robot, movable_joints)
approach_distance = 0.1
#approach_distance = 0.0
approach_vector = approach_distance * np.array([0, 0, -1])
def gen_fn(index, pose, grasp):
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles = list(obstacle_from_name.values()) # + [body]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles=obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=get_custom_limits(robot))
attach_pose = multiply(pose.value, invert(grasp.attach))
approach_pose = multiply(attach_pose, (approach_vector, unit_quat()))
# approach_pose = multiply(pose.value, invert(grasp.approach))
for _ in range(max_attempts):
if IK_FAST:
attach_conf = sample_tool_ik(robot, attach_pose)
else:
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if (attach_conf is None) or collision_fn(attach_conf):
continue
set_joint_positions(robot, movable_joints, attach_conf)
if IK_FAST:
approach_conf = sample_tool_ik(robot,
approach_pose,
nearby_conf=attach_conf)
else:
approach_conf = inverse_kinematics(robot, tool_link,
approach_pose)
if (approach_conf is None) or collision_fn(approach_conf):
continue
set_joint_positions(robot, movable_joints, approach_conf)
path = plan_direct_joint_motion(
robot,
movable_joints,
attach_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
if path is None: # TODO: retreat
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
return gen_fn
