Skip to content
Snippets Groups Projects
Commit 8cdb7ea4 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

fix matrix products

parent 1c671d51
No related branches found
No related tags found
No related merge requests found
......@@ -4,7 +4,6 @@ import ex_4_conf as conf
import matplotlib.pyplot as plt
import numpy as np
import plot_utils as plut
import scipy.sparse as spa
from numpy import nan
from numpy.linalg import norm as norm
from tsid_biped import TsidBiped
......@@ -175,16 +174,16 @@ for i in range(-N_pre, N + N_post):
if tsid_biped.formulation.checkContact(tsid_biped.contactRF.name, sol):
T_RF = tsid_biped.contactRF.getForceGeneratorMatrix
f_RF[:, i] = T_RF @ tsid_biped.formulation.getContactForce(
tsid_biped.contactRF.name, sol
f_RF[:, i] = T_RF.dot(
tsid_biped.formulation.getContactForce(tsid_biped.contactRF.name, sol)
)
if f_RF[2, i] > 1e-3:
cop_RF[0, i] = f_RF[4, i] / f_RF[2, i]
cop_RF[1, i] = -f_RF[3, i] / f_RF[2, i]
if tsid_biped.formulation.checkContact(tsid_biped.contactLF.name, sol):
T_LF = tsid_biped.contactRF.getForceGeneratorMatrix
f_LF[:, i] = T_LF @ tsid_biped.formulation.getContactForce(
tsid_biped.contactLF.name, sol
f_LF[:, i] = T_LF.dot(
tsid_biped.formulation.getContactForce(tsid_biped.contactLF.name, sol)
)
if f_LF[2, i] > 1e-3:
cop_LF[0, i] = f_LF[4, i] / f_LF[2, i]
......
......@@ -152,16 +152,16 @@ for i in range(-N_pre, N + N_post):
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
T_RF = tsid.contactRF.getForceGeneratorMatrix
f_RF[:, i] = T_RF @ tsid.formulation.getContactForce(
tsid.contactRF.name, sol
f_RF[:, i] = T_RF.dot(
tsid.formulation.getContactForce(tsid.contactRF.name, sol)
)
if f_RF[2, i] > 1e-3:
cop_RF[0, i] = f_RF[4, i] / f_RF[2, i]
cop_RF[1, i] = -f_RF[3, i] / f_RF[2, i]
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
T_LF = tsid.contactRF.getForceGeneratorMatrix
f_LF[:, i] = T_LF @ tsid.formulation.getContactForce(
tsid.contactLF.name, sol
f_LF[:, i] = T_LF.dot(
tsid.formulation.getContactForce(tsid.contactLF.name, sol)
)
if f_LF[2, i] > 1e-3:
cop_LF[0, i] = f_LF[4, i] / f_LF[2, i]
......
......@@ -60,11 +60,11 @@ A1 = np.random.rand(n, n) + 0.001 * np.eye(n)
b1 = np.random.rand(n)
cost = tsid.ConstraintEquality("c1", A1, b1)
x = np.linalg.inv(A1) @ b1
x = np.linalg.inv(A1).dot(b1)
A_in = np.random.rand(nin, n)
A_lb = np.random.rand(nin) * NORMAL_DISTR_VAR
A_ub = np.random.rand(nin) * NORMAL_DISTR_VAR
constrVal = A_in @ x
constrVal = A_in.dot(x)
for i in range(0, nin):
if A_ub[i] <= A_lb[i]:
......@@ -78,7 +78,7 @@ for i in range(0, nin):
in_const = tsid.ConstraintInequality("ini1", A_in, A_lb, A_ub)
A_eq = np.random.rand(neq, n)
b_eq = A_eq @ x
b_eq = A_eq.dot(x)
eq_const = tsid.ConstraintEquality("eq1", A_eq, b_eq)
const1 = tsid.ConstraintLevel()
......@@ -111,7 +111,7 @@ for name, solver in solver_list:
HQPoutput = solver.solve(HQPData)
assert np.linalg.norm(A_eq @ HQPoutput.x - b_eq, 2) < EPS
assert (A_in @ HQPoutput.x <= A_ub + EPS).all()
assert (A_in @ HQPoutput.x > A_lb - EPS).all()
assert np.linalg.norm(A_eq.dot(HQPoutput.x) - b_eq, 2) < EPS
assert (A_in.dot(HQPoutput.x) <= A_ub + EPS).all()
assert (A_in.dot(HQPoutput.x) > A_lb - EPS).all()
print("-> succesful")
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment