def main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput():
robot = load_model(MOVO_URDF, fixed_base=True)
for link in get_links(robot):
set_color(robot,
color=apply_alpha(0.25 * np.ones(3), 1),
link=link)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
dump_body(robot)
wait_for_user('Start?')
#for arm in ARMS:
# test_close_gripper(robot, arm)
arm = 'right'
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
print(conf)
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_for_user()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_for_user()
test_retraction(robot,
MOVO_INFOS[arm],
tool_link,
fixed_joints=fixed_joints,
max_time=0.1)
disconnect()
def _update_custom_limits(self, min_extent=0.0):
#robot_extent = get_aabb_extent(get_aabb(self.robot))
# Scaling by 0.5 to prevent getting caught in corners
#min_extent = 0.5 * min(robot_extent[:2]) * np.ones(2) / 2
world_aabb = self.get_world_aabb()
full_lower, full_upper = world_aabb
base_limits = (full_lower[:2] - min_extent,
full_upper[:2] + min_extent)
base_limits[1][0] = COMPUTER_X - min_extent # TODO: robot radius
base_limits[0][1] += 0.1
base_limits[1][1] -= 0.1
for handle in self.base_limits_handles:
remove_debug(handle)
self.base_limits_handles = []
#self.base_limits_handles.extend(draw_aabb(world_aabb))
z = get_point(self.floor)[2] + 1e-2
self.base_limits_handles.extend(draw_base_limits(base_limits, z=z))
self.custom_limits = custom_limits_from_base_limits(
self.robot, base_limits)
return self.custom_limits
def main(num_iterations=10):
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
side = 0.05
block = create_box(w=side, l=side, h=side, color=RED)
start_time = time.time()
with LockRenderer():
with HideOutput():
# TODO: MOVO must be loaded last
robot = load_model(MOVO_URDF, fixed_base=True)
#set_all_color(robot, color=MOVO_COLOR)
assign_link_colors(robot)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
print('Load time: {:.3f}'.format(elapsed_time(start_time)))
dump_body(robot)
#print(get_colliding(robot))
#for arm in ARMS:
# test_close_gripper(robot, arm)
#test_grasps(robot, block)
arm = RIGHT
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_info = MOVO_INFOS[arm]
print_ik_warning(ik_info)
ik_joints = get_ik_joints(robot, ik_info, tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
wait_if_gui('Start?')
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(num_iterations):
conf = sample_fn()
print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations,
np.array(conf)))
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_if_gui()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_if_gui()
test_retraction(robot,
ik_info,
tool_link,
fixed_joints=fixed_joints,
max_time=0.05,
max_candidates=100)
disconnect()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
def main():
np.set_printoptions(precision=N_DIGITS, suppress=True,
threshold=3) # , edgeitems=1) #, linewidth=1000)
parser = argparse.ArgumentParser()
parser.add_argument(
'-a',
'--algorithm',
default='rrt_connect', # choices=[],
help='The motion planning algorithm to use.')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument(
'--holonomic',
action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false', help='')
parser.add_argument('-r', '--reversible', action='store_true', help='')
parser.add_argument(
'-s',
'--seed',
default=None,
type=int, # None | 1
help='The random seed to use.')
parser.add_argument('-n',
'--num',
default=10,
type=int,
help='The number of obstacles.')
parser.add_argument('-o', '--orientation', action='store_true', help='')
parser.add_argument('-v', '--viewer', action='store_false', help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
#set_aabb_buffer(buffer=1e-3)
#set_separating_axis_collisions()
#seed = 0
#seed = time.time()
seed = args.seed
if seed is None:
seed = random.randint(0, 10**3 - 1)
print('Seed:', seed)
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
#print('Random seed:', get_random_seed(), random.random())
#print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1(n_obstacles=args.num)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits(base_limits)
# draw_pose(get_link_pose(robot, base_link), length=0.5)
start_conf = get_joint_positions(robot, base_joints)
for conf in [start_conf, goal_conf]:
draw_waypoint(conf)
#resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
plan_joints = base_joints[:2] if not args.orientation else base_joints
d = len(plan_joints)
holonomic = args.holonomic or (d != 3)
resolutions = 1. * DEFAULT_RESOLUTION * np.ones(
d) # TODO: default resolutions, velocities, accelerations fns
#weights = np.reciprocal(resolutions)
weights = np.array([1, 1, 1e-3])[:d]
cost_fn = get_acceleration_fn(robot,
plan_joints,
max_velocities=MAX_VELOCITIES[:d],
max_accelerations=MAX_ACCELERATIONS[:d])
# TODO: check that taking shortest turning direction (reversible affecting?)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
#set_all_static() # Doesn't seem to affect
#test_aabb(robot)
#test_caching(robot, obstacles)
#return
#########################
# TODO: filter if straight-line path is feasible
saver = WorldSaver()
wait_for_duration(duration=1e-2)
start_time = time.time()
with LockRenderer(lock=args.lock):
with Profiler(field='cumtime', num=25): # tottime | cumtime | None
# TODO: draw the search tree
path = plan_base_joint_motion(
robot,
plan_joints,
goal_conf[:d],
holonomic=holonomic,
reversible=args.reversible,
obstacles=obstacles,
self_collisions=False,
custom_limits=custom_limits,
use_aabb=True,
cache=True,
max_distance=MIN_PROXIMITY,
resolutions=resolutions,
weights=weights, # TODO: use KDTrees
circular={
2: UNBOUNDED_LIMITS if holonomic else CIRCULAR_LIMITS
},
cost_fn=cost_fn,
algorithm=args.algorithm,
verbose=True,
restarts=5,
max_iterations=50,
smooth=None if holonomic else 100,
smooth_time=1, # None | 100 | INF
)
saver.restore()
# TODO: draw ROADMAPS
#wait_for_duration(duration=1e-3)
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = compute_cost(robot,
base_joints,
path,
resolutions=resolutions[:len(plan_joints)])
print(
'Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
wait_if_gui()
disconnect()
return
# for i, conf in enumerate(path):
# set_joint_positions(robot, plan_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i + 1, len(path)))
path = extract_full_path(robot, plan_joints, path, base_joints)
with LockRenderer():
draw_last_roadmap(robot, base_joints)
# for i, conf in enumerate(path):
# set_joint_positions(robot, base_joints, conf)
# wait_if_gui('{}/{}) Continue?'.format(i+1, len(path)))
iterate_path(
robot,
base_joints,
path,
step_size=2e-2,
smooth=holonomic,
custom_limits=custom_limits,
resolutions=DEFAULT_RESOLUTION * np.ones(3), # resolutions
obstacles=obstacles,
self_collisions=False,
max_distance=MIN_PROXIMITY)
disconnect()
