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 to_points(self, link=BASE_LINK):
points = []
for conf in self.path:
with BodySaver(conf.body):
conf.step()
#point = np.array(point_from_pose(get_link_pose(conf.body, link)))
point = np.array(get_group_conf(conf.body, 'base'))
point[2] = 0
point += 1e-2 * np.array([0, 0, 1])
if not (points
and np.allclose(points[-1], point, atol=1e-3, rtol=0)):
points.append(point)
points = get_target_path(self)
return waypoints_from_path(points)
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 fn(bq1, bq2):
if teleport:
path = [bq1, bq2]
elif not is_drake_pr2(robot):
set_pose(robot, bq1.value)
#start_conf = base_values_from_pose(bq1.value)
goal_conf = base_values_from_pose(bq2.value)
raw_path = plan_base_motion(robot,
goal_conf,
BASE_LIMITS,
obstacles=fixed)
if raw_path is None:
print('Failed motion plan!')
return None
path = [
Pose(robot, pose_from_base_values(q, bq1.value))
for q in raw_path
]
else:
base_joints = get_group_joints(robot, 'base')
if PLAN_JOINTS:
# set_pose(robot, unit_pose())
# set_group_conf(robot, 'base', start_conf)
set_joint_positions(robot, base_joints, bq1.values)
raw_path = plan_joint_motion(robot,
base_joints,
bq2.values,
obstacles=fixed,
self_collisions=False)
else:
with BodySaver(robot):
set_base_values(robot, bq1.values)
set_joint_positions(robot, base_joints,
np.zeros(len(base_joints)))
raw_path = plan_base_motion(robot,
bq2.values,
BASE_LIMITS,
obstacles=fixed)
if raw_path is None:
print('Failed motion plan!')
return None
#path = [Pose(robot, pose_from_base_values(q, bq1.value)) for q in raw_path]
path = [Conf(robot, base_joints, q) for q in raw_path]
bt = Trajectory(path)
return (bt, )
def move_look_trajectory(task,
trajectory,
max_tilt=np.pi / 6): # max_tilt=INF):
# TODO: implement a minimum distance instead of max_tilt
# TODO: pr2 movement restrictions
#base_path = [pose.to_base_conf() for pose in trajectory.path]
base_path = trajectory.path
if not base_path:
return trajectory
obstacles = get_fixed_bodies(task) # TODO: movable objects
robot = base_path[0].body
target_path = get_target_path(trajectory)
waypoints = []
index = 0
with BodySaver(robot):
#current_conf = base_values_from_pose(get_pose(robot))
for i, conf in enumerate(base_path): # TODO: just do two loops?
conf.assign()
while index < len(target_path):
if i < index:
# Don't look at past or current conf
target_point = target_path[index]
head_conf = inverse_visibility(
robot, target_point) # TODO: this is slightly slow
#print(index, target_point, head_conf)
if (head_conf is not None) and (head_conf[1] < max_tilt):
break
index += 1
else:
head_conf = get_group_conf(robot, 'head')
set_group_conf(robot, 'head', head_conf)
#print(i, index, conf.values, head_conf) #, get_pose(robot))
waypoints.append(np.concatenate([conf.values, head_conf]))
joints = tuple(base_path[0].joints) + tuple(get_group_joints(
robot, 'head'))
#joints = get_group_joints(robot, 'base') + get_group_joints(robot, 'head')
#set_pose(robot, unit_pose())
#set_group_conf(robot, 'base', current_conf)
path = plan_waypoints_joint_motion(robot,
joints,
waypoints,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS)
return create_trajectory(robot, joints, path)
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, )