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 optimize_angle(robot,
link,
element_pose,
translation,
direction,
reverse,
initial_angles,
collision_fn,
max_error=1e-2):
movable_joints = get_movable_joints(robot)
initial_conf = get_joint_positions(robot, movable_joints)
best_error, best_angle, best_conf = max_error, None, None
for i, angle in enumerate(initial_angles):
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
target_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, target_pose)
# if conf is None:
# continue
#if pairwise_collision(robot, robot):
conf = get_joint_positions(robot, movable_joints)
if not collision_fn(conf):
link_pose = get_link_pose(robot, link)
error = get_distance(point_from_pose(target_pose),
point_from_pose(link_pose))
if error < best_error: # TODO: error a function of direction as well
best_error, best_angle, best_conf = error, angle, conf
# wait_for_interrupt()
if i != len(initial_angles) - 1:
set_joint_positions(robot, movable_joints, initial_conf)
#print(best_error, translation, direction, best_angle)
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
#wait_for_interrupt()
return best_angle, best_conf
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 apply(self, state, **kwargs):
# TODO: identify surface automatically
with LockRenderer():
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
wait_for_duration(1e-2)
if self.detect:
# TODO: the collision geometries are being visualized
# TODO: free the renderer
detections = get_visual_detections(self.robot,
camera_link=self.camera_frame)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
#state.localized.update(detections)
# TODO: pose for each object that can be real or fake
yield
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 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 apply(self, state, **kwargs):
mesh, _ = get_detection_cone(self.robot,
self.body,
depth=MAX_KINECT_DISTANCE)
assert (mesh is not None)
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(self._duration)
# time.sleep(1.0)
remove_body(cone)
state.registered.add(self.body)
def apply(self, state, **kwargs):
# TODO: identify surface automatically
cone = get_viewcone(color=(1, 0, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(self._duration) # TODO: don't sleep if no viewer?
remove_body(cone)
detections = get_visual_detections(self.robot)
print('Detections:', detections)
for body, dist in state.b_on.items():
obs = (body in detections) and (is_center_stable(
body, self.surface))
if obs or (self.surface not in state.task.rooms):
# TODO: make a command for scanning a room instead?
dist.obsUpdate(get_observation_fn(self.surface), obs)
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 apply(self, state, **kwargs):
mesh, _ = get_detection_cone(self.robot,
self.body,
camera_link=self.camera_frame,
depth=self.max_depth)
if mesh is None:
wait_for_user()
assert (mesh is not None)
with LockRenderer():
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration)
remove_body(cone)
wait_for_duration(1e-2)
state.registered.add(self.body)
yield
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 apply(self, state, **kwargs):
# TODO: check if actually can register
mesh, _ = get_detection_cone(self.robot,
self.body,
camera_link=self.camera_frame,
depth=self.max_depth)
if mesh is None:
wait_for_user()
assert (
mesh is not None
) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions
with LockRenderer():
cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5))
set_pose(cone, get_link_pose(self.robot, self.link))
wait_for_duration(1e-2)
wait_for_duration(self._duration)
remove_body(cone)
wait_for_duration(1e-2)
state.registered.add(self.body)
yield
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()
