def randomPosition(self):
'''Return a random position vector'''
res = iDynTree.Position()
for i in range(0, 3):
res.setVal(i, random.uniform(-10, 10))
return res
def testTransformInverse(self, nrOfTests=5):
print("Running test testTransformInverse")
for i in range(0, nrOfTests):
#RTF.testReport("Running test testTransformInverse for position");
p = self.randomPosition()
T = self.randomTransform()
pZero = iDynTree.Position()
pZero.zero()
self.checkPointEqual(p - p, pZero, "testTransformInverse failed")
self.checkPointEqual(T.inverse() * T * p, p,
"testTransformInverse failed")
#RTF.testReport("Running test testTransformInverse for Twist");
v = self.randomTwist()
vZero = iDynTree.Twist()
vZero.zero()
self.checkSpatialVectorEqual(v - v, vZero, "v-v is not zero")
self.checkSpatialVectorEqual(T.inverse() * T * v, v,
"T.inverse()*T*v is not v")
#RTF.testReport("Running test testTransformInverse for Wrench");
f = self.randomWrench()
fZero = iDynTree.Wrench()
fZero.zero()
self.checkSpatialVectorEqual(f - f, fZero, "f-f is not zero")
self.checkSpatialVectorEqual(T.inverse() * T * f, f,
"T.inverse()*T*f is not f")
def _to_idyntree_position(position: np.ndarray):
if position.size != 3:
raise ValueError("The position must have 3 elements")
import iDynTree
p = iDynTree.Position()
for i in range(3):
p.setVal(i, position[i])
return p
def randomTransform(self):
''' Return a random Transform '''
res = iDynTree.Transform()
res.setPosition(
iDynTree.Position(random.uniform(-10, 10), random.uniform(-10, 10),
random.uniform(-10, 10)))
res.setRotation(
iDynTree.Rotation.RPY(random.uniform(-10, 10),
random.uniform(-10, 10),
random.uniform(-10, 10)))
return res
def objectiveFunc(self, x, test=False):
self.iter_cnt += 1
print("call #{}/{}".format(self.iter_cnt, self.iter_max))
wf, q, a, b = self.vecToParams(x)
if self.config['verbose']:
print('wf {}'.format(wf))
print('a {}'.format(np.round(a,5).tolist()))
print('b {}'.format(np.round(b,5).tolist()))
print('q {}'.format(np.round(q,5).tolist()))
#input vars out of bounds, skip call
if not self.testBounds(x):
# give penalty obj value for out of bounds (because we shouldn't get here)
# TODO: for some algorithms (with augemented lagrangian added bounds) this should
# not be very high as it is added again anyway)
f = 1000.0
if self.config['minVelocityConstraint']:
g = [10.0]*self.num_constraints
else:
g = [10.0]*self.num_constraints
fail = 1.0
return f, g, fail
self.trajectory.initWithParams(a,b,q, self.nf, wf)
old_verbose = self.config['verbose']
self.config['verbose'] = 0
#old_floatingBase = self.config['floatingBase']
#self.config['floatingBase'] = 0
trajectory_data, data = self.sim_func(self.config, self.trajectory, model=self.model)
self.config['verbose'] = old_verbose
#self.config['floatingBase'] = old_floatingBase
self.last_trajectory_data = trajectory_data
if self.config['showOptimizationTrajs']:
plotter(self.config, data=trajectory_data)
f = np.linalg.cond(self.model.YBase)
#f = np.log(np.linalg.det(self.model.YBase.T.dot(self.model.YBase))) #fisher information matrix
#xBaseModel = np.dot(model.Binv | K, model.xStdModel)
#f = np.linalg.cond(model.YBase.dot(np.diag(xBaseModel))) #weighted with CAD params
f1 = 0
# add constraints (later tested for all: g(n) <= 0)
g = [1e10]*self.num_constraints
jn = self.model.jointNames
for n in range(self.num_dofs):
# check for joint limits
# joint pos lower
print("minma of trajectory",np.min(trajectory_data['positions'][:, n]))
print("maxima of trajectory",np.max(trajectory_data['positions'][:, n]))
if len(self.config['ovrPosLimit'])>n and self.config['ovrPosLimit'][n]:
g[n] = np.deg2rad(self.config['ovrPosLimit'][n][0]) - np.min(trajectory_data['positions'][:, n])
else:
g[n] = self.limits[jn[n]]['lower'] - np.min(trajectory_data['positions'][:, n])
# joint pos upper
if len(self.config['ovrPosLimit'])>n and self.config['ovrPosLimit'][n]:
g[self.num_dofs+n] = np.max(trajectory_data['positions'][:, n]) - np.deg2rad(self.config['ovrPosLimit'][n][1])
else:
g[self.num_dofs+n] = np.max(trajectory_data['positions'][:, n]) - self.limits[jn[n]]['upper']
# max joint vel
#g[2*self.num_dofs+n] = np.max(np.abs(trajectory_data['velocities'][:, n])) - self.limits[jn[n]]['velocity']
# max torques
# g[3*self.num_dofs+n] = np.nanmax(np.abs(data.samples['torques'][:, n])) - self.limits[jn[n]]['torque']
'''if self.config['minVelocityConstraint']:
# max joint vel of trajectory should at least be 10% of joint limit
g[3*self.num_dofs+n] = self.limits[jn[n]]['velocity']*self.config['minVelocityPercentage'] - \
np.max(np.abs(trajectory_data['velocities'][:, n]))'''
# highest joint torque should at least be 10% of joint limit
'''g[5*self.num_dofs+n] = self.limits[jn[n]]['torque']*0.1 - np.max(np.abs(data.samples['torques'][:, n]))
f_tmp = self.limits[jn[n]]['torque']*0.1 - np.max(np.abs(data.samples['torques'][:, n]))
if f_tmp > 0:
f1+=f_tmp'''
print(g)
# check collision constraints
# (for whole trajectory but only get closest distance as constraint value)
c_s = self.num_constraints - self.num_coll_constraints # start where collision constraints start
traversal = iDynTree.Traversal()
success = self.idyn_model.computeFullTreeTraversal(traversal)
rotation = iDynTree.Rotation(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0)
position = iDynTree.Position(0.0, 0.0, 0.0)
worldHbase = iDynTree.Transform(rotation, position)
jointPosition = iDynTree.VectorDynSize(5)
if self.config['verbose'] > 1:
print('checking collisions')
for p in range(0, trajectory_data['positions'].shape[0], 10):
g_cnt1 = 0
g_cnt2 = 1
if self.config['verbose'] > 1:
print("Sample {}".format(p))
q = trajectory_data['positions'][p]
for i in range(len(q)):
jointPosition[i] = q[i]
linkPositions = iDynTree.LinkPositions(self.idyn_model)
iDynTree.ForwardPositionKinematics(self.idyn_model, traversal, worldHbase,jointPosition, linkPositions)
pose_link5 = linkPositions(5)
z = pose_link5.getPosition().getVal(2)
origin = (0, 0 , 0)
end_eff = (pose_link5.getPosition().getVal(0), pose_link5.getPosition().getVal(1), (pose_link5.getPosition().getVal(2)))
dist = distance.euclidean(origin, end_eff)
if(z < 0.35 ):
if (z < g[c_s+g_cnt1]):
g[c_s+g_cnt1] = z
if(dist < 0.25):
if (dist < g[c_s+g_cnt2]):
g[c_s+g_cnt2] = dist
'''for l0 in range(self.model.num_links + len(self.world_links)):
for l1 in range(self.model.num_links + len(self.world_links)):
l0_name = (self.model.linkNames + self.world_links)[l0]
l1_name = (self.model.linkNames + self.world_links)[l1]
if (l0 >= l1): # don't need, distance is the same in both directions; same link never collides
continue
if l0_name in self.config['ignoreLinksForCollision'] \
or l1_name in self.config['ignoreLinksForCollision']:
continue
if [l0_name, l1_name] in self.config['ignoreLinkPairsForCollision'] or \
[l1_name, l0_name] in self.config['ignoreLinkPairsForCollision']:
continue
# neighbors can't collide with a proper joint range, so ignore--
if l0 < self.model.num_links and l1 < self.model.num_links:
if l0_name in self.neighbors[l1_name]['links'] or l1_name in self.neighbors[l0_name]['links']:
continue
if l0 < l1:
d = self.getLinkDistance(l0_name, l1_name, q)
if d < g[c_s+g_cnt]:
print("l0: {} l1: {}".format(l0,l1))
g[c_s+g_cnt] = d
g_cnt += 1'''
self.last_g = g
#add min join torques as second objective
if f1 > 0:
f+= f1
print("added cost: {}".format(f1))
c = self.testConstraints(g)
if c:
print(Fore.GREEN, end=' ')
else:
print(Fore.YELLOW, end=' ')
print("objective function value: {} (last best: {} + {})".format(f,
self.last_best_f-self.last_best_f_f1,
self.last_best_f_f1), end=' ')
print(Fore.RESET)
if self.config['verbose']:
if self.opt_prob.is_gradient:
print("(Gradient evaluation)")
if self.mpi_rank == 0 and not self.opt_prob.is_gradient and self.config['showOptimizationGraph']:
self.xar.append(self.iter_cnt)
self.yar.append(f)
self.x_constr.append(c)
self.updateGraph()
self.showVisualizerTrajectory(self.trajectory)
#keep last best solution (some solvers don't keep it)
if c and f < self.last_best_f:
self.last_best_f = f
self.last_best_f_f1 = f1
self.last_best_sol = x
print("\n\n")
fail = 0.0
#funcs = {}
#funcs['opt'] = f
#funcs['con'] = g
#return funcs, fail
return f, g, fail