def custom_limits_from_base_limits(robot, base_limits, yaw_limit=None):
x_limits, y_limits = zip(*base_limits)
custom_limits = {
joint_from_name(robot, 'x'): x_limits,
joint_from_name(robot, 'y'): y_limits,
}
if yaw_limit is not None:
custom_limits.update({
joint_from_name(robot, 'theta'): yaw_limit,
})
return custom_limits
def open_surface_joints(world, surface_name, joint_names=ALL_JOINTS):
surface = surface_from_name(surface_name)
for joint_name in surface.joints:
joint = joint_from_name(world.kitchen, joint_name)
if joint_name in joint_names:
# TODO: remove this mechanic in the future
world.open_door(joint)
def parse_robot(robot_json):
pose = parse_pose(robot_json)
if robot_json['name'] == 'pr2':
with HideOutput(True):
robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_group_conf(robot_id, 'base', base_values_from_pose(pose))
else:
# TODO: set the z?
#set_pose(robot_id, pose)
raise NotImplementedError(robot_json['name'])
for joint, values in robot_json['conf'].items():
[value] = values
if has_joint(robot_id, joint):
set_joint_position(robot_id, joint_from_name(robot_id, joint),
value)
else:
print('Robot {} lacks joint {}'.format(robot_json['name'], joint))
if robot_json['name'] == 'pr2':
set_group_conf(robot_id, 'torso', [0.2])
set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM)
set_group_conf(robot_id, 'right_arm',
rightarm_from_leftarm(REST_LEFT_ARM))
return robot_id
def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
base_joints = base_joints[:2]
base_goal = base_goal[:len(base_joints)]
wait_for_user('Plan Base?')
base_path = plan_joint_motion(pr2,
base_joints,
base_goal,
obstacles=obstacles,
disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
if SLEEP is None:
wait_for_user('Continue?')
else:
time.sleep(SLEEP)
def get_surface_obstacles(world, surface_name):
surface = surface_from_name(surface_name)
obstacles = set()
for joint_name in surface.joints:
link = child_link_from_joint(joint_from_name(world.kitchen, joint_name))
obstacles.update(get_descendant_obstacles(world.kitchen, link))
# Be careful to call this before each check
open_surface_joints(world, surface_name, joint_names=CABINET_JOINTS)
return obstacles
def compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles=set(),
teleport=False):
door_paths = []
if door_conf1 == door_conf2:
return door_paths
door_joint = joint_from_name(world.kitchen, joint_name)
door_joints = [door_joint]
# TODO: could unify with grasp path
door_extend_fn = get_extend_fn(world.kitchen,
door_joints,
resolutions=[DOOR_RESOLUTION])
door_path = [door_conf1.values] + list(
door_extend_fn(door_conf1.values, door_conf2.values))
if teleport:
door_path = [door_conf1.values, door_conf2.values]
# TODO: open until collision for the drawers
sign = world.get_door_sign(door_joint)
pull = (sign * door_path[0][0] < sign * door_path[-1][0])
# door_obstacles = get_descendant_obstacles(world.kitchen, door_joint)
for handle_grasp in get_handle_grasps(world, door_joint, pull=pull):
link, grasp, pregrasp = handle_grasp
handle_path = []
for door_conf in door_path:
set_joint_positions(world.kitchen, door_joints, door_conf)
# if any(pairwise_collision(door_obst, obst)
# for door_obst, obst in product(door_obstacles, obstacles)):
# return
handle_path.append(get_link_pose(world.kitchen, link))
# Collide due to adjacency
# TODO: check pregrasp path as well
# TODO: check gripper self-collisions with the robot
set_configuration(world.gripper, world.open_gq.values)
tool_path = [
multiply(handle_pose, invert(grasp)) for handle_pose in handle_path
]
for i, tool_pose in enumerate(tool_path):
set_joint_positions(world.kitchen, door_joints, door_path[i])
set_tool_pose(world, tool_pose)
# handles = draw_pose(handle_path[i], length=0.25)
# handles.extend(draw_aabb(get_aabb(world.kitchen, link=link)))
# wait_for_user()
# for handle in handles:
# remove_debug(handle)
if any(
pairwise_collision(world.gripper, obst)
for obst in obstacles):
break
else:
door_paths.append(
DoorPath(door_path, handle_path, handle_grasp, tool_path))
return door_paths
def command_torso(self, pose, timeout=2.0, blocking=True):
# Commands Torso to a certain position
with ClientSaver(self.client):
torso_joint = joint_from_name(self.robot, self.TORSO)
set_joint_position(self.robot, torso_joint, pose)
if self.execute_motor_control:
control_joint(self.robot, torso_joint, pose)
else:
set_joint_position(self.robot, torso_joint, pose)
def get_ik_generator(robot,
arm,
world_pose,
torso_limits=False,
upper_limits=False):
target_pose = multiply(invert(get_base_pose(robot)), world_pose)
torso_limits = get_limits(robot, joint_from_name(robot, TORSO_JOINT),
torso_limits)
upper_limits = get_limits(robot, joint_from_name(robot, UPPER_JOINT[arm]),
upper_limits)
arm_joints = get_torso_arm_joints(robot, arm)
while True:
torso = random.uniform(*torso_limits)
upper = random.uniform(*upper_limits)
yield [
q for q in inverse_kinematics(arm, target_pose, torso, upper)
if not violates_limits(robot, arm_joints, q)
]
def update_door_conf(self, name, position):
joint = joint_from_name(self.world.kitchen, name)
conf = FConf(self.world.kitchen, [joint], [position], init=True)
if (name not in self.door_confs) or not are_confs_close(
conf, self.door_confs[name], tol=1e-3):
# TODO: different threshold for drawers and doors
self.door_confs[name] = conf
else:
print('At anticipated conf for door {}'.format(name))
return self.door_confs[name]
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 gen(joint_name, door_conf1, door_conf2, *args):
if door_conf1 == door_conf2:
return
door_joint = joint_from_name(world.kitchen, joint_name)
door_paths = compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles,
teleport=teleport)
if not door_paths:
return
if learned:
base_generator = cycle(load_pull_base_poses(world, joint_name))
else:
_, _, _, tool_path = door_paths[0]
index = int(len(tool_path) / 2) # index = 0
target_pose = tool_path[index]
base_generator = uniform_pose_generator(world.robot, target_pose)
safe_base_generator = inverse_reachability(world,
base_generator,
obstacles=obstacles,
**kwargs)
while True:
for i in range(max_attempts):
try:
base_conf = next(safe_base_generator)
except StopIteration:
return
if base_conf is None:
yield None
continue
door_path = random.choice(door_paths)
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_pull(world,
door_joint,
door_path,
base_conf,
randomize=randomize,
collisions=collisions,
teleport=teleport,
**kwargs), None)
if ik_outputs is not None:
print('Pull succeeded after {} attempts'.format(i))
yield (base_conf, ) + ik_outputs
break
else:
if PRINT_FAILURES: print('Pull failure')
yield None
def get_ik_generator(robot, tool_pose, track_limits=False, prev_free_list=[]):
from .ikfast_abb_irb6600_track import get_ik
world_from_base = get_link_pose(robot, link_from_name(robot, BASE_FRAME))
base_from_tool = multiply(invert(world_from_base), tool_pose)
base_from_ik = multiply(base_from_tool, get_tool_from_ik(robot))
sampled_limits = get_ik_limits(robot, joint_from_name(robot, *TRACK_JOINT),
track_limits)
while True:
if not prev_free_list:
sampled_values = [random.uniform(*sampled_limits)]
else:
sampled_values = prev_free_list
ik_joints = get_track_arm_joints(robot)
confs = compute_inverse_kinematics(get_ik, base_from_ik,
sampled_values)
yield [q for q in confs if not violates_limits(robot, ik_joints, q)]
def gen(joint_name, door_conf1, door_conf2, base_conf):
#if door_conf1 == door_conf2:
# return
# TODO: check if within database convex hull
door_joint = joint_from_name(world.kitchen, joint_name)
obstacles = (world.static_obstacles
| get_descendant_obstacles(world.kitchen, door_joint)
) # if collisions else set()
base_conf.assign()
world.carry_conf.assign()
door_plans = [
door_plan for door_plan in compute_door_paths(world,
joint_name,
door_conf1,
door_conf2,
obstacles,
teleport=teleport)
if is_pull_safe(world, door_joint, door_plan)
]
if not door_plans:
print('Unable to open door {} at fixed config'.format(joint_name))
return
max_failures = FIXED_FAILURES if world.task.movable_base else INF
failures = 0
while failures <= max_failures:
for i in range(max_attempts):
door_path = random.choice(door_plans)
# TracIK is itself stochastic
randomize = (random.random() < P_RANDOMIZE_IK)
ik_outputs = next(
plan_pull(world,
door_joint,
door_path,
base_conf,
randomize=randomize,
collisions=collisions,
teleport=teleport,
**kwargs), None)
if ik_outputs is not None:
print('Fixed pull succeeded after {} attempts'.format(i))
yield ik_outputs
break # return
else:
if PRINT_FAILURES: print('Fixed pull failure')
yield None
failures += 1
def test_drake_base_motion(pr2, base_start, base_goal):
# TODO: combine this with test_arm_motion
"""
Drake's PR2 URDF has explicit base joints
"""
disabled_collisions = get_disabled_collisions(pr2)
base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']]
set_joint_positions(pr2, base_joints, base_start)
user_input('Plan Base?')
base_path = plan_joint_motion(pr2, base_joints, base_goal, disabled_collisions=disabled_collisions)
if base_path is None:
print('Unable to find a base path')
return
print(len(base_path))
for bq in base_path:
set_joint_positions(pr2, base_joints, bq)
user_input('Continue?')
def _load_robot(self, real_world):
with ClientSaver(self.client):
# TODO: set the x,y,theta joints using the base pose
pose = get_pose(self.robot) # base_link is origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, multiply(invert(base_pose), pose))
# base_pose = real_world.controller.return_cartesian_pose(arm='l')
# Because the robot might have an xyz
movable_names = real_world.controller.get_joint_names()
movable_joints = [
joint_from_name(self.robot, name) for name in movable_names
]
movable_positions = real_world.controller.get_joint_positions(
movable_names)
set_joint_positions(self.robot, movable_joints, movable_positions)
self.initial_config = get_configuration(self.robot)
self.body_mapping[self.robot] = real_world.robot
def solve_inverse_kinematics(robot,
arm,
world_pose,
obstacles=[],
collision_buffer=COLLISION_BUFFER,
max_attempts=25):
arm_joints = get_arm_joints(robot, arm)
custom_limits = get_pr2_safety_limits(robot) # TODO: self-collisions
collision_fn = get_collision_fn(robot,
arm_joints,
obstacles=obstacles,
attachments=[],
max_distance=collision_buffer,
self_collisions=False,
disabled_collisions=set(),
custom_limits=custom_limits)
base_pose = get_link_pose(robot,
link_from_name(robot,
BASE_FRAME)) # != get_pose(robot)
target_point, target_quat = multiply(invert(base_pose), world_pose)
[torso_position] = get_group_conf(robot, 'torso')
upper_joint = joint_from_name(robot, UPPER_JOINTS[arm])
lower, upper = get_custom_limits(robot, [upper_joint],
custom_limits,
circular_limits=CIRCULAR_LIMITS)
upper_limits = list(zip(lower, upper))[0]
# TODO(caelan): just attempt one IK sample for each generator to restart quickly
for i, arm_conf in enumerate(
islice(
get_arm_ik_generator(get_arm_prefix(arm),
list(target_point),
list(target_quat),
torso_position,
upper_limits=upper_limits),
max_attempts)):
if not collision_fn(arm_conf):
#print('Attempts:', i)
return Conf(arm_conf)
return None
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
def get_pr2_safety_limits(pr2):
return {
joint_from_name(pr2, name): limits
for name, limits in PR2_JOINT_SAFETY_LIMITS.items()
}
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 main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
identity_pose = (unit_point(), unit_quat())
origin_handles = draw_pose(
identity_pose, length=1.0
) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE)
# Bodies are described by an integer index
floor = create_box(w=floor_width, l=floor_width, h=0.001,
color=TAN) # Creates a tan box object for the floor
set_point(floor,
[0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor
obstacle = create_box(w=0.5, l=0.5, h=0.1,
color=RED) # Creates a red box obstacle
set_point(
obstacle,
[0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle
print('Position:', get_point(obstacle))
set_euler(obstacle,
[0, 0, np.pi / 4
]) # Sets the [roll,pitch,yaw] orientation of the obstacle
print('Orientation:', get_euler(obstacle))
with LockRenderer(
): # Temporarily prevents the renderer from updating for improved loading efficiency
with HideOutput(): # Temporarily suppresses pybullet output
robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file
robot_z = stable_z(
robot, floor
) # Returns the z offset required for robot to be placed on floor
set_point(robot,
[0, 0, robot_z]) # Sets the z position of the robot
dump_body(robot) # Prints joint and link information about robot
set_all_static()
# Joints are also described by an integer index
# The turtlebot has explicit joints representing x, y, theta
x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x'
y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y'
theta_joint = joint_from_name(
robot, 'theta') # Looks up the robot joint named 'theta'
joints = [x_joint, y_joint, theta_joint]
base_link = link_from_name(
robot, 'base_link') # Looks up the robot link named 'base_link'
world_from_obstacle = get_pose(
obstacle
) # Returns the pose of the origin of obstacle wrt the world frame
obstacle_aabb = get_subtree_aabb(obstacle)
draw_aabb(obstacle_aabb)
random.seed(0) # Sets the random number generator state
handles = []
for i in range(10):
for handle in handles:
remove_debug(handle)
print('\nIteration: {}'.format(i))
x = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, x_joint,
x) # Sets the current value of the x joint
y = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, y_joint,
y) # Sets the current value of the y joint
yaw = random.uniform(-np.pi, np.pi)
set_joint_position(robot, theta_joint,
yaw) # Sets the current value of the theta joint
values = get_joint_positions(
robot,
joints) # Obtains the current values for the specified joints
print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values))
world_from_robot = get_link_pose(
robot,
base_link) # Returns the pose of base_link wrt the world frame
position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion)
x, y, z = position # Decomposing position into x, y, z
print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format(
x, y, z))
euler = euler_from_quat(
quaternion) # Converting from quaternion to euler angles
roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw
print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'.
format(roll, pitch, yaw))
handles.extend(
draw_pose(world_from_robot, length=0.5)
) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE)
obstacle_from_robot = multiply(
invert(world_from_obstacle),
world_from_robot) # Relative transformation from robot to obstacle
robot_aabb = get_subtree_aabb(
robot,
base_link) # Computes the robot's axis-aligned bounding box (AABB)
lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema
center = (lower + upper) / 2. # Computing the center of the AABB
extent = upper - lower # Computing the dimensions of the AABB
handles.extend(draw_aabb(robot_aabb))
collision = pairwise_collision(
robot, obstacle
) # Checks whether robot is currently colliding with obstacle
print('Collision: {}'.format(collision))
wait_for_duration(1.0) # Like sleep() but also updates the viewer
wait_for_user() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
def get_custom_limits(robot, named_limits=CUSTOM_LIMITS):
return {
joint_from_name(robot, joint): limits
for joint, limits in named_limits.items()
}
#Setup sim
physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pr2_start_orientation = p.getQuaternionFromEuler([0,0,0])
pr2_start_pose = [-.80*k,0,0]
pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation,
useFixedBase=True, globalScaling = 1 )
#The goal here is to get the arm positions of the actual robot using the uber controller
#and simulate them in pybullet
print str(uc.get_head_pose()) + ','
#Left Manipulator
left_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['left']]
l_joint_poses = uc.get_joint_positions('l')
#Right Manipulator
right_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['right']]
r_joint_poses = uc.get_joint_positions('r')
#Torso
torso_joint = 15
torso_pose = np.array([uc.get_torso_pose()])
#Head
#TODO
raw_input('Start?')
print
print
#Aligns the pybullet sim with the robot's state
def main():
# TODO: teleporting kuka arm
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
client = connect(use_gui=args.viewer)
add_data_path()
#planeId = p.loadURDF("plane.urdf")
table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
box = create_box(.07, .05, .15)
# boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
#pr2 = p.loadURDF("pr2_description/pr2.urdf")
pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf")
#pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",)
#useFixedBase=0,)
#flags=p.URDF_USE_SELF_COLLISION)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
#flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
#pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False)
initially_colliding = get_colliding_links(pr2)
print len(initially_colliding)
origin = (0, 0, 0)
print p.getNumConstraints()
# TODO: no way of controlling the base position by itself
# TODO: PR2 seems to collide with itself frequently
# real_time = False
# p.setRealTimeSimulation(real_time)
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# while True:
# control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM)
# if not real_time:
# p.stepSimulation()
# A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints.
# GetRigidlyAttachedLinks
print pr2
# for i in range (10000):
# p.stepSimulation()
# time.sleep(1./240.)
#print get_joint_names(pr2)
print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)]
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER))
#print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER))
print self_collision(pr2)
"""
print p.getNumConstraints()
constraint = fixed_constraint(pr2, -1, box, -1) # table
p.changeConstraint(constraint)
print p.getNumConstraints()
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
p.stepSimulation()
raw_input('Continue?')
set_point(pr2, (-2, 0, 0))
p.stepSimulation()
p.changeConstraint(constraint)
print p.getConstraintInfo(constraint)
print p.getConstraintState(constraint)
raw_input('Continue?')
print get_point(pr2)
raw_input('Continue?')
"""
# TODO: would be good if we could set the joint directly
print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME),
0.2) # Updates automatically
print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME))
#return
left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES]
print set_joint_positions(
pr2, left_joints,
TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
print set_joint_positions(
pr2, right_joints,
REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM
print get_body_name(pr2)
print get_body_names()
# print p.getBodyUniqueId(pr2)
print get_joint_names(pr2)
#for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM):
# set_joint_position(pr2, joint, value)
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# #print name, joint, get_joint_position(pr2, joint), value
# print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint)
# set_joint_position(pr2, joint, value)
# #print name, joint, get_joint_position(pr2, joint), value
# for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM):
# set_joint_position(pr2, joint_from_name(pr2, name), value)
print p.getNumJoints(pr2)
jointId = 0
print p.getJointInfo(pr2, jointId)
print p.getJointState(pr2, jointId)
# for i in xrange(10):
# #lower, upper = BASE_LIMITS
# #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower
# q = np.random.uniform(*BASE_LIMITS)
# theta = np.random.uniform(*REVOLUTE_LIMITS)
# quat = z_rotation(theta)
# print q, theta, quat, env_collision(pr2)
# #set_point(pr2, q)
# set_pose(pr2, q, quat)
# #p.getMouseEvents()
# #p.getKeyboardEvents()
# raw_input('Continue?') # Stalls because waiting for input
#
# # TODO: self collisions
# for i in xrange(10):
# for name in LEFT_JOINT_NAMES:
# joint = joint_from_name(pr2, name)
# value = np.random.uniform(*get_joint_limits(pr2, joint))
# set_joint_position(pr2, joint, value)
# raw_input('Continue?')
#start = (-2, -2, 0)
#set_base_values(pr2, start)
# #start = get_base_values(pr2)
# goal = (2, 2, 0)
# p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText
# print start, goal
# raw_input('Plan?')
# path = plan_base_motion(pr2, goal)
# print path
# if path is None:
# return
# print len(path)
# for bq in path:
# set_base_values(pr2, bq)
# raw_input('Continue?')
# left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
# for joint in left_joints:
# print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \
# is_circular(pr2, joint), get_joint_position(pr2, joint)
#
# #goal = np.zeros(len(left_joints))
# goal = []
# for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM):
# joint = joint_from_name(pr2, name)
# goal.append(wrap_joint(pr2, joint, value))
#
# path = plan_joint_motion(pr2, left_joints, goal)
# print path
# for q in path:s
# set_joint_positions(pr2, left_joints, q)
# raw_input('Continue?')
print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6
movable_joints = get_movable_joints(pr2)
print len(movable_joints)
for joint in xrange(get_num_joints(pr2)):
if is_movable(pr2, joint):
print joint, get_joint_name(pr2, joint), get_joint_type(
pr2, joint), get_joint_limits(pr2, joint)
#joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES]
#set_joint_positions(pr2, joints, sample_joints(pr2, joints))
#print get_joint_positions(pr2, joints) # Need to print before the display updates?
# set_base_values(pr2, (1, -1, -np.pi/4))
# movable_joints = get_movable_joints(pr2)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# print gripper_pose
# print get_joint_positions(pr2, movable_joints)
# p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0))
# p.stepSimulation()
# raw_input('Pre2 IK')
# set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM
# print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Pre IK')
# conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons
# print conf
# print get_joint_positions(pr2, movable_joints)
# set_joint_positions(pr2, movable_joints, conf)
# print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #print get_joint_positions(pr2, movable_joints)
# p.stepSimulation()
# raw_input('Post IK')
# return
# print pose_from_tform(TOOL_TFORM)
# gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK))
# #gripper_pose = multiply(gripper_pose, TOOL_POSE)
# #gripper_pose = get_gripper_pose(pr2)
# for i, grasp_pose in enumerate(get_top_grasps(box)):
# grasp_pose = multiply(TOOL_POSE, grasp_pose)
# box_pose = multiply(gripper_pose, grasp_pose)
# set_pose(box, *box_pose)
# print get_pose(box)
# raw_input('Grasp {}'.format(i))
# return
torso = joint_from_name(pr2, TORSO_JOINT_NAME)
torso_point, torso_quat = get_link_pose(pr2, torso)
#torso_constraint = p.createConstraint(pr2, torso, -1, -1,
# p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED
# parentFramePosition=torso_point,
# childFramePosition=torso_quat)
create_inverse_reachability(pr2, box, table)
ir_database = load_inverse_reachability()
print len(ir_database)
return
link = link_from_name(pr2, LEFT_ARM_LINK)
point, quat = get_link_pose(pr2, link)
print point, quat
p.addUserDebugLine(origin, point,
lineColorRGB=(1, 1, 0)) # addUserDebugText
raw_input('Continue?')
current_conf = get_joint_positions(pr2, movable_joints)
#ik_conf = p.calculateInverseKinematics(pr2, link, point)
#ik_conf = p.calculateInverseKinematics(pr2, link, point, quat)
min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints]
max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints]
max_velocities = [
get_max_velocity(pr2, joint) for joint in movable_joints
] # Range of Jacobian
print min_limits
print max_limits
print max_velocities
ik_conf = p.calculateInverseKinematics(pr2,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf)
value_from_joint = dict(zip(movable_joints, ik_conf))
print[value_from_joint[joint] for joint in joints]
#print len(ik_conf), ik_conf
set_joint_positions(pr2, movable_joints, ik_conf)
#print len(movable_joints), get_joint_positions(pr2, movable_joints)
print get_joint_positions(pr2, joints)
raw_input('Finish?')
p.disconnect()
def create_custom_base_limits(robot, base_limits):
return {joint_from_name(robot, joint): limits
for joint, limits in safe_zip(BASE_JOINTS[:2], zip(*base_limits))}
def fn(body, pose, grasp):
obstacles = [body] + fixed
gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose)
approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose)
draw_pose(get_tool_pose(robot, ARM), length=0.04)
draw_pose(approach_pose, length=0.04)
draw_pose(gripper_pose, length=0.04)
# print(movable_joints)
# print(torso_arm)
# wait_for_interrupt()
# TODO: gantry_x_joint
# TODO: proper inverse reachability
base_link = link_from_name(robot, IK_BASE_FRAMES[ARM])
base_pose = get_link_pose(robot, base_link)
body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose))
y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM)))
initial_y = get_joint_position(robot, y_joint)
ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1]
set_joint_position(robot, y_joint, ik_y)
for _ in range(num_attempts):
if USE_IKFAST:
q_approach = sample_tool_ik(robot, ARM, approach_pose)
if q_approach is not None:
set_joint_positions(robot, torso_arm, q_approach)
else:
set_joint_positions(robot, torso_arm, sample_fn()) # Random seed
q_approach = inverse_kinematics(robot, grasp.link, approach_pose)
if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for approaching fails!')
continue
conf = BodyConf(robot, joints=arm_joints)
if USE_IKFAST:
q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True)
if q_grasp is not None:
set_joint_positions(robot, torso_arm, q_grasp)
else:
conf.assign()
q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose)
if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles):
print('- ik for grasp fails!')
continue
if teleport:
path = [q_approach, q_grasp]
else:
conf.assign()
#direction, _ = grasp.approach_pose
#path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction,
# quat_from_pose(approach_pose))
path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles,
self_collisions=self_collisions)
if path is None:
if DEBUG_FAILURE:
print('Approach motion failed')
continue
command = Command([BodyPath(robot, path, joints=torso_arm),
Attach(body, robot, grasp.link),
BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])])
return (conf, command)
# TODO: holding collisions
return None
