def load_localized_model(km, model_path, reference_frame):
if model_path != 'nobilia':
urdf_model = load_urdf_file(model_path)
return load_model(km,
model_path,
reference_frame,
*[gm.Position(f'{urdf_model.name}_location_{x}') for x in 'xyz'],
gm.Position(f'{urdf_model.name}_location_yaw'))
else:
return load_model(km,
model_path,
reference_frame,
*[gm.Position(f'nobilia_location_{x}') for x in 'xyz'],
gm.Position('nobilia_location_yaw'))
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'
if not rospy.has_param('~links'):
print(
'Parameter ~links needs to be set to a list of paths in the model that shall be operated'
)
exit(1)
model_path = rospy.get_param('~model')
body_paths = rospy.get_param('~links')
if not rospy.has_param('/robot_description'):
print(
'PR2 will be loaded from parameter server. It is currently not there.'
)
exit(1)
urdf_model = load_urdf_file(
'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml')
# urdf_model = load_urdf_str(rospy.get_param('/robot_description'))
if urdf_model.name.lower() != 'pr2':
print(
f'The loaded robot is not the PR2. Its name is "{urdf_model.name}"'
)
exit(1)
km = GeometryModel()
load_urdf(km, Path('pr2'), urdf_model)
km.clean_structure()
reference_frame = rospy.get_param('~reference_frame',
urdf_model.get_root())
use_base = reference_frame != urdf_model.get_root()
return joint_symbols, \
robot_controlled_symbols, \
start_pose, \
km.get_data(pr2_path).links['r_gripper_tool_frame'], \
blacklist
if __name__ == '__main__':
rospy.init_node('synchronized_motion')
vis = ROSBPBVisualizer('~vis', base_frame='world')
km = GeometryModel()
robot_type = rospy.get_param('~robot', 'pr2')
if robot_type.lower() == 'pr2':
robot_urdf = load_urdf_file('package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml')
elif robot_type.lower() == 'hsrb':
robot_urdf = load_urdf_file('package://hsr_description/robots/hsrb4s.obj.urdf')
elif robot_type.lower() == 'fetch':
robot_urdf = load_urdf_file('package://fetch_description/robots/fetch.urdf')
else:
print(f'Unknown robot {robot_type}')
exit(1)
robot_name = robot_urdf.name
robot_path = Path(robot_name)
load_urdf(km, robot_path, robot_urdf, Path('world'))
km.clean_structure()
model_name = load_model(km,
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):
print(
f'Description is supposed to be located at "{description}" but that parameter does not exist'
)
urdf_model = load_urdf_str(rospy.get_param(description))
name = rospy.get_param('~name', urdf_model.name)
reference_frame = rospy.get_param('~reference_frame', 'world')
km = GeometryModel()
load_urdf(km,
Path(name),
urdf_model,