def load_model(km, model_path, reference_frame, x, y, z, yaw):
origin_pose = gm.frame3_rpy(0, 0, yaw, gm.point3(x, y, z))
if model_path != 'nobilia':
urdf_model = load_urdf_file(model_path)
load_urdf(km,
Path(urdf_model.name),
urdf_model,
reference_frame,
root_transform=origin_pose)
return urdf_model.name
else:
create_nobilia_shelf(km, Path('nobilia'), origin_pose, parent_path=Path(reference_frame))
return 'nobilia'
def create_symbol_frame3_rpy(prefix):
if not isinstance(prefix, Path):
prefix = Path(prefix)
symbols = [p.to_symbol() for p in ([prefix + (x,) for x in 'xyz'] + [prefix + (f'r{r}',) for r in 'rpy'])]
return SymbolPose3RPY(gm.frame3_rpy(*symbols[-3:], gm.point3(*symbols[:3])), *symbols)
def P(n, x, i):
return Position('{}_{}_{}'.format(n, x, i))
if __name__ == '__main__':
iterations = 200
rec_d = ValueRecorder('Evaluation Speed Comparison', ('direct', 'b'), ('llvm', 'm'))
x_labels = []
for x in tqdm(range(1, 101)):
y = x * 1 #10
frames = [frame3_rpy(P('rr', x, i), P('rp', x, i), P('ry', x, i), point3(P('x', x, i), P('y', x, i), P('z', x, i))) for i in range(y)]
matrix = frames[0]
for f in frames:
matrix = matrix.col_join(f)
x_labels.append(len(matrix.free_symbols))
cython_m = llvm.speed_up(matrix, matrix.free_symbols)
avg_d = 0.0
avg_l = 0.0
for i in range(iterations):
state = {s: random.random() for s in matrix.free_symbols}
'r_wrist_roll_joint': 0,
# 'torso_lift_joint' : 0.16825
}
resting_pose = {gm.Position(Path(f'pr2/{n}')): v for n, v in resting_pose.items()}
# Object stuff
grasp_poses = {}
for p, pose_params in body_paths.items():
if not km.has_data(p):
print(f'Link {p} is not part of articulation model')
exit(1)
if type(pose_params) is not list and type(pose_params) is not tuple:
print(f'Grasp pose for {p} is not a list.')
exit(1)
if len(pose_params) == 6:
grasp_poses[Path(p)] = gm.frame3_rpy(*pose_params[-3:], gm.point3(*pose_params[:3]))
elif len(pose_params) == 7:
grasp_poses[Path(p)] = gm.frame3_quaternion(*pose_params)
else:
print(f'Grasp pose of {p} has {len(pose_params)} parameters but only 6 or 7 are permissable.')
exit(1)
monitoring_threshold = rospy.get_param('~m_threshold', 0.6)
pr2_commander = PR2VelCommandProcessor(Path('pr2'),
'/pr2_vel_controller/command',
robot_controlled_symbols,
'/base_controller/command',
base_symbols,
reference_frame=reference_frame)
location_y = rospy.get_param('/nobilia_y')
location_z = rospy.get_param('/nobilia_z')
location_yaw = rospy.get_param('/nobilia_yaw')
except KeyError as e:
print(
f'Failed to get nobilia localization params. Original error:\n{e}')
exit()
reference_frame = rospy.get_param('~reference_frame', 'world')
grab_from_tf = rospy.get_param('~grab_from_tf', False)
grab_rate = rospy.get_param('~grab_rate', 20.0)
shelf_location = gm.point3(location_x, location_y, location_z)
# origin_pose = gm.frame3_rpy(0, 0, 0, shelf_location)
origin_pose = gm.frame3_rpy(0, 0, location_yaw, shelf_location)
create_nobilia_shelf(km,
Path('nobilia'),
origin_pose,
parent_path=Path(reference_frame))
km.clean_structure()
km.dispatch_events()
shelf = km.get_data('nobilia')
tracker = Kineverse6DEKFTracker(
km,
# [Path(f'nobilia/markers/{name}') for name in shelf.markers.keys()], #
# [Path(f'nobilia/markers/body')],
{Path(f'nobilia/markers/top_panel'): 'obs_shelf_body'},
'omni', 'static', 'diff'
}:
print(
f'Base joint parameter refers to unknown type "{special_base}". Supported are [static (default), omni, diff]'
)
exit(1)
root_transform = rospy.get_param('~root_transform', None)
if root_transform is not None:
if type(root_transform) is not list and type(
root_transform) is not tuple:
print('root_transform parameter is expected to be a list.')
exit(1)
if len(root_transform) == 6:
root_transform = gm.frame3_rpy(root_transform[3],
root_transform[4],
root_transform[5],
root_transform[:3])
elif len(root_transform) == 7:
root_transform = gm.frame3_rpy(*root_transform)
else:
print(
'root_transform needs to encode transform eiter as xyzrpy, or as xyzqxqyqzqw'
)
exit(1)
description = rospy.get_param('~description')
if description[-5:].lower() == '.urdf' or description[-4:].lower(
) == '.xml':
urdf_model = load_urdf_file(description)
else:
if not rospy.has_param(description):