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_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 parse_fluents(world, fluents):
obstacles = set()
for fluent in fluents:
predicate, args = fluent[0], fluent[1:]
if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}:
q, = args
q.assign()
elif predicate == 'AtAngle'.lower():
j, a = args
a.assign()
link = child_link_from_joint(a.joints[0])
obstacles.update(get_descendant_obstacles(a.body, link))
elif predicate in 'AtWorldPose'.lower():
# TODO: conditional effects are not being correctly updated in pddlstream
#b, p = args
#if isinstance(p, SurfaceDist):
# continue
#p.assign()
#obstacles.update(get_link_obstacles(world, b))
raise RuntimeError()
elif predicate in 'AtRelPose'.lower():
pass
elif predicate == 'AtGrasp'.lower():
pass
else:
raise NotImplementedError(predicate)
attachments = []
for fluent in fluents:
predicate, args = fluent[0], fluent[1:]
if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}:
pass
elif predicate == 'AtAngle'.lower():
pass
elif predicate in 'AtWorldPose'.lower():
raise RuntimeError()
elif predicate in 'AtRelPose'.lower():
o1, rp, o2 = args
if isinstance(rp, SurfaceDist):
continue
rp.assign()
obstacles.update(get_link_obstacles(world, o1))
elif predicate == 'AtGrasp'.lower():
o, g = args
if o is not None:
attachments.append(g.get_attachment())
attachments[-1].assign()
else:
raise NotImplementedError(predicate)
return attachments, obstacles
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 main(use_turtlebot=True):
parser = argparse.ArgumentParser()
parser.add_argument('-sim', action='store_true')
parser.add_argument('-video', action='store_true')
args = parser.parse_args()
video = 'video.mp4' if args.video else None
connect(use_gui=True, mp4=video)
#set_renderer(enable=False)
# print(list_pybullet_data())
# print(list_pybullet_robots())
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
plane = load_plane()
#door = load_pybullet('models/door.urdf', fixed_base=True) # From drake
#set_point(door, Point(z=-.1))
door = create_door()
#set_position(door, z=base_aligned_z(door))
set_point(door, base_aligned(door))
#set_collision_margin(door, link=0, margin=0.)
set_configuration(door, [math.radians(-5)])
dump_body(door)
door_joint = get_movable_joints(door)[0]
door_link = child_link_from_joint(door_joint)
#draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link)
draw_pose(Pose(), parent=door, parent_link=door_link)
wait_if_gui()
##########
start_x = +2
target_x = -start_x
if not use_turtlebot:
side = 0.25
robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE)
set_position(robot, x=start_x)
#set_velocity(robot, linear=Point(x=-1))
else:
turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF)
print(turtlebot_urdf)
#print(read(turtlebot_urdf))
robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True)
robot_joints = get_movable_joints(robot)[:3]
set_joint_positions(robot, robot_joints, [start_x, 0, PI])
set_all_color(robot, BLUE)
set_position(robot, z=base_aligned_z(robot))
dump_body(robot)
##########
set_renderer(enable=True)
#test_door(door)
if args.sim:
test_simulation(robot, target_x, video)
else:
assert use_turtlebot # TODO: extend to the block
test_kinematic(robot, door, target_x)
disconnect()
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 base_link(self):
return child_link_from_joint(self.base_joints[-1])
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)