1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
|
#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
#
#This program is distributed in the hope that it will be useful,
#but WITHOUT ANY WARRANTY; without even the implied warranty of
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
#GNU Lesser General Public License for more details.
#
#You should have received a copy of the GNU Lesser General Public License
#along with this program. If not, see <http://www.gnu.org/licenses/>.
from __future__ import division
import time
import numpy as np
import poseutil
# TODO this function seems to be unused
def _cost_min_scipy(objective_func, initialpose, args):
import scipy.optimize as optimize
initialpose = initialpose.getTuple()
returns = optimize.fmin(objective_func, initialpose, args=args, disp=True, full_output=True)
#returns = optimize.fmin_powell(objective_func, initialpose, args=args, disp=True, full_output=True)
#returns = optimize.fmin_slsqp(objective_func, initialpose, args=args, full_output=True) # doesn't work?
#initpose = np.asarray(initialpose)
#delta = np.asarray((0.5, 0.5, 0.5, 0.2, 0.2, 0.2))
#lower = initpose - delta
#upper = initpose + delta
#returns = optimize.anneal(objective_func, initialpose, args=args, lower=lower, upper=upper)
bestpose = poseutil.Pose3D(X=returns[0])
cost = returns[1]
return bestpose, cost
def cost_min(cost_func, initialpose, args, dx, dq, max_iterations=100, verbosity=1, two_D=False):
''' The format of this function allows standard optimizers from the
scipy.optimize library and arbitrary cost functions.'''
if isinstance(initialpose, poseutil.Pose3D):
bestpose = np.asarray(initialpose.getTuple())
else:
bestpose = np.asarray(initialpose)
already_checked = {}
# TODO use manhatten moves so no diagonal moves, can still move
# diagonally but takes two steps however drastically cuts number of
# adjacent poses to test from 2^6 to 2*6
# each coord is plus or minus 1 individually all other are held the same
# this is equivalent to partial differentiation and should only need 12
# poses to check
X = np.array([1,0,0])
Y = np.array([0,1,0])
Z = np.array([0,0,1])
if two_D:
raise NotImplemented
# TODO fix this as per below
#mg = np.vstack((
#(1. , 0 , 0 , 0 , 0 , 0) ,
#(0 , 1. , 0 , 0 , 0 , 0) ,
#(0 , 0 , 0 , 0 , 0 , 1.) ,
#(-1. , 0 , 0 , 0 , 0 , 0) ,
#(0 , -1. , 0 , 0 , 0 , 0) ,
#(0 , 0 , 0 , 0 , 0 , -1.) ,
#))
else:
# TODO Try parallel testing of these poses.
mg = np.vstack((np.identity(6), -np.identity(6)))
x_ = np.hstack([np.vstack([X,X,X]),np.identity(3)])
x__ = np.hstack([np.vstack([X,X,X]),-np.identity(3)])
y_ = np.hstack([np.vstack([Y,Y,Y]),np.identity(3)])
y__ = np.hstack([np.vstack([Y,Y,Y]),-np.identity(3)])
z_ = np.hstack([np.vstack([Z,Z,Z]),np.identity(3)])
z__ = np.hstack([np.vstack([Z,Z,Z]),-np.identity(3)])
mg = np.vstack([mg,x_,-x_,x__,-x__,y_,-y_,y__,-y__,z_,-z_,z__,-z__])
mg[:, 0:3] *= dx
mg[:, 3:6] *= dq
initialoverlap = -cost_func(bestpose, args[0])
previousmax = initialoverlap
maxo = initialoverlap
p_tup = poseutil.tuplepose(bestpose)
already_checked[p_tup] = True
called_count = 1
iter_count = 0
cost_func_time = 0
for i in range(max_iterations):
iter_count += 1
poses = bestpose + mg
overlaps = []
# check poses surrounding bestpose
for p in poses:
p_tup = poseutil.tuplepose(p)
# optimization to save calculating the cost for poses which we have
# previously calculated the cost for
if already_checked.has_key(p_tup):
overlaps.append(0)
continue
else:
already_checked[p_tup] = True
start = time.time()
cost = -cost_func(p, args[0])
cost_func_time += time.time() - start
# negative because this is a maximiser
called_count += 1
overlaps.append(cost)
if verbosity > 3:
print poseutil.Pose3D(p), cost
assert len(overlaps) != 0, "Already checked ALL of these poses: Some sort of circular minimisation error?"
overlaps = np.array(overlaps)
maxoi = np.argmax(overlaps)
maxo = overlaps[maxoi]
if sum(overlaps == maxo) > 1:
print 'WARNING: multiple maxima'
if verbosity > 2:
print i, ':', overlaps, maxo
#print 'Best pose:', poses[maxoi]
# break if current pose is maximum in the case when alternative pose is
# of equal overlap pick the previous pose
if maxo <= previousmax:
maxo = previousmax
break
else:
# re-assign for next loop
bestpose = poses[maxoi]
previousmax = maxo
if verbosity > 1:
print bestpose, maxo
if iter_count >= max_iterations:
print "WARNING: Maximum number of iterations reached. Solution did not reach convergence."
if verbosity > 0:
print 'cost function evaluated:', called_count, 'times over', iter_count, 'iterations'
print 'Average cost function evaluation time (ms): %.2f' % (1e3 * cost_func_time/called_count)
print len(args[0]), 'cost increase:', initialoverlap, '->', maxo
return bestpose, -maxo # -ve again because it is standard for the calling function to assume a minimisation.
def plotobjective(cost_func, initialpose, xyzs, plot_range=(-0.2, 0.2, np.radians(-5), np.radians(5)), dx=None, dq=None):
'''
Plots the cost function for various poses, centered about the initialpose.
'''
dofs = {0:'x', 1:'y', 2:'z', 3:'rotx', 4:'roty', 5:'rotz'}
xmin, xmax = plot_range[:2]
qmin, qmax = plot_range[2:]
if dx == None:
dx = (xmax-xmin)/100.0
if dq == None:
dq = (qmax-qmin)/100.0
ranges = np.array([
[xmin, xmin, xmin, qmin, qmin, qmin],
[xmax, xmax, xmax, qmax, qmax, qmax],
[dx, dx, dx, dq, dq, dq]])
ranges = ranges.T
import pylab
pylab.ion()
pylab.figure()
pylab.subplot(2, 1, 1)
for i in range(3):
X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
pose = np.array(initialpose, dtype=float)
Y = []
for x in X:
pose[i] = x
Y.append(cost_func(pose, xyzs))
pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
print "+"
pylab.legend()
pylab.xlabel('m')
pylab.ylabel('objective function value')
pylab.subplot(2, 1, 2)
for i in range(3, 6):
X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
pose = np.array(initialpose, dtype=float)
Y = []
for x in X:
pose[i] = x
Y.append(cost_func(pose, xyzs))
pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
print "*"
pylab.legend()
pylab.xlabel('rad')
pylab.ylabel('objective function value')
#pylab.show()
#pylab.draw()
|