def save_transform(self) -> str:
"""
Actual is measured data
Expected is based on mechanical drawings of the robot
This method computes the transformation matrix from actual -> expected.
Saves this transform to disc.
"""
expected = [self._expected_points[p][:2] for p in [1, 2, 3]]
log.debug("save_transform expected: {}".format(expected))
actual = [self.actual_points[p][:2] for p in [1, 2, 3]]
log.debug("save_transform actual: {}".format(actual))
# Generate a 2 dimensional transform matrix from the two matricies
flat_matrix = solve(expected, actual)
log.debug("save_transform flat_matrix: {}".format(flat_matrix))
current_z = self.current_transform[2][3]
# Add the z component to form the 3 dimensional transform
self.current_transform = add_z(flat_matrix, current_z)
gantry_calibration = list(
map(lambda i: list(i), self.current_transform))
log.debug(
"save_transform calibration_matrix: {}".format(gantry_calibration))
self.hardware.update_config(gantry_calibration=gantry_calibration)
res = str(self.hardware.config)
return '{}\n{}'.format(res, save_config(self.hardware.config))
async def save_transform(data) -> CommandResult:
"""
Calculate the transformation matrix that calibrates the gantry to the deck
:param data: unused
"""
if not session_wrapper.session:
raise NoSessionInProgress()
if any([v is None for v in session_wrapper.session.points.values()]):
message = "Not all points have been saved"
status = False
else:
# expected values based on mechanical drawings of the robot
expected_pos = expected_points()
expected = [
expected_pos[p] for p in expected_pos.keys()]
# measured data
actual = [session_wrapper.session.points[p] for p in
sorted(session_wrapper.session.points.keys())]
# Generate a 2 dimensional transform matrix from the two matricies
flat_matrix = solve(expected, actual).round(4)
# replace relevant X, Y and angular components
# [[cos_x, sin_y, const_zero, delta_x___],
# [-sin_x, cos_y, const_zero, delta_y___],
# [const_zero, const_zero, const_one_, delta_z___],
# [const_zero, const_zero, const_zero, const_one_]]
session_wrapper.session.current_transform = \
add_z(flat_matrix, session_wrapper.session.z_value)
session_wrapper.session.adapter.update_config(
gantry_calibration=list(
list(i) for i in session_wrapper.session.current_transform)
)
new_gantry_cal =\
session_wrapper.session.adapter.config.gantry_calibration
session_wrapper.session.backup_gantry_cal = new_gantry_cal
robot_configs.save_deck_calibration(
session_wrapper.session.adapter.config)
message = "Config file saved and backed up"
status = True
return CommandResult(success=status, message=message)
async def save_transform(data):
"""
Calculate the transormation matrix that calibrates the gantry to the deck
:param data: Information obtained from a POST request.
The content type is application/json.
The correct packet form should be as follows:
{
'token': UUID token from current session start
'command': 'save transform'
}
"""
if any([v is None for v in session_wrapper.session.points.values()]):
message = "Not all points have been saved"
status = 400
else:
# expected values based on mechanical drawings of the robot
expected_pos = expected_points()
expected = [
expected_pos[p] for p in expected_pos.keys()]
# measured data
actual = [session_wrapper.session.points[p] for p in
sorted(session_wrapper.session.points.keys())]
# Generate a 2 dimensional transform matrix from the two matricies
flat_matrix = solve(expected, actual).round(4)
# replace relevant X, Y and angular components
# [[cos_x, sin_y, const_zero, delta_x___],
# [-sin_x, cos_y, const_zero, delta_y___],
# [const_zero, const_zero, const_one_, delta_z___],
# [const_zero, const_zero, const_zero, const_one_]]
session_wrapper.session.current_transform = \
add_z(flat_matrix, session_wrapper.session.z_value)
session_wrapper.session.adapter.update_config(
gantry_calibration=list(
list(i) for i in session_wrapper.session.current_transform)
)
robot_configs.save_deck_calibration(
session_wrapper.session.adapter.config)
message = "Config file saved and backed up"
status = 200
return web.json_response({'message': message}, status=status)
def test_solve():
theta = pi / 3.0 # 60 deg
scale = 2.0
expected = [
[cos(0) , sin(0)], # NOQA
[cos(pi / 2) , sin(pi / 2)], # NOQA
[cos(pi) , sin(pi)]] # NOQA
actual = [
[cos(theta) * scale + 0.5 , sin(theta) * scale + 0.25], # NOQA
[cos(theta + pi / 2) * scale + 0.5 , sin(theta + pi / 2) * scale + 0.25], # NOQA
[cos(theta + pi) * scale + 0.5 , sin(theta + pi) * scale + 0.25]] # NOQA
X = solve(expected, actual)
expected = np.array([cos(theta + pi / 2) * scale + 0.5 , sin(theta + pi / 2) * scale + 0.25, 1]) # NOQA
result = np.dot(X, np.array([[0], [1], [1]])).transpose()
return np.isclose(expected, result).all()