def _initialize_ik(self, urdf_path):
if not USE_TRACK_IK:
self.ik_solver = None
return
from trac_ik_python.trac_ik import IK # killall -9 rosmaster
base_link = get_link_name(
self.robot, parent_link_from_joint(self.robot, self.arm_joints[0]))
tip_link = get_link_name(self.robot,
child_link_from_joint(self.arm_joints[-1]))
# limit effort and velocities are required
# solve_type: Speed, Distance, Manipulation1, Manipulation2
# TODO: fast solver and slow solver
self.ik_solver = IK(base_link=str(base_link),
tip_link=str(tip_link),
timeout=0.01,
epsilon=1e-5,
solve_type="Speed",
urdf_string=read(urdf_path))
if not CONSERVITIVE_LIMITS:
return
lower, upper = self.ik_solver.get_joint_limits()
buffer = JOINT_LIMITS_BUFFER * np.ones(len(self.ik_solver.joint_names))
lower, upper = lower + buffer, upper - buffer
lower[6] = -MAX_FRANKA_JOINT7
upper[6] = +MAX_FRANKA_JOINT7
self.ik_solver.set_joint_limits(lower, upper)
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE):
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
#half_extent = 1.0*FINGER_EXTENT[2] # Collides
half_extent = 1.05 * FINGER_EXTENT[2]
grasps = []
for link in get_link_subtree(world.kitchen, joint):
if 'handle' in get_link_name(world.kitchen, link):
# TODO: can adjust the position and orientation on the handle
for yaw in [0, np.pi]: # yaw=0 DOESN'T WORK WITH LULA
handle_grasp = (Point(z=-half_extent),
quat_from_euler(
Euler(roll=np.pi, pitch=np.pi / 2,
yaw=yaw)))
#if not pull:
# handle_pose = get_link_pose(world.kitchen, link)
# for distance in np.arange(0., 0.05, step=0.001):
# pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp)
# tool_pose = multiply(handle_pose, invert(pregrasp))
# set_tool_pose(world, tool_pose)
# # TODO: check collisions
# wait_for_user()
handle_pregrasp = multiply((pre_direction, unit_quat()),
handle_grasp)
grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp))
return grasps
def test_clone_arm(pr2):
first_joint_name = PR2_GROUPS['left_arm'][0]
first_joint = joint_from_name(pr2, first_joint_name)
parent_joint = get_link_parent(pr2, first_joint)
print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name,
first_joint)
print(get_link_descendants(pr2,
first_joint)) # TODO: least common ancestor
links = [first_joint] + get_link_descendants(pr2, first_joint)
new_arm = clone_body(pr2, links=links, collision=False)
dump_world()
set_base_values(pr2, (-2, 0, 0))
wait_for_interrupt()
def test(at, o, wp):
if not collisions:
return True
# TODO: check door collisions
# TODO: still need to check static links at least once
if isinstance(wp, SurfaceDist):
return True # TODO: perform this probabilistically
wp.assign()
state = at.context.copy()
state.assign()
all_bodies = {
body
for command in at.commands for body in command.bodies
}
for command in at.commands:
obstacles = get_link_obstacles(world, o) - all_bodies
# TODO: why did I previously remove o at p?
#obstacles = get_link_obstacles(world, o) - command.bodies # - p.bodies # Doesn't include o at p
if not obstacles:
continue
if isinstance(command, DoorTrajectory):
[door_joint] = command.door_joints
surface_name = get_link_name(world.kitchen,
child_link_from_joint(door_joint))
if wp.support == surface_name:
return True
for _ in command.iterate(state):
state.derive()
#for attachment in state.attachments.values():
# if any(pairwise_collision(attachment.child, obst) for obst in obstacles):
# return False
# TODO: just check collisions with moving links
if any(
pairwise_collision(world.robot, obst)
for obst in obstacles):
#print(at, o, p)
#wait_for_user()
return False
return True
def collect_place(world, object_name, surface_name, grasp_type, args):
date = get_date()
#set_seed(args.seed)
#dump_body(world.robot)
surface_pose = get_surface_reference_pose(world.kitchen, surface_name) # TODO: assumes the drawer is open
stable_gen_fn = get_stable_gen(world, z_offset=Z_EPSILON, visibility=False,
learned=False, collisions=not args.cfree)
grasp_gen_fn = get_grasp_gen(world)
ik_ir_gen = get_pick_gen_fn(world, learned=False, collisions=not args.cfree, teleport=args.teleport)
stable_gen = stable_gen_fn(object_name, surface_name)
grasps = list(grasp_gen_fn(object_name, grasp_type))
robot_name = get_body_name(world.robot)
path = get_place_path(robot_name, surface_name, grasp_type)
print(SEPARATOR)
print('Robot name: {} | Object name: {} | Surface name: {} | Grasp type: {} | Filename: {}'.format(
robot_name, object_name, surface_name, grasp_type, path))
entries = []
start_time = time.time()
failures = 0
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time): #and (failures <= max_failures):
(rel_pose,) = next(stable_gen)
if rel_pose is None:
break
(grasp,) = random.choice(grasps)
with LockRenderer(lock=True):
result = next(ik_ir_gen(object_name, rel_pose, grasp), None)
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
# TODO: ensure an arm motion exists
bq, aq, at = result
rel_pose.assign()
bq.assign()
aq.assign()
base_pose = get_link_pose(world.robot, world.base_link)
object_pose = rel_pose.get_world_from_body()
tool_pose = multiply(object_pose, invert(grasp.grasp_pose))
entries.append({
'tool_from_base': multiply(invert(tool_pose), base_pose),
'surface_from_object': multiply(invert(surface_pose), object_pose),
'base_from_object': multiply(invert(base_pose), object_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
if has_gui():
wait_for_user()
#visualize_database(tool_from_base_list)
if not entries:
safe_remove(path)
return None
# Assuming the kitchen is fixed but the objects might be open world
data = {
'date': date,
'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'surface_name': surface_name,
'object_name': object_name,
'grasp_type': grasp_type,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
write_json(path, data)
print('Saved', path)
return data
def pdddlstream_from_problem(belief,
additional_init=[],
fixed_base=True,
**kwargs):
world = belief.world # One world per state
task = world.task # One task per world
print(task)
domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl'))
# TODO: repackage stream outputs to avoid recomputation
# Despite the base not moving, it could be re-estimated
init_bq = belief.base_conf
init_aq = belief.arm_conf
init_gq = belief.gripper_conf
carry_aq = world.carry_conf
init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq
# TODO: the following doesn't work. Maybe because carry_conf is used elsewhere
#carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf
#calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf
# Don't need this now that returning to old confs
#open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq
#closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq
open_gq = world.open_gq
closed_gq = world.closed_gq
constant_map = {
'@world': 'world',
'@gripper': 'gripper',
'@stove': 'stove',
'@none': None,
'@rest_aq': carry_aq,
#'@calibrate_aq': calibrate_aq,
'@open_gq': open_gq,
'@closed_gq': closed_gq,
'@open': OPEN,
'@closed': CLOSED,
'@top': TOP_GRASP,
'@side': SIDE_GRASP,
'@bq0': init_bq,
}
top_joint = JOINT_TEMPLATE.format(TOP_DRAWER)
bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER)
init = [
('BConf', init_bq),
('AtBConf', init_bq),
('AConf', init_bq, carry_aq),
#('RestAConf', carry_aq),
#('AConf', init_bq, calibrate_aq),
(
'Stationary', ),
('AConf', init_bq, init_aq),
('AtAConf', init_aq),
('GConf', open_gq),
('GConf', closed_gq),
('Grasp', None, None),
('AtGrasp', None, None),
('Above', top_joint, bottom_joint),
('Adjacent', top_joint, bottom_joint),
('Adjacent', bottom_joint, top_joint),
('Calibrated', ),
('CanMoveBase', ),
('CanMoveArm', ),
('CanMoveGripper', ),
] + list(task.init) + list(additional_init)
for action_name, cost in ACTION_COSTS.items():
function_name = '{}Cost'.format(title_from_snake(action_name))
function = (function_name, )
init.append(Equal(function, cost)) # TODO: stove state
init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \
[('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \
[('Pressed', name) for name in belief.pressed] + \
[('Cookable', name) for name in task.goal_cooked] + \
[('Cooked', name) for name in belief.cooked] + \
[('Status', status) for status in DOOR_STATUSES] + \
[('Knob', knob) for knob in KNOBS] + \
[('Joint', knob) for knob in KNOBS] + \
[('Liquid', liquid) for _, liquid in task.init_liquid] + \
[('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \
[('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \
[('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object
#[('Type', obj_name, 'stove') for obj_name in STOVES] + \
#[('Camera', name) for name in world.cameras]
if task.movable_base:
init.append(('MovableBase', ))
if fixed_base:
init.append(('InitBConf', init_bq))
if task.noisy_base:
init.append(('NoisyBase', ))
compute_pose_kin = get_compute_pose_kin(world)
compute_angle_kin = get_compute_angle_kin(world)
initial_poses = {}
for joint_name, init_conf in belief.door_confs.items():
if joint_name in DRAWER_JOINTS:
init.append(('Drawer', joint_name))
if joint_name in CABINET_JOINTS:
init.append(('Cabinet', joint_name))
joint = joint_from_name(world.kitchen, joint_name)
surface_name = surface_from_joint(joint_name)
init.append(('SurfaceJoint', surface_name, joint_name))
# Relies on the fact that drawers have identical surface and link names
link_name = get_link_name(world.kitchen, child_link_from_joint(joint))
#link_name = str(link_name.decode('UTF-8'))
#link_name = str(link_name.encode('ascii','ignore'))
for conf in {
init_conf, world.open_kitchen_confs[joint],
world.closed_kitchen_confs[joint]
}:
# TODO: return to initial poses?
world_pose, = compute_angle_kin(link_name, joint_name, conf)
init.extend([
('Joint', joint_name),
('Angle', joint_name, conf),
('Obstacle', link_name),
('AngleKin', link_name, world_pose, joint_name, conf),
('WorldPose', link_name, world_pose),
])
if joint in world.kitchen_joints:
init.extend([
('Sample', world_pose),
#('Value', world_pose), # comment out?
])
if conf == init_conf:
initial_poses[link_name] = world_pose
init.extend([
('AtAngle', joint_name, conf),
('AtWorldPose', link_name, world_pose),
])
for surface_name in ALL_SURFACES:
if surface_name in OPEN_SURFACES:
init.append(('Counter', surface_name)) # Fixed surface
if surface_name in DRAWERS:
init.append(('Drawer', surface_name))
surface = surface_from_name(surface_name)
surface_link = link_from_name(world.kitchen, surface.link)
parent_joint = parent_joint_from_link(surface_link)
if parent_joint not in world.kitchen_joints:
# TODO: attach to world frame?
world_pose = RelPose(world.kitchen, surface_link, init=True)
initial_poses[surface_name] = world_pose
init += [
#('RelPose', surface_name, world_pose, 'world'),
('WorldPose', surface_name, world_pose),
#('AtRelPose', surface_name, world_pose, 'world'),
('AtWorldPose', surface_name, world_pose),
('Sample', world_pose),
#('Value', world_pose),
]
init.extend([
('CheckNearby', surface_name),
#('InitPose', world_pose),
('Localized', surface_name),
])
for grasp_type in task.grasp_types:
if (surface_name in OPEN_SURFACES) or has_place_database(
world.robot_name, surface_name, grasp_type):
init.append(('AdmitsGraspType', surface_name, grasp_type))
if belief.grasped is None:
init.extend([
('HandEmpty', ),
('GConf', init_gq),
('AtGConf', init_gq),
])
else:
obj_name = belief.grasped.body_name
assert obj_name not in belief.pose_dists
grasp = belief.grasped
init += [
# Static
#('Graspable', obj_name),
('Grasp', obj_name, grasp),
('IsGraspType', obj_name, grasp, grasp.grasp_type),
# Fluent
('AtGrasp', obj_name, grasp),
('Holding', obj_name),
('Localized', obj_name),
]
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
for obj_name in world.movable:
obj_type = type_from_name(obj_name)
if obj_type in BOWLS:
init.append(('Bowl', obj_name))
else:
init.append(
('Obstacle', obj_name)) # TODO: hack to place within bowls
if obj_type in COOKABLE:
init.append(('Cookable', obj_name))
if obj_type in POURABLE:
init.append(('Pourable', obj_name))
init += [
('Entity', obj_name),
('CheckNearby', obj_name),
] + [('Stackable', obj_name, counter)
for counter in set(ALL_SURFACES) & set(COUNTERS)]
# TODO: track poses over time to produce estimates
for obj_name, pose_dist in belief.pose_dists.items():
dist_support = pose_dist.dist.support()
localized = pose_dist.is_localized()
graspable = True
if localized:
init.append(('Localized', obj_name))
[rel_pose] = dist_support
roll, pitch, yaw = euler_from_quat(
quat_from_pose(rel_pose.get_reference_from_body()))
if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)):
graspable = False
print(
'{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'.
format(obj_name, roll, pitch))
if graspable:
#init.append(('Graspable', obj_name))
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
# Could also fully decompose into points (but many samples)
# Could immediately add likely points for collision checking
for rel_pose in (dist_support if localized else pose_dist.decompose()):
surface_name = rel_pose.support
if surface_name is None:
# Treats as obstacle
# TODO: could temporarily add to fixed
world_pose = rel_pose
init += [
('WorldPose', obj_name, world_pose),
('AtWorldPose', obj_name, world_pose),
]
poses = [world_pose]
#raise RuntimeError(obj_name, supporting)
else:
surface_pose = initial_poses[surface_name]
world_pose, = compute_pose_kin(obj_name, rel_pose,
surface_name, surface_pose)
init += [
# Static
('RelPose', obj_name, rel_pose, surface_name),
('WorldPose', obj_name, world_pose),
('PoseKin', obj_name, world_pose, rel_pose, surface_name,
surface_pose),
# Fluent
('AtRelPose', obj_name, rel_pose, surface_name),
('AtWorldPose', obj_name, world_pose),
]
if localized:
init.append(('On', obj_name, surface_name))
poses = [rel_pose, world_pose]
for pose in poses:
if isinstance(pose, PoseDist):
init.append(('Dist', pose))
else:
init.extend([('Sample', pose)]) #, ('Value', pose)])
#for body, ty in problem.body_types:
# init += [('Type', body, ty)]
#bodies_from_type = get_bodies_from_type(problem)
#bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty]
goal_formula = get_goal(belief, init)
stream_pddl, stream_map = get_streams(world,
teleport_base=task.teleport_base,
**kwargs)
print('Constants:', constant_map)
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal_formula)
#print('Streams:', stream_map.keys()) # DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def collect_pull(world, joint_name, args):
date = get_date()
#set_seed(args.seed)
robot_name = get_body_name(world.robot)
if is_press(joint_name):
press_gen = get_press_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
else:
joint = joint_from_name(world.kitchen, joint_name)
open_conf = Conf(world.kitchen, [joint], [world.open_conf(joint)])
closed_conf = Conf(world.kitchen, [joint], [world.closed_conf(joint)])
pull_gen = get_pull_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
#handle_link, handle_grasp, _ = get_handle_grasp(world, joint)
path = get_pull_path(robot_name, joint_name)
print(SEPARATOR)
print('Robot name {} | Joint name: {} | Filename: {}'.format(
robot_name, joint_name, path))
entries = []
failures = 0
start_time = time.time()
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time):
if is_press(joint_name):
result = next(press_gen(joint_name), None)
else:
result = next(pull_gen(joint_name, open_conf, closed_conf),
None) # Open to closed
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
if not is_press(joint_name):
open_conf.assign()
joint_pose = get_joint_reference_pose(world.kitchen, joint_name)
bq, aq1 = result[:2]
bq.assign()
aq1.assign()
#next(at.commands[2].iterate(None, None))
base_pose = get_link_pose(world.robot, world.base_link)
#handle_pose = get_link_pose(world.robot, base_link)
entries.append({
'joint_from_base':
multiply(invert(joint_pose), base_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(len(entries),
args.num_samples,
elapsed_time(start_time)))
if has_gui():
wait_for_user()
if not entries:
safe_remove(path)
return None
#visualize_database(joint_from_base_list)
# Assuming the kitchen is fixed but the objects might be open world
# TODO: could store per data point
data = {
'date': date,
'robot_name':
robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'joint_name': joint_name,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
if not is_press(joint_name):
data.update({
'open_conf': open_conf.values,
'closed_conf': closed_conf.values,
})
write_json(path, data)
print('Saved', path)
return data