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,