Skip to content
Permalink
fsr
Switch branches/tags

Name already in use

A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
Go to file
 
 
Cannot retrieve contributors at this time
"""Simulation of Hangprinter auto-calibration
"""
from __future__ import division # Always want 3/2 = 1.5
import numpy as np
# Tips on how to use differential solver:
# build/lib.linux-x86_64-2.7/mystic/differential_evolution.py
# http://www.icsi.berkeley.edu/~storn/code.html
# Axes indexing
A = 0
B = 1
C = 2
D = 3
X = 0
Y = 1
Z = 2
def symmetric_anchors(l):
anchors = np.array(np.zeros((4, 3)))
#anchors[A, X] = 0 # Fixated
anchors[A, Y] = -l
#anchors[A, Z] = 0 # Fixated
anchors[B, X] = l*np.cos(np.pi/6)
anchors[B, Y] = l*np.sin(np.pi/6)
#anchors[B, Z] = 0 # Fixated
anchors[C, X] = -l*np.cos(np.pi/6)
anchors[C, Y] = l*np.sin(np.pi/6)
#anchors[C, Z] = 0 # Fixated
#anchors[D, X] = 0 # Fixated
#anchors[D, Y] = 0 # Fixated
anchors[D, Z] = l
return anchors
def centered_rand(l):
"""Sample from U(-l, l)"""
return l*(2.*np.random.rand()-1.)
def irregular_anchors(l, fuzz_percentage = .2):
"""Realistic exact positions of anchors.
Each dimension of each anchor is treated separately to
resemble the use case.
Six anchor coordinates must be constant and known
for the coordinate system to be uniquely defined by them.
A 3d coordinate system, like a rigid body, has six degrees of freedom.
Parameters
---------
l : The line length to create the symmetric anchors first
fuzz_percentage : Percentage of l that line lenghts are allowed to differ
(except Z-difference of B- and C-anchors)
"""
fuzz = np.array(np.zeros((4, 3)))
#fuzz[A, X] = 0 # Fixated
fuzz[A, Y] = centered_rand(l*fuzz_percentage)
#fuzz[A, Z] = 0 # Fixated
fuzz[B, X] = centered_rand(l*fuzz_percentage*np.cos(np.pi/6))
fuzz[B, Y] = centered_rand(l*fuzz_percentage*np.sin(np.pi/6))
#fuzz[B, Z] = 0 # Fixated
fuzz[C, X] = centered_rand(l*fuzz_percentage*np.cos(np.pi/6))
fuzz[C, Y] = centered_rand(l*fuzz_percentage*np.sin(np.pi/6))
#fuzz[C, Z] = 0 # Fixated
#fuzz[D, X] = 0 # Fixated
#fuzz[D, Y] = 0 # Fixated
fuzz[D, Z] = l*fuzz_percentage*np.random.rand() # usually higher than A is long
return symmetric_anchors(l)+fuzz
def positions(n, l, fuzz=10):
"""Return (n^3)x3 matrix of positions in fuzzed grid of side length 2*l
Move to u=n^3 positions in an fuzzed grid of side length 2*l
centered around (0, 0, l).
Parameters
----------
n : Number of positions of which to sample along each axis
l : Max length from origo along each axis to sample
fuzz: How much each measurement point can differ from the regular grid
"""
from itertools import product
pos = np.array(list(product(np.linspace(-l, l, n), repeat = 3))) \
+ 2.*fuzz*(np.random.rand(n**3, 3) - 0.5) \
+ [0, 0, 1*l]
index_closest_to_origo = np.int(np.shape(pos)[0]/2)-int(n/2)
# Make pos[0] a point fairly close to origo
tmp = pos[0].copy()
pos[0] = pos[index_closest_to_origo]
pos[index_closest_to_origo] = tmp
return pos
def samples(anchors, pos, fuzz=1):
"""Possible relative line length measurments according to anchors and position.
Parameters
----------
anchors : 4x3 matrix of anhcor positions in mm
pos : ux3 matrix of positions
fuzz: Maximum measurment error per motor in mm
"""
# pos[:,np.newaxis,:]: ux1x3
# Broadcasting happens u times and we get ux4x3 output before norm operation
line_lengths = np.linalg.norm(anchors - pos[:,np.newaxis,:], 2, 2)
return line_lengths - line_lengths[0] + 2.*fuzz*(np.random.rand(np.shape(pos)[0], 1) - 0.5)
def samples_relative_to_origo(anchors, pos, fuzz=1):
"""Possible relative line length measurments according to anchors and position.
Parameters
----------
anchors : 4x3 matrix of anhcor positions in mm
pos : ux3 matrix of positions
fuzz: Maximum measurment error per motor in mm
"""
# pos[:,np.newaxis,:]: ux1x3
# Broadcasting happens u times and we get ux4x3 output before norm operation
line_lengths = np.linalg.norm(anchors - pos[:,np.newaxis,:], 2, 2)
return line_lengths - np.linalg.norm(anchors,2,1) + 2.*fuzz*(np.random.rand(np.shape(pos)[0], 1) - 0.5)
def cost(anchors, pos, samp):
"""If all positions and samples correspond perfectly, this returns 0.
This is the systems of equations:
sum for i from 1 to u
sum for k from a to d
|sqrt(sum for s from x to z (A_ks-s_i)^2) - sqrt(sum for s from x to z (A_ks-s_0)^2) - t_ik|
or...
sum for i from 1 to u
|sqrt((A_ax-x_i)^2 + (A_ay-y_i)^2 + (A_az-z_i)^2) - sqrt((A_ax-x_0)^2 + (A_ay-y_0)^2 + (A_az-z_0)^2) - t_ia| +
|sqrt((A_bx-x_i)^2 + (A_by-y_i)^2 + (A_bz-z_i)^2) - sqrt((A_bx-x_0)^2 + (A_by-y_0)^2 + (A_bz-z_0)^2) - t_ib| +
|sqrt((A_cx-x_i)^2 + (A_cy-y_i)^2 + (A_cz-z_i)^2) - sqrt((A_cx-x_0)^2 + (A_cy-y_0)^2 + (A_cz-z_0)^2) - t_ic| +
|sqrt((A_dx-x_i)^2 + (A_dy-y_i)^2 + (A_dz-z_i)^2) - sqrt((A_dx-x_0)^2 + (A_dy-y_0)^2 + (A_dz-z_0)^2) - t_id|
Parameters
---------
anchors : 4x3 matrix of anchor positions
pos: ux3 matrix of positions
samp : ux4 matrix of corresponding samples, starting with [0., 0., 0., 0.]
"""
return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp))
def cost_sq(anchors, pos, samp):
#pos[0] = [0.0, 0.0, 0.0]
return np.sum(pow((samples_relative_to_origo(anchors, pos, fuzz = 0) - samp), 2)) # Sum of squares
def anchorsvec2matrix(anchorsvec, az = 0., bz = 0., cz = 0.):
""" Create a 4x3 anchors matrix from 6 element anchors vector.
"""
anchors = np.array(np.zeros((4, 3)))
anchors[A,Y] = anchorsvec[0];
anchors[A,Z] = az;
anchors[B,X] = anchorsvec[1];
anchors[B,Y] = anchorsvec[2];
anchors[B,Z] = bz;
anchors[C,X] = anchorsvec[3];
anchors[C,Y] = anchorsvec[4];
anchors[C,Z] = cz;
anchors[D,Z] = anchorsvec[5];
return anchors
def anchorsmatrix2vec(a):
return [a[A,Y], a[B, X], a[B,Y], a[C, X], a[C, Y], a[D, Z]]
def posvec2matrix(v, u):
return np.reshape(v, (u,3))
def posmatrix2vec(m):
return np.reshape(m, np.shape(m)[0]*3)
def solve(samp, cb, _cost = cost_sq, az = 0., bz = 0., cz = 0.):
"""Find reasonable positions and anchors given a set of samples.
"""
def costx(posvec, anchvec):
"""Identical to cost, except the shape of inputs and capture of samp and u
Parameters
----------
x : [A_ay A_bx A_by A_cx A_cy A_dz
x1 y1 z1 x2 y2 z2 ... xu yu zu
"""
anchors = anchorsvec2matrix(anchvec, az, bz, cz)
pos = np.reshape(posvec, (u,3))
return _cost(anchors, pos, samp)
l_anch = 1500.0
l_pos = 450
l_long = 4000.0
l_short = 1700.0
u = np.shape(samp)[0]
cos30 = np.cos(30*np.pi/180)
sin30 = np.sin(30*np.pi/180)
number_of_params_pos = 3*u
number_of_params_anch = 6
# Define bounds
lb = [ -l_long, # A_ay > -5000.0
-l_long*cos30, # A_bx > -5000*cos(30)
0, # A_by > 0
0, # A_cx > 0
0, # A_cy > 0
0, # A_dz > 0
-50.0, # x0 > -50.0
-50.0, # y0 > -50.0
-10, # z0 > -10
] + [-l_short, -l_short, -10]*(u-1)
ub = [ 0, # A_ay < 0
0, # A_bx < 0
l_long*sin30, # A_by < 5000.0*sin(30)
l_long*cos30, # A_cx < 5000.0*cos(30)
l_long*sin30, # A_cy < 5000.0*sin(30)
l_long, # A_dz < 10000.0
50.0, # x0 < 50.0
50.0, # y0 < 50.0
l_short, # z0 < l_short
] + [l_short, l_short, 2*l_short]*(u-1)
from mystic.termination import ChangeOverGeneration, NormalizedChangeOverGeneration, VTR
from mystic.solvers import DifferentialEvolutionSolver2, PowellDirectionalSolver
#pos_est0 = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 0]
#pos_est0 = positions(5*5*5, 0, fuzz = 0)
pos_est0 = np.zeros((u,3))
#anchors_est = symmetric_anchors(l_anch)
anchors_est = np.array([[0.0, 0.0, az],
[0.0, 0.0, bz],
[0.0, 0.0, cz],
[0.0, 0.0, 0]])
x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0))
from mystic.termination import Or, CollapseAt, CollapseAs
#from mystic.termination import VTRChangeOverGeneration as COG
from mystic.termination import ChangeOverGeneration as COG
target = 1.0
term = Or((COG(generations=100), CollapseAt(target, generations=100)))
#print("Solver 0")
solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch)
solver0.SetEvaluationLimits(evaluations=3200000, generations=10000)
solver0.SetTermination(term)
solver0.SetInitialPoints(x_guess0)
solver0.SetStrictRanges(lb, ub)
solver0.Solve(lambda x: costx(x[6:], x[0:6]), callback = cb)
x_guess0 = solver0.bestSolution
for i in range(1,20):
#print(", %d" % i)
solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch)
solver0.SetInitialPoints(x_guess0)
solver0.SetStrictRanges(lb, ub)
solver0.Solve(lambda x: costx(x[6:], x[0:6]), callback = cb)
x_guess0 = solver0.bestSolution
return solver0.bestSolution
if __name__ == "__main__":
# Gotten from manual measuring
anchors = np.array([[0.0, -1675.0, 0.0],
[-1290.0, 1450.0, 0.0],
[1330.0, 1400.0, 0.0],
[0.0, 0.0, 1900.0]])
# The last and final data collection
samp = np.array([
[0.00, 0.00, 0.00, 0.00],
[-18.48, 545.58, 150.52, -739.12],
[17.11, 643.61, 310.02, -902.75],
[82.10, 295.06, 578.13, -930.10],
[416.71, -30.98, 598.49, -909.81],
[398.76, 129.05, 276.42, -1026.36],
[580.15, -145.47, -201.86, -571.18],
[822.88, -119.11, -189.72, -689.79],
[878.81, 92.06, -241.07, -775.01],
[890.11, 30.04, -197.74, -780.53],
[751.27, -211.24, 110.05, -814.89],
[653.38, -216.52, 194.84, -817.91],
[513.43, -102.17, 184.61, -855.04],
[49.59, -72.44, 464.47, -477.06],
[-1501.57, 1347.78, 1215.70, 531.90],
[-1547.40, 1389.66, 1259.09, 567.85],
[1515.69, -1658.76, 598.58, 626.25],
[1377.30, -1387.86, 547.61, 267.91],
[931.55, -824.87, 271.78, -258.19],
[747.75, -446.98, 220.77, -624.25],
[535.42, -115.40, 116.39, -821.45],
[1078.13, 327.34, -1297.26, 401.30],
[1083.88, 410.36, -1181.46, 113.76],
[744.47, 215.68, -732.59, -280.05],
[428.15, 69.37, -81.03, -767.44],
[715.93, 575.91, 646.29, -1659.38],
])
# Use all the points
# samp = np.array([
#[0.00, 0.00, 0.00, 0.00],
#[-170.05, 152.38, 137.88, -179.97],
#[-314.59, 304.17, 276.27, -267.88],
#[-452.23, 437.76, 309.98, -145.62],
#[-485.80, 338.49, 643.30, -141.04],
#[-579.14, 443.48, 643.13, -119.91],
#[-487.91, 476.20, 592.15, -337.04],
#[-319.81, 433.71, 432.97, -502.55],
#[-194.65, 404.48, 359.65, -617.36],
#[101.11, 244.73, 237.07, -801.82],
#[312.27, 129.08, 89.20, -828.35],
#[451.16, -36.50, -60.20, -691.99],
#[552.92, -53.49, -270.62, -548.23],
#[633.16, -69.08, -305.83, -546.69],
#[648.55, -152.04, -416.74, -351.97],
#[716.93, -308.74, -429.31, -185.34],
#[745.06, -496.04, -289.73, -108.97],
#[804.04, -394.94, -506.72, 98.59],
#[803.65, -146.07, -737.17, 120.83],
#[767.01, -252.29, -610.04, 60.58],
##[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2
#[559.23, -708.94, 464.54, -85.73], ## This one at mark
##[570.23, -720.94, 460.54, -85.73], ## corrected at mark: A: +11, B: -12, C: -4
#[772.27, -877.16, 203.65, 33.78],
##[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2
#[582.82, -610.76, -15.97, 0.67],
#
#[260.97, 193.43, 223.06, -915.84],
#[764.20, -5.79, 25.35, -915.84],
#[766.09, -6.06, 25.35, -915.84],
#[349.73, 33.83, 546.69, -915.84],
#[139.19, 548.54, 190.50, -915.84],
#[379.42, 292.80, 8.90, -915.84],
#[313.46, 107.93, -294.35, -451.37],
#[653.79, 85.09, -516.36, -451.37],
#[815.93, -592.42, 155.61, -451.37],
#[807.74, -592.89, 162.96, -451.37],
#[347.51, -349.48, 469.19, -451.37],
#[-265.20, 266.39, 528.10, -451.37],
#[-759.54, 657.01, 664.33, -61.40],
#[-762.04, 659.33, 668.54, -61.40],
#[-804.31, 892.10, 589.58, -61.40],
#[-334.78, 568.93, 50.68, -61.40],
#[2.52, -1.66, 1.61, -2.57],
#[689.62, -238.24, -563.38, -0.57],
#
#[260.97, 193.43, 223.06, -915.84],
#[764.20, -5.79, 25.35, -915.84],
#[766.09, -6.06, 25.35, -915.84],
#[349.73, 33.83, 546.69, -915.84],
#[139.19, 548.54, 190.50, -915.84],
#[379.42, 292.80, 8.90, -915.84],
#[313.46, 107.93, -294.35, -451.37],
#[653.79, 85.09, -516.36, -451.37],
#[815.93, -592.42, 155.61, -451.37],
#[807.74, -592.89, 162.96, -451.37],
#[347.51, -349.48, 469.19, -451.37],
#[-265.20, 266.39, 528.10, -451.37],
#[-759.54, 657.01, 664.33, -61.40],
#[-762.04, 659.33, 668.54, -61.40],
#[-804.31, 892.10, 589.58, -61.40],
#[-334.78, 568.93, 50.68, -61.40],
#[2.52, -1.66, 1.61, -2.57],
#[691.62, -239.24, -562.38, -2.57],
# ])
# samp = np.array([
#[276.94, 53.00, 65.10, -737.12],
#[285.35, 60.23, 92.42, -771.43],
#[293.33, 66.82, 113.66, -798.70],
#[305.34, 78.91, 142.21, -838.42],
#[309.79, 112.75, 177.17, -891.07],
#[331.22, 117.31, 185.32, -917.18],
#[347.79, 124.86, 186.45, -937.50],
#[355.02, 130.68, 187.74, -948.36],
#[364.99, 140.88, 191.88, -965.49],
#[374.09, 146.57, 195.26, -978.68],
#[378.99, 152.48, 195.90, -987.22],
#[379.85, 156.19, 198.20, -991.83],
#[380.49, 161.54, 203.07, -999.10],
#[380.71, 162.02, 203.03, -999.53],
#[380.71, 162.23, 203.15, -999.81],
#[380.71, 162.23, 203.36, -999.94],
#])
## # Combination one, converges
# samp = np.array([
#[0.00, 0.00, 0.00, 0.00],
#[276.94, 53.00, 65.10, -737.12],
#[285.35, 60.23, 92.42, -771.43],
#[293.33, 66.82, 113.66, -798.70],
#[305.34, 78.91, 142.21, -838.42],
#[309.79, 112.75, 177.17, -891.07],
#[331.22, 117.31, 185.32, -917.18],
#[347.79, 124.86, 186.45, -937.50],
#[355.02, 130.68, 187.74, -948.36],
#[364.99, 140.88, 191.88, -965.49],
#[374.09, 146.57, 195.26, -978.68],
#[378.99, 152.48, 195.90, -987.22],
#[379.85, 156.19, 198.20, -991.83],
#[380.49, 161.54, 203.07, -999.10],
#[380.71, 162.02, 203.03, -999.53],
#[380.71, 162.23, 203.15, -999.81],
#[380.71, 162.23, 203.36, -999.94],
#[260.97, 193.43, 223.06, -915.84], # D is off
#[764.20, -5.79, 25.35, -915.84], # D is off
#[766.09, -6.06, 25.35, -915.84], # D is off
#[349.73, 33.83, 546.69, -915.84], # D is off
##[260.97, 193.43, 223.06, -905.84], # corrected D +10
##[764.20, -5.79, 25.35, -905.84], # corrected D +10
##[766.09, -6.06, 25.35, -905.84], # corrected D +10
##[349.73, 33.83, 546.69, -905.84], # corrected D +10
#[-170.05, 152.38, 137.88, -179.97], #last
#[-314.59, 304.17, 276.27, -267.88],
#[-452.23, 437.76, 309.98, -145.62],
#[-485.80, 338.49, 643.30, -141.04], #4
##
##[367.86, 163.80, 223.92, -997.07],
##
###[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2
###[565.23, -715.94, 462.54, -85.73], ## corrected ..
###[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2
#])
# samp = np.array([
#[0.00, 0.00, 0.00, 0.00],
#[-170.05, 152.38, 137.88, -179.97],
#[-314.59, 304.17, 276.27, -267.88],
#[-452.23, 437.76, 309.98, -145.62],
#[-485.80, 338.49, 643.30, -141.04],
#[-579.14, 443.48, 643.13, -119.91],
#[-487.91, 476.20, 592.15, -337.04],
#[-319.81, 433.71, 432.97, -502.55],
#[-194.65, 404.48, 359.65, -617.36],
#[101.11, 244.73, 237.07, -801.82],
#[312.27, 129.08, 89.20, -828.35],
#[451.16, -36.50, -60.20, -691.99],
#[552.92, -53.49, -270.62, -548.23],
#[633.16, -69.08, -305.83, -546.69],
#[648.55, -152.04, -416.74, -351.97],
#[716.93, -308.74, -429.31, -185.34],
#[745.06, -496.04, -289.73, -108.97],
#[804.04, -394.94, -506.72, 98.59],
#[803.65, -146.07, -737.17, 120.83],
#[767.01, -252.29, -610.04, 60.58],
#[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2
##[559.23, -708.94, 464.54, -85.73], ## This one at mark
#[570.23, -720.94, 460.54, -85.73], ## corrected at mark: A: +11, B: -12, C: -4
##[772.27, -877.16, 203.65, 33.78],
#[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2
#[582.82, -610.76, -15.97, 0.67],
#])
# Adapted to new zero
# samp = np.array([
#[0.00, 0.00, 0.00, 0.00],
#[260.97, 193.43, 223.06, -915.84],
#[764.20, -5.79, 25.35, -915.84],
#[766.09, -6.06, 25.35, -915.84],
#[349.73, 33.83, 546.69, -915.84],
#[139.19, 548.54, 190.50, -915.84],
#[379.42, 292.80, 8.90, -915.84],
#[313.46, 107.93, -294.35, -451.37],
#[653.79, 85.09, -516.36, -451.37],
#[815.93, -592.42, 155.61, -451.37],
#[807.74, -592.89, 162.96, -451.37],
#[347.51, -349.48, 469.19, -451.37],
#[-265.20, 266.39, 528.10, -451.37],
#[-759.54, 657.01, 664.33, -61.40],
#[-762.04, 659.33, 668.54, -61.40],
#[-804.31, 892.10, 589.58, -61.40],
#[-334.78, 568.93, 50.68, -61.40],
#[2.52, -1.66, 1.61, -2.57],
#[689.62, -238.24, -563.38, -0.57],
# ])
# samp = np.array([
#[0.00, 0.00, 0.00, 0.00],
#[260.97, 193.43, 223.06, -915.84],
#[764.20, -5.79, 25.35, -915.84],
#[766.09, -6.06, 25.35, -915.84],
#[349.73, 33.83, 546.69, -915.84],
#[139.19, 548.54, 190.50, -915.84],
#[379.42, 292.80, 8.90, -915.84],
#[313.46, 107.93, -294.35, -451.37],
#[653.79, 85.09, -516.36, -451.37],
#[815.93, -592.42, 155.61, -451.37],
#[807.74, -592.89, 162.96, -451.37],
#[347.51, -349.48, 469.19, -451.37],
#[-265.20, 266.39, 528.10, -451.37],
#[-759.54, 657.01, 664.33, -61.40],
#[-762.04, 659.33, 668.54, -61.40],
#[-804.31, 892.10, 589.58, -61.40],
#[-334.78, 568.93, 50.68, -61.40],
#[2.52, -1.66, 1.61, -2.57],
#[691.62, -239.24, -562.38, -2.57],
# ])
u = np.shape(samp)[0]
pos = np.zeros((u, 3))
# Plot out real position and anchor
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.ion()
# Make anchor figure and position figure.
# Put the right answers onto those figures
plt.close("all")
#fig_anch = plt.figure()
#fig_pos = plt.figure()
#ax_anch = fig_anch.add_subplot(111, projection='3d')
#ax_pos = fig_pos.add_subplot(111, projection='3d')
#scat_anch0 = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'ro')
#scat_pos0 = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'k+')
#plt.pause(1)
#scat_anch = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'yx')
#scat_pos = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'b+')
def mute(x):
"""A function that does nothing
"""
return
iter = 0
def replot(x):
"""Call while pos solver is running.
"""
global iter, u
if iter%30 == 0:
if len(x) == 6 + 3*u:
ps = posvec2matrix(x[6:], u)
scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2])
anch = anchorsvec2matrix(x[0:6])
scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2])
print("Anchor errors: ")
print(anchorsvec2matrix(x[0:6]) - anchors)
print("cost: %f" % \
cost_sq(anchorsvec2matrix(x[0:6]), np.reshape(x[6:], (u,3)), samp))
elif len(x) == 6:
anch = anchorsvec2matrix(x[0:6])
scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2])
print("Anchor errors: ")
print(anchorsvec2matrix(x[0:6]) - anchors)
else:
ps = posvec2matrix(x, u)
#scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2])
plt.draw()
plt.pause(0.001)
iter += 1
the_cost = 100000.0
best_cost = 100000.0
best_az = 1000.0
best_bz = 1000.0
best_cz = 1000.0
#az = 0.
#bz = 0.
#cz = 0.
az = -5.
bz = 2.5
cz = 2.5
#for az in np.arange(-5.,5.,2.5):
# for bz in np.arange(-5.,5.,2.5):
# for cz in np.arange(-5.,5.,2.5):
solution = solve(samp, mute, cost_sq, az, bz, cz)
sol_anch = anchorsvec2matrix(solution[0:6], az, bz, cz)
print("Output Anchors were: ")
print(sol_anch)
print("Anchor errors were: ")
print(sol_anch - anchors)
#print("Positions were: ")
#print(posvec2matrix(solution[6:], u))
the_cost = cost_sq(anchorsvec2matrix(solution[0:6], az, bz, cz), np.reshape(solution[6:], (u,3)), samp)
print("cost: %f" % the_cost)
#if(the_cost < best_cost):
# best_cost = the_cost
# best_az = az
# best_bz = bz
# best_cz = cz
#print("Best az: %f\nBest bz: %f\nBest cz: %f\nBest cost: %f" % (best_az, best_bz, best_cz, best_cost))