import namedtensors
import mechanicalstate
import staticprimitives
variants = []
for r,g,d in [(2,1,1),(2,2,1),(2,3,1),(2,2,8)]: #test on various combinations of r,g,d parameters
#make a tensor namespace that hold the right index definitions:
tns_global = mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(r=r, g=g, d=d)
tns_global.registerTensor('CurrentMean', (('r', 'g', 'd'),()) )
tns_global.registerTensor('CurrentCov', (('r', 'g', 'd'),('r_', 'g_', 'd_')), initial_values='identity' )
msd_current = mechanicalstate.MechanicalStateDistribution(tns_global, 'CurrentMean', 'CurrentCov')
ltigoal = staticprimitives.LTIGoal(tns_global, name='test_7_a')
variants.append( (tns_global, ltigoal, msd_current))
positions = _np.zeros((d))
positions[0] = 13.8
velocities = _np.zeros((d))
velocities[d-1] = 3.21
Kv = _np.ones((d,d))
Kd = _np.arange(d)
#test various setting methods:
ltigoal.setDesired()
ltigoal.setDesired(Kp=10.0)
import numpy as _np
import matplotlib.pylab as pylab
import matplotlib.pyplot as plt
import mechanicalstate
#for r,g,d in [(2,1,1),(2,2,1),(2,3,1),(2,2,8)]: #test on various combinations of r,g,d parameters
for r, g, d in [(2, 2, 8)]: #test on various combinations of r,g,d parameters
#make a tensor namespace that hold the right index definitions:
tns_global = mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(
r=r, g=g, d=d)
tns_global.registerTensor('LastMean', (('r', 'g', 'd'), ()))
tns_global.registerTensor('LastCov', (('r', 'g', 'd'), ('r_', 'g_', 'd_')),
initial_values='identity')
last_msd = mechanicalstate.MechanicalStateDistribution(
tns_global, 'LastMean', 'LastCov')
tns_global['LastMean'].data[0, 0, :] = 1.0
tns_global['LastMean'].data[0, 1, :] = 1.0
ti = mechanicalstate.TimeIntegrator(tns_global)
dt = 0.01
current_msd = ti.integrate(last_msd, dt)
#if __name__=='__main__':
# import common
# common.savePlots()
def __init__(self,
tensornamespace,
current_msd_from=None,
task_space='jointspace',
name='unnamed',
expected_torque_noise=0.123,
**kwargs):
self.name = name
self.phaseAssociable = False #indicate that this motion generator is not parameterized by phase
self.timeAssociable = True #indicate that this motion generator is parameterizable by time
self.taskspace_name = task_space
if not tensornamespace is None:
self.tns = _nt.TensorNameSpace(
tensornamespace) #inherit index sizes
if self.tns['r'].size < 2:
raise NotImplementedError(
) #sorry, this "trivial" functionality is not implemented. Try using a simple, fixed msd instead
else:
self.tns = _mechanicalstate.makeTensorNameSpaceForMechanicalStateDistributions(
r=2, g=2, d=1)
self.tns.cloneIndex('r', 'r2')
self.tns.cloneIndex('g', 'g2')
self.tns.cloneIndex('d', 'd2')
#desired values:
self.tns.registerTensor('DesiredMean', (('r', 'g', 'd'), ()))
self.tns.registerTensor('DesiredCov',
(('r', 'g', 'd'), ('r_', 'g_', 'd_')))
self.msd_desired = _mechanicalstate.MechanicalStateDistribution(
self.tns, "DesiredMean", "DesiredCov")
#initialize the desired torque variance to some non-zero value:
self.tns.setTensorToIdentity('DesiredCov', scale=1e-6)
self.msd_desired.addVariance('torque', expected_torque_noise**2)
# self.msd_desired.addVariance('impulse', expected_torque_noise**2)
self.commonnames2rg = self.msd_desired.commonnames2rg #we use them a lot here, so remember a shorthand
self.commonnames2rglabels = self.msd_desired.commonnames2rglabels
if current_msd_from is None: #local copy
self.tns.registerTensor('CurrentMean', (('r', 'g', 'd'), ()))
self.tns.registerTensor('CurrentCov',
(('r', 'g', 'd'), ('r_', 'g_', 'd_')))
self.msd_current = _mechanicalstate.MechanicalStateDistribution(
self.tns, "CurrentMean", "CurrentCov")
else: #use data from somewhere else:
self.tns.registerTensor(
'CurrentMean',
current_msd_from.tns[current_msd_from.meansName].index_tuples,
external_array=current_msd_from.tns[
current_msd_from.meansName].data,
initial_values='keep')
self.tns.registerTensor(
'CurrentCov',
current_msd_from.tns[
current_msd_from.covariancesName].index_tuples,
external_array=current_msd_from.tns[
current_msd_from.covariancesName].data,
initial_values='keep')
self.msd_current = current_msd_from
rlabel_torque, glabel_torque = self.commonnames2rglabels['torque']
rlabel_pos, glabel_pos = self.commonnames2rglabels['position']
self.tns.registerBasisTensor('e_ktau', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_torque, glabel_torque)))
self.tns.registerTensor('delta_d2d', (('d2', ), ('d', )),
initial_values='identity')
self.tns.registerBasisTensor('e_kp', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_pos, glabel_pos)))
self.tns.registerTensor('Kp', (('d2', ), ('d', )))
slice_tau = self.tns.registerContraction('e_ktau', 'delta_d2d')
slice_kp = self.tns.registerContraction('e_kp', 'Kp')
if 'velocity' in self.commonnames2rg:
rlabel_vel, glabel_vel = self.commonnames2rglabels['velocity']
self.tns.registerBasisTensor('e_kv', (('r2', 'g2'), ('r', 'g')),
((rlabel_torque, glabel_torque),
(rlabel_vel, glabel_vel)))
self.tns.registerTensor('Kv', (('d2', ), ('d', )))
slice_kv = self.tns.registerContraction('e_kv', 'Kv')
#add together:
self.tns.registerSum([slice_tau, slice_kp, slice_kv],
result_name='U')
else:
self.tns.registerAddition(slice_tau, slice_kp, result_name='U')
self.tns.registerTensor('I',
self.tns['U'].index_tuples,
initial_values='identity')
if 'velocity' in self.commonnames2rg: #can we add damping terms? (kd)
#Compute the K tensor:
self.tns.registerTensor(
'Kd', (('d2', ), ('d', ))
) #kd is equal to kv but with goal velocity=0 -> only appears here and not in the computation of 'U'
slice_kd = self.tns.registerContraction('e_kv', 'Kd')
U_minus_damping = self.tns.registerAddition('U', slice_kd)
self.tns.registerSubtraction('I', U_minus_damping, result_name='K')
else:
self.tns.registerSubtraction('I', 'U', result_name='K')
self.tns.registerTranspose('U')
self.tns.registerTranspose('K')
#influence of desired mean on expected mean:
term_meanU = self.tns.registerContraction('U', 'DesiredMean')
#influence of desired cov on expected cov:
previous = 'DesiredCov'
# previous = self.tns.registerAddition('DesiredCov','CurrentCov')
previous = self.tns.registerContraction('U', previous)
term_covU = self.tns.registerContraction(previous, '(U)^T')
#up to here we can precompute the equations if goals don't change
self._update_cheap_start = len(self.tns.update_order)
#influence of current cov to expected cov:
previous = self.tns.registerContraction('K', 'CurrentCov')
term_covK = self.tns.registerContraction(previous, '(K)^T')
self.tns.registerAddition(term_covK,
term_covU,
result_name='ExpectedCov')
#influence of current mean on expected mean:
previous = self.tns.registerContraction('K', 'CurrentMean')
self.tns.registerAddition(previous,
term_meanU,
result_name='ExpectedMean')
droptwos = {
'r2': 'r',
'g2': 'g',
'd2': 'd',
'r2_': 'r_',
'g2_': 'g_',
'd2_': 'd_'
}
self.tns.renameIndices('ExpectedCov', droptwos, inPlace=True)
self.tns.renameIndices('ExpectedMean', droptwos, inPlace=True)
#package the result:
self.msd_expected = _mechanicalstate.MechanicalStateDistribution(
self.tns, "ExpectedMean", "ExpectedCov")
#set values, if provided:
self.setDesired(**kwargs)
self.tns.update(*self.tns.update_order[:self._update_cheap_start])
def __init__(self, tns, max_active_inputs=5, force_product_of_distributions=False, force_no_preconditioner=False):
"""
tns: a TensorNameSpace object containing the index definitions to use:
'r': realms index
'g': time derivatives index
'd': degrees of freedom index
max_active_inputs: maximum number of generators that can be active at the same time (limits computation time)
The following two arguments should not be set, unless you want to demonstrate degraded performance:
force_product_of_distributions: if true, compute the mixture like it is usually done in ProMP papers
force_no_preconditioner: Force the preconditioner matrix to be the identity
"""
self.active_generators_indices = []
#et up our tensor namespace:
self.tns = _namedtensors.TensorNameSpace(tns)
self.tns.cloneIndex('r', 'rp')
self.tns.cloneIndex('g', 'gp')
self.tns.cloneIndex('d', 'dp')
self.tns.registerIndex('slots', max_active_inputs)
self.tensorNameLists = {
'Mean': ["Mean{}".format(i) for i in range(max_active_inputs)],
'Cov': ["Cov{}".format(i) for i in range(max_active_inputs)],
}
self.tns.registerTensor("invCovMinimum", (('r_', 'g_', 'd_'),('r', 'g','d')))
self.tns.setTensorToIdentity('invCovMinimum', 0.001)
self.tns.registerTensor("MeanScaledSum", (('r_', 'g_', 'd_'),()), initial_values='zeros' )
self.tns.registerTensor("invCovMixed", (('r_', 'g_', 'd_'),('r', 'g','d')), initial_values='identity' ) #inverse of mixed covariance of last iteration
self.tns.registerScalarMultiplication('invCovMixed', 1.0, result_name = 'preconditioner_wrongindices') #use the last computed invCovMixed as preconditioner
self.tns.renameIndices('preconditioner_wrongindices', {'r_': 'rp', 'g_': 'gp', 'd_':'dp'}, result_name = 'preconditioner') #use the last computed invCovMixed as preconditioner
if force_product_of_distributions:
self.tns.registerReset('preconditioner', 'identity') #precondition covariance tensor
self.tns.registerTensor('alpha', (('slots',),()), initial_values='zeros' )
self.tns.registerElementwiseMultiplication('alpha', 'alpha', result_name='alpha_sqaure')
self.tns.registerSum('alpha', result_name='sumalpha', sumcoordinates=True)
self.tns.registerSum('alpha_sqaure', result_name='sumalpha2', sumcoordinates=True)
self.tns.registerTensor("one", ((),()), initial_values='ones')
self.tns.registerTensor("I", (('rp', 'gp', 'dp'),('r_', 'g_','d_')), initial_values='identity' )
#setup all tensors for each distribution input slot:
self._update_order_upto_slot = []
for slot in range(max_active_inputs):
self.tns.registerTensor(self.tensorNameLists['Mean'][slot], (('r', 'g', 'd'),()) )
self.tns.registerTensor(self.tensorNameLists['Cov'][slot], (('r', 'g', 'd'),('r_', 'g_','d_')) )
alphai = self.tns.registerSlice('alpha', {'slots': slot}, result_name='alpha'+str(slot))
previous = self.tns.registerContraction(self.tensorNameLists['Cov'][slot], 'preconditioner') #precondition covariance tensor
covpreconditioned = self.tns.registerScalarMultiplication(previous, 'sumalpha2') #adjust for total activation, so to not inflate covariances if sumalpha != 1.0
#compute the regularization factor due to (possibly) not being the only one active:
previous = self.tns.registerSubtraction('one', alphai)
previous = self.tns.registerScalarMultiplication(previous, 'sumalpha')
lambdai = self.tns.registerScalarMultiplication('I', previous) #Lambda
previous = self.tns.registerAddition(covpreconditioned, lambdai) #regularize
if force_product_of_distributions: #skip adding the lambdai (decorrelating) term
previous = covpreconditioned
previous = self.tns.registerInverse(previous, flip_underlines=False) #compute precision
previous = self.tns.registerScalarMultiplication(previous, alphai) #scale precision
invcovweighted = self.tns.registerContraction( 'preconditioner', previous, result_name='invCovWeighted{}'.format(slot)) #de-precondition
invcovweightedT = self.tns.registerTranspose(invcovweighted, flip_underlines=False)
#scale means by both precision and activation for computing a sum afterwards
meansscaled = self.tns.registerContraction(self.tensorNameLists['Mean'][slot], invcovweighted, result_name = 'meansscaled{}'.format(slot))
#in order to avoid unnecessary computation, gather all equations we need to recompute up to the current slot:
self._update_order_upto_slot.append(list(self.tns.update_order))
#self._update_order_upto_slot.append(self.tns.update_order.copy() + ['CovMixed_unsym', 'CovMixed_unsym2', '(CovMixed_unsym2)^T','CovMixed', 'MeanMixed'])
include_everything_after = len(self.tns.update_order)
#aggregate all mixed
self.tns.registerReset('invCovMixed', 'zeros')
self.tns.registerReset('MeanScaledSum', 'zeros')
#self.tns.registerAdditionToSlice('invCovSum', 'invCovMinimum' , slice_indices={})
for slot in range(max_active_inputs):
a1 = self.tns.registerAdditionToSlice('invCovMixed', 'invCovWeighted{}'.format(slot) , slice_indices={})
a2 = self.tns.registerAdditionToSlice('MeanScaledSum', 'meansscaled{}'.format(slot) , slice_indices={})
self._update_order_upto_slot[slot] = self._update_order_upto_slot[slot] + self.tns.update_order[include_everything_after:]
include_everything_after = len(self.tns.update_order)
#final computation on the accumulated sums:
previous = self.tns.registerInverse('invCovMixed', result_name='CovMixed', flip_underlines=False)
self.tns.registerContraction('CovMixed', 'MeanScaledSum', result_name='MeanMixed')
#wrap up everything into an msd object to hand out:
self.msd_mixed = _mechanicalstate.MechanicalStateDistribution(self.tns, 'MeanMixed','CovMixed', precisionsName='invCovMixed')
for slot in range(max_active_inputs):
self._update_order_upto_slot[slot] = self._update_order_upto_slot[slot] + self.tns.update_order[include_everything_after:]