def test_error(self): dom = [[0, 0], [1, 1], [2, 2]] ran = [[0, -1], [1, 2], [2, -1]] t = nudged.estimate(dom, ran) self.assertEqual(t.transform(dom), [[0, 0], [1, 0], [2, 0]]) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 2.0)
def test_error(self): dom = [(0, 0), (1, 1), (2, 2)] ran = [(0, -1), (1, 2), (2, -1)] t = nudged.estimate(dom, ran) self.assertEqual(t.transform(dom), [(0, 0), (1, 0), (2, 0)]) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 2.0)
def test_error(self): dom = [[0,0], [1,1], [2, 2]] ran = [[0,-1], [1,2], [2,-1]] t = nudged.estimate(dom, ran) self.assertEqual(t.transform(dom), [[0,0], [1,0], [2,0]]) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 2.0)
def __init__(self, fleet_config): super().__init__('fleet_driver_mir') self.fleet_config = fleet_config self.robots = {} self.api_clients = [] self.status_pub = self.create_publisher(FleetState, 'fleet_states', 1) self.pub_timer = self.create_timer(self.STATUS_PUB_PERIOD, self.pub_fleet) self.ref_coordinates_rmf = [[26.95, -20.23], [29.26, -22.38], [11.4, -16.48], [12.46, -16.99]] self.ref_coordinates_mir = [[7.2, 16.6], [5.15, 18.35], [23, 12.35], [22.05, 12.95]] self.rmf2mir_transform = nudged.estimate(self.ref_coordinates_rmf, self.ref_coordinates_mir) self.mir2rmf_transform = nudged.estimate(self.ref_coordinates_mir, self.ref_coordinates_rmf) mse = nudged.estimate_error(self.rmf2mir_transform, self.ref_coordinates_rmf, self.ref_coordinates_mir) self.get_logger().info(f'transformation estimate error: {mse}') for api_client in self.create_all_api_clients(self.fleet_config): self.get_logger().info(f'initializing robot from \ {api_client.configuration.host}') robot = Robot(self) robot.api = mir100_client.DefaultApi(api_client) # temporary retry configuration to workaround startup race condition while launching connection_pool_kw = robot.api.api_client.rest_client.pool_manager.connection_pool_kw orig_retries = connection_pool_kw.get('retries') retries = urllib3.Retry(10) retries.backoff_factor = 1 retries.status_forcelist = (404, ) connection_pool_kw['retries'] = retries mir_status = robot.api.status_get() robot.name = mir_status.robot_name self.load_missions(robot) self.update_positions(robot) # reset retries if orig_retries is not None: connection_pool_kw['retries'] = orig_retries else: del connection_pool_kw['retries'] self.robots[robot.name] = robot self.get_logger().info(f'successfully initialized robot \ {robot.name}') # Setup fleet driver ROS2 topic subscriptions self.path_request_sub = self.create_subscription( PathRequest, '/robot_path_requests', self.on_path_request, 1) self.mode_sub = self.create_subscription(ModeRequest, f'/robot_mode_requests', self.on_robot_mode_request, 1)
def compute_transforms(rmf_coordinates, mir_coordinates, node=None): """Get transforms between RMF and MIR coordinates.""" transforms = { 'rmf_to_mir': nudged.estimate(rmf_coordinates, mir_coordinates), 'mir_to_rmf': nudged.estimate(mir_coordinates, rmf_coordinates) } if node: mse = nudged.estimate_error(transforms['rmf_to_mir'], rmf_coordinates, mir_coordinates) node.get_logger().info(f"Transformation estimate error: {mse}") return transforms
def test_zero_error(self): dom = [(0, 0), (1, 1)] ran = [(1, 1), (2, 2)] t = nudged.estimate(dom, ran) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 0.0)
def test_zero_error(self): dom = [[0,0], [1,1]] ran = [[1,1], [2,2]] t = nudged.estimate(dom, ran) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 0.0)
def test_zero_error(self): dom = [[0, 0], [1, 1]] ran = [[1, 1], [2, 2]] t = nudged.estimate(dom, ran) mse = nudged.estimate_error(t, dom, ran) self.assertEqual(mse, 0.0)
def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time): # Profile and traits fleet_config = config_yaml['rmf_fleet'] profile = traits.Profile( geometry.make_final_convex_circle( fleet_config['profile']['footprint']), geometry.make_final_convex_circle(fleet_config['profile']['vicinity'])) vehicle_traits = traits.VehicleTraits( linear=traits.Limits(*fleet_config['limits']['linear']), angular=traits.Limits(*fleet_config['limits']['angular']), profile=profile) vehicle_traits.differential.reversible = fleet_config['reversible'] # Battery system voltage = fleet_config['battery_system']['voltage'] capacity = fleet_config['battery_system']['capacity'] charging_current = fleet_config['battery_system']['charging_current'] battery_sys = battery.BatterySystem.make(voltage, capacity, charging_current) # Mechanical system mass = fleet_config['mechanical_system']['mass'] moment = fleet_config['mechanical_system']['moment_of_inertia'] friction = fleet_config['mechanical_system']['friction_coefficient'] mech_sys = battery.MechanicalSystem.make(mass, moment, friction) # Power systems ambient_power_sys = battery.PowerSystem.make( fleet_config['ambient_system']['power']) tool_power_sys = battery.PowerSystem.make( fleet_config['tool_system']['power']) # Power sinks motion_sink = battery.SimpleMotionPowerSink(battery_sys, mech_sys) ambient_sink = battery.SimpleDevicePowerSink(battery_sys, ambient_power_sys) tool_sink = battery.SimpleDevicePowerSink(battery_sys, tool_power_sys) nav_graph = graph.parse_graph(nav_graph_path, vehicle_traits) # Adapter fleet_name = fleet_config['name'] adapter = adpt.Adapter.make(f'{fleet_name}_fleet_adapter') if use_sim_time: adapter.node.use_sim_time() assert adapter, ("Unable to initialize fleet adapter. Please ensure " "RMF Schedule Node is running") adapter.start() time.sleep(1.0) fleet_handle = adapter.add_fleet(fleet_name, vehicle_traits, nav_graph) if not fleet_config['publish_fleet_state']: fleet_handle.fleet_state_publish_period(None) # Account for battery drain drain_battery = fleet_config['account_for_battery_drain'] recharge_threshold = fleet_config['recharge_threshold'] recharge_soc = fleet_config['recharge_soc'] finishing_request = fleet_config['task_capabilities']['finishing_request'] node.get_logger().info(f"Finishing request: [{finishing_request}]") # Set task planner params ok = fleet_handle.set_task_planner_params(battery_sys, motion_sink, ambient_sink, tool_sink, recharge_threshold, recharge_soc, drain_battery, finishing_request) assert ok, ("Unable to set task planner params") task_capabilities = [] if fleet_config['task_capabilities']['loop']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Loop tasks") task_capabilities.append(TaskType.TYPE_LOOP) if fleet_config['task_capabilities']['delivery']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Delivery tasks") task_capabilities.append(TaskType.TYPE_DELIVERY) if fleet_config['task_capabilities']['clean']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Clean tasks") task_capabilities.append(TaskType.TYPE_CLEAN) # Callable for validating requests that this fleet can accommodate def _task_request_check(task_capabilities, msg: TaskProfile): if msg.description.task_type in task_capabilities: return True else: return False fleet_handle.accept_task_requests( partial(_task_request_check, task_capabilities)) # Transforms rmf_coordinates = config_yaml['reference_coordinates']['rmf'] robot_coordinates = config_yaml['reference_coordinates']['robot'] transforms = { 'rmf_to_robot': nudged.estimate(rmf_coordinates, robot_coordinates), 'robot_to_rmf': nudged.estimate(robot_coordinates, rmf_coordinates) } transforms['orientation_offset'] = \ transforms['rmf_to_robot'].get_rotation() mse = nudged.estimate_error(transforms['rmf_to_robot'], rmf_coordinates, robot_coordinates) print(f"Coordinate transformation error: {mse}") print("RMF to Robot transform:") print(f" rotation:{transforms['rmf_to_robot'].get_rotation()}") print(f" scale:{transforms['rmf_to_robot'].get_scale()}") print(f" trans:{transforms['rmf_to_robot'].get_translation()}") print("Robot to RMF transform:") print(f" rotation:{transforms['robot_to_rmf'].get_rotation()}") print(f" scale:{transforms['robot_to_rmf'].get_scale()}") print(f" trans:{transforms['robot_to_rmf'].get_translation()}") def _updater_inserter(cmd_handle, update_handle): """Insert a RobotUpdateHandle.""" cmd_handle.update_handle = update_handle # Initialize robots for this fleet robots = {} for robot_name, robot_config in config_yaml['robots'].items(): node.get_logger().info(f"Initializing robot:{robot_name}") rmf_config = robot_config['rmf_config'] robot_config = robot_config['robot_config'] initial_waypoint = rmf_config['start']['waypoint'] initial_orientation = rmf_config['start']['orientation'] robot = RobotCommandHandle( name=robot_name, config=robot_config, node=node, graph=nav_graph, vehicle_traits=vehicle_traits, transforms=transforms, map_name=rmf_config['start']['map_name'], initial_waypoint=initial_waypoint, initial_orientation=initial_orientation, charger_waypoint=rmf_config['charger']['waypoint'], update_frequency=rmf_config.get('robot_state_update_frequency', 1), adapter=adapter) if robot.initialized: robots[robot_name] = robot # Add robot to fleet fleet_handle.add_robot(robot, robot_name, profile, robot.starts, partial(_updater_inserter, robot)) node.get_logger().info( f"Successfully added new robot:{robot_name}") else: node.get_logger().error(f"Failed to initialize robot:{robot_name}") return adapter, fleet_handle, robots
def main(): #2, 62, 108 path = '../datasets/dataset_house/' y1 = np.genfromtxt(path + 'house6.csv', delimiter=',') x1 = plt.imread(path + 'house.seq6.png') y2 = np.genfromtxt(path + 'house70.csv', delimiter=',') x2 = plt.imread(path + 'house.seq70.png') y3 = np.genfromtxt(path + 'house100.csv', delimiter=',') x3 = plt.imread(path + 'house.seq100.png') fig, ax = plt.subplots(1, 3) ax[0].imshow(x1) ax[0].plot(y1[:, 0], y1[:, 1], 'o', color='red') for i in range(len(y1)): ax[0].annotate(i, y1[i]) ax[1].imshow(x2) ax[1].plot(y2[:, 0], y2[:, 1], 'o', color='red') for i in range(len(y2)): ax[1].annotate(i, y2[i]) ax[2].imshow(x3) ax[2].plot(y3[:, 0], y3[:, 1], 'o', color='red') for i in range(len(y3)): ax[2].annotate(i, y3[i]) #plt.show() mv = mview.basic([y1, y2, y3], smart_initialize=True, batch_size=10, max_iter=700, verbose=2) with open('output.csv', 'w') as fd: for each in mv.embedding: fd.write(",".join([str(each[0]), str(each[1]), str(each[2])]) + "\n") preds = np.array(mv.embedding) fig = plt.figure(figsize=plt.figaspect(.5)) bx = fig.add_subplot(1, 2, 2, projection='3d') surf = bx.scatter(preds[:, 0] ,\ preds[:, 1],\ preds[:, 2],\ c='cyan',\ alpha=1.0,\ edgecolor='b') preds = mv.embedding #choice is the two points that chose to be connected choice = [(0, 1), (1, 2), (23, 24), (25, 26), (8, 9), (4, 5), (5, 6), (5, 15), (15, 17), (6, 16), (16, 18), (15, 16), (17, 18), (19, 21), (19, 20), (20, 22), (21, 22), (27, 28), (28, 29), (10, 11), (11, 13), (11, 12), (12, 14), (13, 14), (13, 29), (14, 29), (7, 11)] for each in choice: start = each[0] end = each[1] xs = preds[start][0], preds[end][0] ys = preds[start][1], preds[end][1] zs = preds[start][2], preds[end][2] line = plt3d.art3d.Line3D(xs, ys, zs) bx.add_line(line) #bx.plot([preds[0]], [preds[1]] , color='blue') ''' #bx.plot3D(preds[1:3, 0], preds[1:3, 1], preds[1:3, 2], color='blue') #bx.plot3D(preds[23:25, 0], preds[23:25, 1], preds[23:25, 2], color='blue') #bx.plot3D(preds[25:27, 0], preds[25:27, 1], preds[25:27, 2], color='blue') #bx.plot3D(preds[8:10, 0], preds[8:10, 1], preds[8:10, 2], color='blue') #bx.plot3D(preds[4:6, 0], preds[4:6, 1], preds[4:6, 2], color='blue') bx.plot3D(preds[5:7, 0], preds[5:7, 1], preds[5:7, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[5:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') bx.plot3D(preds[0:2, 0], preds[0:2, 1], preds[0:2, 2], color='blue') #bx.plot3D(preds[0:4, 0], preds[0:4, 1], preds[0:4, 1], color='blue') ''' mv.plot_embedding() mv.plot_computations() mv.plot_images(labels=True) projections = [] projections.append(mv.embedding @ mv.projections[0].T) projections.append(mv.embedding @ mv.projections[1].T) projections.append(mv.embedding @ mv.projections[2].T) trans = [] trans.append(nudged.estimate(y1, projections[0])) trans.append(nudged.estimate(y2, projections[1])) trans.append(nudged.estimate(y3, projections[2])) fig1, cx = plt.subplots(1, 3) Z = [x1, x2, x3] Y = [y1, y2, y3] for i in range(len(trans)): each = trans[i] m = each.get_matrix() r = each.get_rotation() s = each.get_scale() t = each.get_translation() #s = 1.0 #print(rotate(Y[i], r)) mse = nudged.estimate_error(each, Y[i], projections[i]) print(i, 'error:', mse, 'scale:', s) do_plot( cx[i], Z[i], mtransforms.Affine2D().rotate(r).scale(s).translate(t[0], t[1]), projections[i]) plt.show() input()