def test_pour(visualize):
arms = [LEFT_ARM]
#cup_name, bowl_name = 'bluecup', 'bluebowl'
#cup_name, bowl_name = 'cup_7', 'cup_8'
#cup_name, bowl_name = 'cup_7-1', 'cup_7-2'
#cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2)
cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup
bowl_name = create_name('bowl', 1) # bowl | steel_bowl
world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat())
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms,
empty_arms=True, reset_arms=True, reset_items=True)
return world, task
def mirror_robot(robot1, node_points):
# TODO: place robots side by side or diagonal across
set_extrusion_camera(node_points, theta=-np.pi / 3)
#draw_pose(Pose())
centroid = np.average(node_points, axis=0)
centroid_pose = Pose(point=centroid)
#draw_pose(Pose(point=centroid))
# print(centroid)
scale = 0. # 0.15
vector = get_point(robot1) - centroid
set_pose(robot1, Pose(point=Point(*+scale * vector[:2])))
# Inner product of end-effector z with base->centroid or perpendicular to this line
# Partition by sides
robot2 = load_robot()
set_pose(
robot2,
Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi)))
# robots = [robot1]
robots = [robot1, robot2]
for robot in robots:
set_configuration(robot, DUAL_CONF)
# joint1 = get_movable_joints(robot)[0]
# set_joint_position(robot, joint1, np.pi / 8)
draw_pose(get_pose(robot), length=0.25)
return robots
def fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
def sample_push_contact(world, feature, parameter, under=False):
robot = world.robot
arm = feature['arm_name']
body = world.get_body(feature['block_name'])
push_yaw = feature['push_yaw']
center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
max_backoff = width + 0.1 # TODO: add gripper bounding box
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
collision_links = get_link_subtree(robot, gripper_link)
urdf_from_center = Pose(point=center)
reverse_z = Pose(euler=Euler(pitch=math.pi))
rotate_theta = Pose(euler=Euler(yaw=push_yaw))
#translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))
grasps = []
for i in range(1 + under):
flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
for x in np.arange(0, max_backoff, step=0.01):
translate_x = Pose(point=Point(x=-x))
grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
reverse_z, invert(urdf_from_center))
set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
grasps.append(grasp_pose)
break
return grasps
def test_push_pour(visualize):
arms = ARMS
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
items = [bowl_name, cup_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
update_world(world, table_body)
# TODO: can prevent the right arm from being able to pick
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
# Most of the time places the cup to exchange arms
return world, task
def test_block(visualize):
#arms = [LEFT_ARM]
arms = ARMS
block_name = create_name('greenblock', 1)
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON
#attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame')
#dump_body(world.perception.pr2)
#block_y = 0.0
block_y = -0.4
world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat())
world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat())
update_world(world, table_body)
init = [
('Stackable', block_name, tray_name, TOP),
]
goal = [
('On', block_name, tray_name, TOP),
]
#goal = [
# ('Grasped', arms[0], block_name),
#]
task = Task(init=init, goal=goal, arms=arms, empty_arms=True)
return world, task
def pour_path_from_parameter(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
cup_body = world.get_body(cup_name)
cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)
#####
obj_type = type_from_name(cup_name)
if obj_type in [MUSTARD]:
initial_pitch = final_pitch = -np.pi
radius = 0
else:
initial_pitch = 0 # different if mustard
final_pitch = -3 * np.pi / 4
radius = bowl_d / 2
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0 # meters
#axis_in_cup_center_z = -cup_h/2.
axis_in_cup_center_z = 0. # meters
#axis_in_cup_center_z = +cup_h/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = final_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))
pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
base_from_pivot = Pose(
Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))
#####
assert -np.pi <= final_pitch <= initial_pitch
pitches = [initial_pitch]
if final_pitch != initial_pitch:
pitches = list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + pitches
cup_path_in_bowl = []
for pitch in pitches:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
rotate_pivot, invert(base_from_pivot),
invert(Pose(point=cup_center))))
return cup_path_in_bowl
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
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 main(display='control'): # control | execute | step
connect(use_gui=True)
disable_real_time()
# KUKA_IIWA_URDF | DRAKE_IIWA_URDF
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
# floor = load_model('models/short_floor.urdf')
floor = p.loadURDF("plane.urdf")
block = load_model(
"models/drake/objects/block_for_pick_and_place_heavy.urdf", fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_user()
disconnect()
def observe_pybullet(world):
# TODO: randomize robot's pose
# TODO: probabilities based on whether in viewcone or not
# TODO: sample from poses on table
# world_saver = WorldSaver()
visible_entities = are_visible(world)
detections = {}
assert OBS_P_FP == 0
for name in visible_entities:
# TODO: false positives
if random.random() < OBS_P_FN:
continue
body = world.get_body(name)
pose = get_pose(body)
dx, dy = np.random.multivariate_normal(mean=np.zeros(2),
cov=math.pow(OBS_POS_STD, 2) *
np.eye(2))
dyaw, = np.random.multivariate_normal(mean=np.zeros(1),
cov=math.pow(OBS_ORI_STD, 2) *
np.eye(1))
print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format(
name, dx, dy, dyaw))
noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw))
observed_pose = multiply(pose, noise_pose)
#world.get_body_type(name)
detections.setdefault(name, []).append(
observed_pose) # TODO: use type instead
#world_saver.restore()
return detections
def test_retraction(robot, info, tool_link, distance=0.1, **kwargs):
ik_joints = get_ik_joints(robot, info, tool_link)
start_pose = get_link_pose(robot, tool_link)
end_pose = multiply(start_pose, Pose(Point(z=-distance)))
handles = [
add_line(point_from_pose(start_pose),
point_from_pose(end_pose),
color=BLUE)
]
#handles.extend(draw_pose(start_pose))
#handles.extend(draw_pose(end_pose))
path = []
pose_path = list(
interpolate_poses(start_pose, end_pose, pos_step_size=0.01))
for i, pose in enumerate(pose_path):
print('Waypoint: {}/{}'.format(i + 1, len(pose_path)))
handles.extend(draw_pose(pose))
conf = next(
either_inverse_kinematics(robot, info, tool_link, pose, **kwargs),
None)
if conf is None:
print('Failure!')
path = None
wait_for_user()
break
set_joint_positions(robot, ik_joints, conf)
path.append(conf)
wait_for_user()
# for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1):
# set_joint_positions(robot, joints[:len(conf)], conf)
# wait_for_user()
remove_handles(handles)
return path
def create_door(width=0.08,
length=1,
height=2,
mass=1,
handle=True,
frame=True,
**kwargs):
# TODO: hinge, cylinder on end, sliding door, knob
# TODO: explicitly disable self collisions (happens automatically)
geometry = get_box_geometry(width, length, height)
hinge = 0 # -width/2
com_point = Point(x=hinge, y=-length / 2., z=height / 2.)
door_collision, door_visual = create_shape(geometry,
pose=Pose(com_point),
**kwargs)
door_link = LinkInfo(
mass=mass,
collision_id=door_collision,
visual_id=door_visual,
#point=com_point,
inertial_point=com_point, # TODO: be careful about the COM
parent=0,
joint_type=p.JOINT_REVOLUTE,
joint_axis=[0, 0, 1])
links = [door_link]
if handle:
links.append(create_handle(width, length, height))
if frame:
links.append(create_frame(width, length, height))
body = create_multi_body(base_link=LinkInfo(), links=links)
#draw_circle(center=unit_point(), diameter=width/2., parent=body, parent_link=0)
#set_joint_limits(body, link=0, lower=-PI, upper=PI)
return body
def add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
def main():
# TODO: move to pybullet-planning for now
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(args)
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1)
floor = create_floor()
with HideOutput():
robot = load_pybullet(get_model_path(ROOMBA_URDF),
fixed_base=True,
scale=2.0)
for link in get_all_links(robot):
set_color(robot, link=link, color=ORANGE)
robot_z = stable_z(robot, floor)
set_point(robot, Point(z=robot_z))
#set_base_conf(robot, rover_confs[i])
data_path = add_data_path()
shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
fixed_base=True) # TODO: returns a tuple
dump_body(shelf)
#draw_aabb(get_aabb(shelf))
wait_for_user()
disconnect()
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
draw_global_system()
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera(distance=2)
dump_world()
saved_world = WorldSaver()
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
return
saved_world.restore()
update_state()
wait_if_gui('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.005)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_if_gui()
disconnect()
def load_plane(z=-1e-3):
add_data_path()
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
if z is not None:
set_point(plane, Point(z=z))
return plane
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_user()
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_user()
def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z):
assert min_spoon_z <= max_spoon_z
bowl_body = world.get_body(bowl_name)
bowl_urdf_from_center = get_urdf_from_base(
bowl_body) # get_urdf_from_center
spoon_body = world.get_body(spoon_name)
spoon_quat = lookup_orientation(spoon_name, STIR_QUATS)
spoon_urdf_from_center = get_urdf_from_base(
spoon_body, reference_quat=spoon_quat) # get_urdf_from_center
# Keeping the orientation consistent for generalization purposes
# TODO: what happens when the base isn't flat (e.g. bowl)
bowl_pose = get_pose(bowl_body)
stir_z = None
for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005):
bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)),
invert(spoon_urdf_from_center))
set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer))
#wait_for_user()
if pairwise_collision(bowl_body, spoon_body):
# Want to be careful not to make contact with the base
break
stir_z = z
return stir_z
def pose2d_on_surface(world, entity_name, surface_name, pose2d=UNIT_POSE2D):
x, y, yaw = pose2d
body = world.get_body(entity_name)
surface_aabb = compute_surface_aabb(world, surface_name)
z = stable_z_on_aabb(body, surface_aabb)
pose = Pose(Point(x, y, z), Euler(yaw=yaw))
set_pose(body, pose)
return pose
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-shape',
default='box',
choices=['box', 'sphere', 'cylinder', 'capsule'])
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)
if video is not None:
set_renderer(enable=False)
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
add_data_path()
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
set_point(plane, Point(z=-1e-3))
ramp = create_ramp()
dump_body(ramp)
obj = create_object(args.shape)
set_euler(obj, np.random.uniform(-np.math.radians(1), np.math.radians(1),
3))
set_point(obj, np.random.uniform(-1e-3, +1e-3, 3))
#set_velocity(obj, linear=Point(x=-1))
set_position(obj, z=2.)
#set_position(obj, z=base_aligned_z(obj))
dump_body(obj)
#add_pose_constraint(obj, pose=(target_point, target_quat), max_force=200)
set_renderer(enable=True)
if video is None:
wait_if_gui('Begin?')
simulate(controller=condition_controller(lambda step:
(step != 0) and at_rest(obj)))
if video is None:
wait_if_gui('Finish?')
disconnect()
def sample_pour_parameter(world, feature):
# TODO: adjust for RELATIVE_POUR
cup_pour_pitch = -3 * np.pi / 4
# pour_cup_pitch = -5*np.pi/6
# pour_cup_pitch = -np.pi
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0
#axis_in_cup_center_z = -feature['cup_height']/2.
axis_in_cup_center_z = 0. # This is in meters (not a fraction of the high)
#axis_in_cup_center_z = feature['cup_height']/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array(
[-feature['cup_diameter'] / 2, 0, feature['cup_height'] / 2])
cup_tl_in_axis = cup_tl_in_center - Point(x=axis_in_cup_center_x,
z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = cup_pour_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + 0.01))
bowl_tr_in_bowl_center = Point(x=feature['bowl_diameter'] / 2,
z=feature['bowl_height'] / 2)
pivot_in_bowl_center = bowl_tr_in_bowl_center + pivot_in_bowl_tr
parameter = {
'pitch': cup_pour_pitch,
'axis_in_cup_x': axis_in_cup_center_x,
'axis_in_cup_z': axis_in_cup_center_z,
'axis_in_bowl_x': pivot_in_bowl_center[0],
'axis_in_bowl_z': pivot_in_bowl_center[2],
#'velocity': None,
#'bowl_yaw': None,
#'cup_yaw': None,
'relative': RELATIVE_POUR,
}
if RELATIVE_POUR:
parameter = scale_parameter(feature, parameter, RELATIVE_POUR_SCALING)
yield parameter
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
floor_x = 2
set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
# set_default_camera()
dump_world()
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
print('Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
user_input('{}?'.format(display))
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def load_world():
#print(get_data_path())
#p.loadURDF("samurai.urdf", useFixedBase=True) # World
#p.loadURDF("kuka_lwr/kuka.urdf", useFixedBase=True)
#p.loadURDF("kuka_iiwa/model_free_base.urdf", useFixedBase=True)
# TODO: store internal world info here to be reloaded
robot = load_model(DRAKE_IIWA_URDF)
# robot = load_model(KUKA_IIWA_URDF)
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
block = load_model(BLOCK_URDF, fixed_base=False)
#cup = load_model('models/dinnerware/cup/cup_small.urdf',
# Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
# print(get_camera())
set_default_camera()
return robot, block
def main(display='execute'): # control | execute | step
# One of the arm's gantry carriage is fixed when the other is moving.
connect(use_gui=True)
set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
draw_pose(unit_pose(), length=1.0)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
# floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
#link = link_from_name(robot, 'gantry_base_link')
#print(get_aabb(robot, link))
block_x = -0.2
#block_y = 1 if ARM == 'right' else 13.5
#block_x = 10
block_y = 5.
# set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
# set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
# set_default_camera()
dump_world()
#print(get_camera())
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
if (command is None) or (display is None):
print('Unable to find a plan! Quit?')
wait_for_interrupt()
disconnect()
return
saved_world.restore()
update_state()
print('{}?'.format(display))
wait_for_interrupt()
if display == 'control':
enable_gravity()
command.control(real_time=False, dt=0)
elif display == 'execute':
command.refine(num_steps=10).execute(time_step=0.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
def get_bowl_from_pivot(world, feature, parameter):
bowl_body = world.get_body(feature['bowl_name'])
bowl_urdf_from_center = get_urdf_from_top(
bowl_body) # get_urdf_from_base | get_urdf_from_center
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
bowl_base_from_pivot = Pose(
Point(x=parameter['axis_in_bowl_x'], z=parameter['axis_in_bowl_z']))
return multiply(bowl_urdf_from_center, bowl_base_from_pivot)
def move_occluding(world):
# Prevent obstruction by other objects
# TODO: this is a bit of a hack due to pybullet
world.set_base_conf([-5.0, 0, 0])
for joint in world.kitchen_joints:
joint_name = get_joint_name(world.kitchen, joint)
if joint_name in DRAWERS:
world.open_door(joint)
else:
world.close_door(joint)
for name in world.movable:
set_pose(world.get_body(name), Pose(Point(z=-5.0)))
def parse_region(region):
lower = np.min(region['hull'], axis=0)
upper = np.max(region['hull'], axis=0)
x, y = (lower + upper) / 2.
w, h = (upper - lower) # / 2.
geom = get_box_geometry(w, h, 1e-3)
collision_id, visual_id = create_shape(geom,
pose=Pose(Point(x, y)),
color=parse_color(region['color']))
#region_id = create_body(NULL_ID, visual_id)
region_id = create_body(collision_id, visual_id)
set_pose(region_id, parse_pose(region))
return region_id
def create_frame(width, length, height, side=0.1):
# TODO: could be part of the base link instead
shapes = [
Shape(get_box_geometry(width=width,
length=length + 2 * side,
height=side),
pose=Pose(Point(z=height / 2. + side / 2.)),
color=BLACK),
Shape(get_box_geometry(width=width, length=side, height=height),
pose=Pose(Point(y=(length + side) / 2.)),
color=BLACK),
Shape(get_box_geometry(width=width, length=side, height=height),
pose=Pose(Point(y=-(length + side) / 2.)),
color=BLACK),
]
frame_collision, frame_visual = create_shape_array(*unzip(shapes))
return LinkInfo(mass=STATIC_MASS,
collision_id=frame_collision,
visual_id=frame_visual,
point=Point(y=-length / 2., z=height / 2.),
parent=0,
joint_type=p.JOINT_FIXED)
