def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_user()
def command_collision(end_effector, command, bodies):
# TODO: each new addition makes collision checking more expensive
#offset = 4
#for robot_conf in trajectory[offset:-offset]:
collisions = [False for _ in range(len(bodies))]
# Orientation remains the same for the extrusion trajectory
idx_from_body = dict(zip(bodies, range(len(bodies))))
# TODO: separate into another method. Sort paths by tool poses first
for trajectory in command.trajectories:
for tool_pose in randomize(trajectory.get_link_path()): # TODO: bisect
end_effector.set_pose(tool_pose)
#for body, _ in get_bodies_in_region(tool_aabb): # TODO
for i, body in enumerate(bodies):
if body not in idx_from_body: # Robot
continue
idx = idx_from_body[body]
if not collisions[idx]:
collisions[idx] |= pairwise_collision(
end_effector.body, body)
for trajectory in command.trajectories:
for robot_conf in randomize(trajectory.path):
set_joint_positions(trajectory.robot, trajectory.joints,
robot_conf)
for i, body in enumerate(bodies):
if not collisions[i]:
collisions[i] |= pairwise_collision(trajectory.robot, body)
#for element, unsafe in zip(elements, collisions):
# command.safe_per_element[element] = unsafe
return collisions
def fn(conf1, conf2, fluents=[]):
if teleport is True:
path = [conf1, conf2]
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
# TODO: double check that collisions with movable obstacles are considered
obstacles = list(obstacle_from_name.values())
attachments = parse_fluents(robot, brick_from_index, fluents,
obstacles)
set_joint_positions(robot, movable_joints, conf1)
path = plan_joint_motion(
robot,
movable_joints,
conf2,
obstacles=obstacles,
attachments=attachments,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
#weights=weights, resolutions=resolutions,
restarts=5,
iterations=50,
smooth=100)
if path is None:
return None
traj = MotionTrajectory(robot, movable_joints, path)
return traj,
def plan_workspace_motion(robot,
arm,
tool_path,
collision_buffer=COLLISION_BUFFER,
**kwargs):
_, resolutions = get_weights_resolutions(robot, arm)
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
arm_joints = get_arm_joints(robot, arm)
arm_waypoints = plan_cartesian_motion(robot,
arm_joints[0],
tool_link,
tool_path,
pos_tolerance=5e-3)
if arm_waypoints is None:
return None
arm_waypoints = [[conf[j] for j in movable_from_joints(robot, arm_joints)]
for conf in arm_waypoints]
set_joint_positions(robot, arm_joints, arm_waypoints[0])
return plan_waypoints_joint_motion(
robot,
arm_joints,
arm_waypoints[1:],
resolutions=resolutions,
max_distance=collision_buffer,
custom_limits=get_pr2_safety_limits(robot),
**kwargs)
def main():
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with HideOutput():
with LockRenderer():
robot = load_model(FRANKA_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
tool_link = link_from_name(robot, 'panda_hand')
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
test_retraction(robot,
PANDA_INFO,
tool_link,
max_distance=0.01,
max_time=0.05)
disconnect()
def experimental_inverse_kinematics(robot,
link,
pose,
null_space=False,
max_iterations=200,
tolerance=1e-3):
(point, quat) = pose
# https://github.com/bulletphysics/bullet3/blob/389d7aaa798e5564028ce75091a3eac6a5f76ea8/examples/SharedMemory/PhysicsClientC_API.cpp
# https://github.com/bulletphysics/bullet3/blob/c1ba04a5809f7831fa2dee684d6747951a5da602/examples/pybullet/examples/inverse_kinematics_husky_kuka.py
joints = get_joints(
robot) # Need to have all joints (although only movable returned)
movable_joints = get_movable_joints(robot)
current_conf = get_joint_positions(robot, joints)
# TODO: sample other values for the arm joints as the reference conf
min_limits = [get_joint_limits(robot, joint)[0] for joint in joints]
max_limits = [get_joint_limits(robot, joint)[1] for joint in joints]
#min_limits = current_conf
#max_limits = current_conf
#max_velocities = [get_max_velocity(robot, joint) for joint in joints] # Range of Jacobian
max_velocities = [10000] * len(joints)
# TODO: cannot have zero velocities
# TODO: larger definitely better for velocities
#damping = tuple(0.1*np.ones(len(joints)))
#t0 = time.time()
#kinematic_conf = get_joint_positions(robot, movable_joints)
for iterations in range(max_iterations): # 0.000863273143768 / iteration
# TODO: return none if no progress
if null_space:
kinematic_conf = p.calculateInverseKinematics(
robot,
link,
point,
quat,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=max_velocities,
restPoses=current_conf,
#jointDamping=damping,
)
else:
kinematic_conf = p.calculateInverseKinematics(
robot, link, point, quat)
if (kinematic_conf is None) or any(map(math.isnan, kinematic_conf)):
return None
set_joint_positions(robot, movable_joints, kinematic_conf)
link_point, link_quat = get_link_pose(robot, link)
if np.allclose(link_point, point, atol=tolerance) and np.allclose(
link_quat, quat, atol=tolerance):
#print iterations
break
else:
return None
if violates_limits(robot, movable_joints, kinematic_conf):
return None
#total_time = (time.time() - t0)
#print total_time
#print (time.time() - t0)/max_iterations
return kinematic_conf
def test_close_gripper(robot, arm):
gripper_joints = get_gripper_joints(robot, arm)
extend_fn = get_extend_fn(robot, gripper_joints)
for positions in extend_fn(get_open_positions(robot, arm), get_closed_positions(robot, arm)):
set_joint_positions(robot, gripper_joints, positions)
print(positions)
wait_if_gui('Continue?')
def is_safe(self, elements, element_bodies):
# TODO: check the end-effector first
known_elements = set(self.safe_per_element) & set(elements)
if not all(self.safe_per_element[e] for e in known_elements):
return False
unknown_elements = randomize(set(elements) - known_elements)
if not unknown_elements:
return True
for trajectory in randomize(
self.trajectories
): # TODO: could cache each individual collision
intersecting = trajectory.get_intersecting()
for i in randomize(range(len(trajectory))):
set_joint_positions(trajectory.robot, trajectory.joints,
trajectory.path[i])
for element in unknown_elements:
body = element_bodies[element]
#if not pairwise_collision(trajectory.robot, body):
# self.set_unsafe(element)
# return False
for robot_link, bodies in intersecting[i].items():
#print(robot_link, bodies, len(bodies))
if (element_bodies[element]
in bodies) and pairwise_link_collision(
trajectory.robot,
robot_link,
body,
link2=BASE_LINK):
self.set_unsafe(element)
return False
self.update_safe(elements)
return True
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 HideOutput():
with LockRenderer():
robot = load_model(MOVO_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
disconnect()
def solve_inverse_kinematics(self,
world_from_tool,
nearby_tolerance=INF,
**kwargs):
if self.ik_solver is not None:
return self.solve_trac_ik(world_from_tool, **kwargs)
#if nearby_tolerance != INF:
# return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance)
current_conf = get_joint_positions(self.robot, self.arm_joints)
start_time = time.time()
if nearby_tolerance == INF:
generator = ikfast_inverse_kinematics(self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_attempts=10,
use_halton=True)
else:
generator = closest_inverse_kinematics(
self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_time=0.01,
max_distance=nearby_tolerance,
use_halton=True)
conf = next(generator, None)
#conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100)
if conf is None:
return conf
max_distance = get_distance(current_conf, conf, norm=INF)
#print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format(
# elapsed_time(start_time), max_distance, nearby_tolerance))
set_joint_positions(self.robot, self.arm_joints, conf)
return get_configuration(self.robot)
def set_initial_conf(self):
set_joint_positions(self.robot, self.base_joints, [2.0, 0, np.pi])
#for rule in self.robot_yaml['cspace_to_urdf_rules']: # gripper: max is open
# joint = joint_from_name(self.robot, rule['name'])
# set_joint_position(self.robot, joint, rule['value'])
set_joint_positions(self.robot, self.arm_joints,
self.default_conf) # active_task_spaces
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 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 extract_full_path(robot, path_joints, path, all_joints):
with BodySaver(robot):
new_path = []
for conf in path:
set_joint_positions(robot, path_joints, conf)
new_path.append(get_joint_positions(
robot, all_joints)) # TODO: do without assigning
return new_path
def at(self, time_from_start):
assert self.spline is not None
if (time_from_start < self.start_time) or (self.end_time <
time_from_start):
return None
conf = self.spline(time_from_start)
set_joint_positions(self.robot, self.joints, conf)
return conf
def main(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
wait_for_user('Finish?')
disconnect()
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 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 iterate(self, world, attachments):
joints = get_gripper_joints(world.robot, self.arm)
start_conf = get_joint_positions(world.robot, joints)
end_conf = [self.position] * len(joints)
extend_fn = get_extend_fn(world.robot, joints)
path = [start_conf] + list(extend_fn(start_conf, end_conf))
for positions in path:
set_joint_positions(world.robot, joints, positions)
yield positions
def step_curve(robot, joints, path, step_size=None):
wait_if_gui(message='Begin?')
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
if step_size is None:
wait_if_gui(message='{}/{} Continue?'.format(i, len(path)))
else:
wait_for_duration(duration=step_size)
wait_if_gui(message='Finish?')
def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model(
"models/drake/pr2_description/urdf/pr2_simplified.urdf")
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
# head_link = link_from_name(pr2, HEAD_LINK)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
head_link = head_joints[-1]
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
lineColorRGB=(1, 0, 0)) # addUserDebugText
p.addUserDebugLine(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
lineColorRGB=(0, 0, 1)) # addUserDebugText
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2, target_point)
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_for_user()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def test_confs(robot, num_samples=10):
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(num_samples):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
def get_link_path(self, link_name=TOOL_LINK):
link = link_from_name(self.robot, link_name)
if link not in self.path_from_link:
with BodySaver(self.robot):
self.path_from_link[link] = []
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
self.path_from_link[link].append(
get_link_pose(self.robot, link))
return self.path_from_link[link]
def is_pull_safe(world, door_joint, door_plan):
obstacles = get_descendant_obstacles(world.kitchen, door_joint)
door_path, handle_path, handle_plan, tool_path = door_plan
for door_conf in [door_path[0], door_path[-1]]:
# TODO: check the whole door trajectory
set_joint_positions(world.kitchen, [door_joint], door_conf)
# TODO: just check collisions with the base of the robot
if any(pairwise_collision(world.robot, b) for b in obstacles):
if PRINT_FAILURES: print('Door start/end failure')
return False
return True
def test(arm1, conf1, arm2, conf2):
# TODO: don't let the arms get too close
if not collisions or (arm1 == arm2):
return False
arm1_joints = get_arm_joints(robot, arm1)
set_joint_positions(robot, arm1_joints, conf1)
arm2_joints = get_arm_joints(robot, arm2)
set_joint_positions(robot, arm2_joints, conf2)
return link_pairs_collision(robot,
get_moving_links(robot,
arm1_joints), robot,
get_moving_links(robot, arm2_joints))
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
user_input('Plan Arm?')
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#raw_input('Continue?')
time.sleep(0.01)
def get_aabbs(self):
#traj.aabb = aabb_union(map(get_turtle_traj_aabb, traj.iterate())) # TODO: union
if self.aabbs is not None:
return self.aabbs
self.aabbs = []
links = get_all_links(self.robot)
with BodySaver(self.robot):
for conf in self.path:
set_joint_positions(self.robot, self.joints, conf)
self.aabbs.append(
{link: get_aabb(self.robot, link)
for link in links})
return self.aabbs
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
wait_if_gui('Plan Arm?')
with LockRenderer(lock=False):
arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions)
if arm_path is None:
print('Unable to find an arm path')
return
print(len(arm_path))
for q in arm_path:
set_joint_positions(pr2, left_joints, q)
#wait_if_gui('Continue?')
wait_for_duration(0.01)
def test_door(door):
door_joints = get_movable_joints(door)
sample_fn = get_sample_fn(door, door_joints)
set_joint_positions(door, door_joints, sample_fn())
while True:
positions = sample_fn()
set_joint_positions(door, door_joints, positions)
wait_if_gui()
lower, upper = get_joint_intervals(door, door_joints)
control_joints(door, door_joints, positions=lower)
velocity_control_joints(door, door_joints,
velocities=[PI / 4]) # Able to exceed limits
def iterate_path(robot, joints, path, step_size=None): # 1e-2 | None
if path is None:
return
path = adjust_path(robot, joints, path)
with LockRenderer():
handles = draw_path(path)
wait_if_gui(message='Begin?')
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
if step_size is None:
wait_if_gui(message='{}/{} Continue?'.format(i, len(path)))
else:
wait_for_duration(duration=step_size)
wait_if_gui(message='Finish?')
