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()
