Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
#num_samples = YBase.shape[0]/idf.model.num_dofs
l = (float(idf.base_error) / len(p_nid)) * idf.opt['regularizationFactor']
#TODO: also use symengine to gain speedup?
#p = BlockMatrix([[(K*delta)], [delta_nonid]])
#Y = BlockMatrix([[Matrix(R1), ZeroMatrix(R1.shape[0], len(p_nid))],
# [ZeroMatrix(len(p_nid), R1.shape[1]), l*Identity(len(p_nid))]])
Y = BlockMatrix([[R1*(K*delta)],[l*Identity(len(p_nid))*delta_nonid]]).as_explicit()
rho1_hat = np.concatenate((rho1, l*idf.model.xStdModel[p_nid]))
e_rho1 = (Matrix(rho1_hat - contactForces) - Y)
else:
try:
from symengine import DenseMatrix as eMatrix
if idf.opt['verbose']:
print('using symengine')
edelta = eMatrix(delta.shape[0], delta.shape[1], delta)
eK = eMatrix(K.shape[0], K.shape[1], K)
eR1 = eMatrix(R1.shape[0], R1.shape[1], Matrix(R1))
Y = eR1*eK*edelta
e_rho1 = Matrix(eMatrix(rho1) - contactForces - Y)
except ImportError:
if idf.opt['verbose']:
print('not using symengine')
Y = R1*(K*delta)
e_rho1 = Matrix(rho1 - contactForces) - Y
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
u = Symbol('u')
l = (float(idf.base_error) / len(p_nid)) * idf.opt['regularizationFactor']
#TODO: also use symengine to gain speedup?
#p = BlockMatrix([[(K*delta)], [delta_nonid]])
#Y = BlockMatrix([[Matrix(R1), ZeroMatrix(R1.shape[0], len(p_nid))],
# [ZeroMatrix(len(p_nid), R1.shape[1]), l*Identity(len(p_nid))]])
Y = BlockMatrix([[R1*(K*delta)],[l*Identity(len(p_nid))*delta_nonid]]).as_explicit()
rho1_hat = np.concatenate((rho1, l*idf.model.xStdModel[p_nid]))
e_rho1 = (Matrix(rho1_hat - contactForces) - Y)
else:
try:
from symengine import DenseMatrix as eMatrix
if idf.opt['verbose']:
print('using symengine')
edelta = eMatrix(delta.shape[0], delta.shape[1], delta)
eK = eMatrix(K.shape[0], K.shape[1], K)
eR1 = eMatrix(R1.shape[0], R1.shape[1], Matrix(R1))
Y = eR1*eK*edelta
e_rho1 = Matrix(eMatrix(rho1) - contactForces - Y)
except ImportError:
if idf.opt['verbose']:
print('not using symengine')
Y = R1*(K*delta)
e_rho1 = Matrix(rho1 - contactForces) - Y
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
#TODO: also use symengine to gain speedup?
#p = BlockMatrix([[(K*delta)], [delta_nonid]])
#Y = BlockMatrix([[Matrix(R1), ZeroMatrix(R1.shape[0], len(p_nid))],
# [ZeroMatrix(len(p_nid), R1.shape[1]), l*Identity(len(p_nid))]])
Y = BlockMatrix([[R1*(K*delta)],[l*Identity(len(p_nid))*delta_nonid]]).as_explicit()
rho1_hat = np.concatenate((rho1, l*idf.model.xStdModel[p_nid]))
e_rho1 = (Matrix(rho1_hat - contactForces) - Y)
else:
try:
from symengine import DenseMatrix as eMatrix
if idf.opt['verbose']:
print('using symengine')
edelta = eMatrix(delta.shape[0], delta.shape[1], delta)
eK = eMatrix(K.shape[0], K.shape[1], K)
eR1 = eMatrix(R1.shape[0], R1.shape[1], Matrix(R1))
Y = eR1*eK*edelta
e_rho1 = Matrix(eMatrix(rho1) - contactForces - Y)
except ImportError:
if idf.opt['verbose']:
print('not using symengine')
Y = R1*(K*delta)
e_rho1 = Matrix(rho1 - contactForces) - Y
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
[e_rho1, I(e_rho1.shape[0])]])
#p = BlockMatrix([[(K*delta)], [delta_nonid]])
#Y = BlockMatrix([[Matrix(R1), ZeroMatrix(R1.shape[0], len(p_nid))],
# [ZeroMatrix(len(p_nid), R1.shape[1]), l*Identity(len(p_nid))]])
Y = BlockMatrix([[R1*(K*delta)],[l*Identity(len(p_nid))*delta_nonid]]).as_explicit()
rho1_hat = np.concatenate((rho1, l*idf.model.xStdModel[p_nid]))
e_rho1 = (Matrix(rho1_hat - contactForces) - Y)
else:
try:
from symengine import DenseMatrix as eMatrix
if idf.opt['verbose']:
print('using symengine')
edelta = eMatrix(delta.shape[0], delta.shape[1], delta)
eK = eMatrix(K.shape[0], K.shape[1], K)
eR1 = eMatrix(R1.shape[0], R1.shape[1], Matrix(R1))
Y = eR1*eK*edelta
e_rho1 = Matrix(eMatrix(rho1) - contactForces - Y)
except ImportError:
if idf.opt['verbose']:
print('not using symengine')
Y = R1*(K*delta)
e_rho1 = Matrix(rho1 - contactForces) - Y
if idf.opt['verbose'] > 1:
print("Step 2...", time.ctime())
# minimize estimation error of to-be-found parameters delta
# (regressor dot std variables projected to base - contacts should be close to measured torques)
u = Symbol('u')
U_rho = BlockMatrix([[Matrix([u - rho2_norm_sqr]), e_rho1.T],
[e_rho1, I(e_rho1.shape[0])]])
if idf.opt['verbose'] > 1: