def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def are_visible(world):
ray_names = []
rays = []
for name in world.movable:
for camera, info in world.cameras.items():
camera_pose = get_pose(info.body)
camera_point = point_from_pose(camera_pose)
point = point_from_pose(get_pose(world.get_body(name)))
if is_visible_point(CAMERA_MATRIX,
KINECT_DEPTH,
point,
camera_pose=camera_pose):
ray_names.append(name)
rays.append(Ray(camera_point, point))
ray_results = batch_ray_collision(rays)
visible_indices = [
idx for idx, (name, result) in enumerate(zip(ray_names, ray_results))
if result.objectUniqueId == world.get_body(name)
]
visible_names = {ray_names[idx] for idx in visible_indices}
print('Detected:', sorted(visible_names))
if has_gui():
handles = [
add_line(rays[idx].start, rays[idx].end, color=BLUE)
for idx in visible_indices
]
wait_for_duration(1.0)
remove_handles(handles)
# TODO: the object drop seems to happen around here
return visible_names
def main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput():
robot = load_model(MOVO_URDF, fixed_base=True)
for link in get_links(robot):
set_color(robot,
color=apply_alpha(0.25 * np.ones(3), 1),
link=link)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
dump_body(robot)
wait_for_user('Start?')
#for arm in ARMS:
# test_close_gripper(robot, arm)
arm = 'right'
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
print(conf)
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_for_user()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_for_user()
test_retraction(robot,
MOVO_INFOS[arm],
tool_link,
fixed_joints=fixed_joints,
max_time=0.1)
disconnect()
def draw(self, **kwargs):
with LockRenderer(True):
remove_handles(self.handles)
self.handles = []
with WorldSaver():
for name, pose_dist in self.pose_dists.items():
self.handles.extend(
pose_dist.draw(color=self.color_from_name[name],
**kwargs))
def follow_curve(robot,
joints,
curve,
goal_t=None,
time_step=None,
max_velocities=MAX_VELOCITIES,
**kwargs):
if goal_t is None:
goal_t = curve.x[-1]
if time_step is None:
time_step = 10 * get_time_step()
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=max_velocities,
norm=INF) # get_distance
positions = np.array(get_joint_positions(robot, joints))
closest_dist, closest_t = find_closest(positions,
curve,
t_range=(curve.x[0], goal_t),
max_time=1e-1,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
print('Closest dist: {:.3f} | Closest time: {:.3f}'.format(
closest_dist, closest_t))
target_t = closest_t
# TODO: condition based on closest_dist
while True:
print('\nTarget time: {:.3f} | Goal time: {}'.format(target_t, goal_t))
target_positions = curve(target_t)
target_velocities = curve(target_t, nu=1) # TODO: draw the velocity
#print('Positions: {} | Velocities: {}'.format(target_positions, target_velocities))
handles = draw_waypoint(target_positions)
is_goal = (target_t == goal_t)
position_tol = 1e-2 if is_goal else 1e-2
for output in control_state(robot,
joints,
target_positions=target_positions,
target_velocities=target_velocities,
position_tol=position_tol,
velocity_tol=INF,
max_velocities=max_velocities,
**kwargs):
yield output
remove_handles(handles)
target_t = min(goal_t, target_t + time_step)
if is_goal:
break
def visualize_learned_pours(learner, bowl_type='blue_bowl', cup_type='red_cup', num_steps=None):
learner.query_type = REJECTION # BEST | REJECTION | STRADDLE
learner.validity_learner = None
world = create_world()
#add_obstacles()
#environment = get_bodies()
world.bodies[bowl_type] = load_cup_bowl(bowl_type)
world.bodies[cup_type] = load_cup_bowl(cup_type)
feature = get_pour_feature(world, bowl_type, cup_type)
draw_pose(get_pose(world.get_body(bowl_type)), length=0.2)
# TODO: sample different sizes
print('Feature:', feature)
for parameter in learner.parameter_generator(world, feature, valid=False):
handles = []
print('Parameter:', parameter)
valid = is_valid_pour(world, feature, parameter)
score = learner.score(feature, parameter)
x = learner.func.x_from_feature_parameter(feature, parameter)
[prediction] = learner.predict_x(np.array([x]), noise=True)
print('Valid: {} | Mean: {:.3f} | Var: {:.3f} | Score: {:.3f}'.format(
valid, prediction['mean'], prediction['variance'], score))
#fraction = rescale(clip(prediction['mean'], *DEFAULT_INTERVAL), DEFAULT_INTERVAL, new_interval=(0, 1))
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN)
#set_color(world.get_body(cup_type), apply_alpha(color, alpha=0.25))
# TODO: overlay many cups at different poses
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
#handles.extend(draw_pose(bowl_from_pivot))
handles.extend(draw_point(point_from_pose(bowl_from_pivot), color=BLACK))
# TODO: could label bowl, cup dimensions
path, _ = pour_path_from_parameter(world, feature, parameter, cup_yaw=0)
for p1, p2 in safe_zip(path[:-1], path[1:]):
handles.append(add_line(point_from_pose(p1), point_from_pose(p2), color=RED))
if num_steps is not None:
path = path[:num_steps]
for i, pose in enumerate(path[::-1]):
#step_handles = draw_pose(pose, length=0.1)
#step_handles = draw_point(point_from_pose(pose), color=BLACK)
set_pose(world.get_body(cup_type), pose)
print('Step {}/{}'.format(i+1, len(path)))
wait_for_user()
#remove_handles(step_handles)
remove_handles(handles)
def test_grasps(robot, block):
for arm in ARMS:
gripper_joints = get_gripper_joints(robot, arm)
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
tool_pose = get_link_pose(robot, tool_link)
#handles = draw_pose(tool_pose)
#grasps = get_top_grasps(block, under=True, tool_pose=unit_pose())
grasps = get_side_grasps(block, under=True, tool_pose=unit_pose())
for i, grasp_pose in enumerate(grasps):
block_pose = multiply(tool_pose, grasp_pose)
set_pose(block, block_pose)
close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm),
closed_conf=get_closed_positions(robot, arm))
handles = draw_pose(block_pose)
wait_if_gui('Grasp {}'.format(i))
remove_handles(handles)
def solve_collision_free(door,
obstacle,
max_iterations=100,
step_size=math.radians(5),
min_distance=2e-2,
draw=True):
joints = get_movable_joints(door)
door_link = child_link_from_joint(joints[-1])
# print(get_com_pose(door, door_link))
# print(get_link_inertial_pose(door, door_link))
# print(get_link_pose(door, door_link))
# draw_pose(get_com_pose(door, door_link))
handles = []
success = False
start_time = time.time()
for iteration in range(max_iterations):
current_conf = np.array(get_joint_positions(door, joints))
collision_infos = get_closest_points(door,
obstacle,
link1=door_link,
max_distance=min_distance)
if not collision_infos:
success = True
break
collision_infos = sorted(collision_infos,
key=lambda info: info.contactDistance)
collision_infos = collision_infos[:1] # TODO: average all these
if draw:
for collision_info in collision_infos:
handles.extend(draw_collision_info(collision_info))
wait_if_gui()
[collision_info] = collision_infos[:1]
distance = collision_info.contactDistance
print(
'Iteration: {} | Collisions: {} | Distance: {:.3f} | Time: {:.3f}'.
format(iteration, len(collision_infos), distance,
elapsed_time(start_time)))
if distance >= min_distance:
success = True
break
# TODO: convergence or decay in step size
direction = step_size * get_unit_vector(
collision_info.contactNormalOnB) # B->A (already normalized)
contact_point = collision_info.positionOnA
#com_pose = get_com_pose(door, door_link) # TODO: be careful here
com_pose = get_link_pose(door, door_link)
local_point = tform_point(invert(com_pose), contact_point)
#local_point = unit_point()
translate, rotate = compute_jacobian(door,
door_link,
point=local_point)
delta_conf = np.array([
np.dot(translate[mj], direction) # + np.dot(rotate[mj], direction)
for mj in movable_from_joints(door, joints)
])
new_conf = current_conf + delta_conf
if violates_limits(door, joints, new_conf):
break
set_joint_positions(door, joints, new_conf)
if draw:
wait_if_gui()
remove_handles(handles)
print('Success: {} | Iteration: {} | Time: {:.3f}'.format(
success, iteration, elapsed_time(start_time)))
#quit()
return success
def iterate(self, state):
handles = self.draw()
steps = int(math.ceil(self.duration / 0.02))
for _ in range(steps):
yield
remove_handles(handles)
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
def follow_curve_old(robot, joints, curve, goal_t=None):
# TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py
if goal_t is None:
goal_t = curve.x[-1]
time_step = get_time_step()
target_step = 10 * time_step
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=MAX_VELOCITIES,
norm=INF)
for i in irange(INF):
# if (i % 10) != 0:
# continue
current_p = np.array(get_joint_positions(robot, joints))
current_v = np.array(get_joint_velocities(robot, joints))
goal_dist = distance_fn(current_p, curve(goal_t))
print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format(
current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist))
if goal_dist < 1e-2:
return True
# _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS,
# dt_max=1e-1, max_time=1e-1)
# assert connection is not None
# target_t = 0.5*connection.x[-1]
# target_p = connection(target_t)
# target_v = connection(target_t, nu=1)
# #print(target_p)
closest_dist, closest_t = find_closest(current_p,
curve,
t_range=None,
max_time=1e-2,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
target_t = min(closest_t + target_step, curve.x[-1])
target_p = curve(target_t)
#target_v = curve(target_t, nu=1)
target_v = curve(closest_t, nu=1)
#target_v = MAX_VELOCITIES
#target_v = INF*np.zeros(len(joints))
handles = draw_waypoint(target_p)
#times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max,
# set_joint_positions(robot, joints, target_p)
max_velocity_control_joints(robot,
joints,
positions=target_p,
velocities=target_v,
max_velocities=MAX_VELOCITIES)
#next_t = closest_t + time_step
#next_p = current_p + current_v*time_step
yield target_t
actual_p = np.array(get_joint_positions(robot, joints))
actual_v = np.array(get_joint_velocities(robot, joints))
next_p = current_p + actual_v * time_step
print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS),
actual_p.round(N_DIGITS)))
remove_handles(handles)
