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

[Python] allow Python 3 & fix format

parent d904cd15
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
python/test/binding_tests.py
+ 25
− 23
View file @ c9d5314c
import hpp_spline # noqa - necessary to register spline::bezier_curve
import numpy as np import numpy as np
from hpp_centroidal_dynamics import * from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP
from hpp_spline import *
from numpy import array, asmatrix, matrix from numpy import array, asmatrix, matrix
from hpp_bezier_com_traj import * from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData,
computeCOMTraj, zeroStepCapturability)
#testing constructors # testing constructors
eq = Equilibrium("test", 54., 4) eq = Equilibrium("test", 54., 4)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES) eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES) eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
...@@ -13,26 +14,26 @@ eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False) ...@@ -13,26 +14,26 @@ eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1) eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True) eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
#whether useWarmStart is enable (True by default) # whether useWarmStart is enable (True by default)
previous = eq.useWarmStart() previous = eq.useWarmStart()
#enable warm start in solver (only for QPOases) # enable warm start in solver (only for QPOases)
eq.setUseWarmStart(False) eq.setUseWarmStart(False)
assert (previous != eq.useWarmStart()) assert (previous != eq.useWarmStart())
#access solver name # access solver name
assert (eq.getName() == "test") assert (eq.getName() == "test")
z = array([0., 0., 1.]) z = array([0., 0., 1.])
P = asmatrix(array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]])) P = asmatrix(array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]]))
N = asmatrix(array([z for _ in range(4)])) N = asmatrix(array([z for _ in range(4)]))
#setting contact positions and normals, as well as friction coefficients # setting contact positions and normals, as well as friction coefficients
eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
#~ eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP) # eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
# setting up optimization problem # setting up optimization problem
c0 = matrix([0., 0., 1.]).T c0 = matrix([0., 0., 1.]).T
#~ dc0 = matrix(np.random.uniform(-1, 1, size=3)); # dc0 = matrix(np.random.uniform(-1, 1, size=3));
dc0 = matrix([0.1, 0., 0.]).T dc0 = matrix([0.1, 0., 0.]).T
l0 = matrix([0., 0., 0.]).T l0 = matrix([0., 0., 0.]).T
T = 1.2 T = 1.2
...@@ -56,7 +57,7 @@ kin = 10 * np.ones(3) ...@@ -56,7 +57,7 @@ kin = 10 * np.ones(3)
# #
# a = zeroStepCapturability(eq, c0, dc0, l0, True, T, tstep, Kin, matrix(kin)) # a = zeroStepCapturability(eq, c0, dc0, l0, True, T, tstep, Kin, matrix(kin))
#testing contactData # testing contactData
cData = ContactData(Equilibrium("test", 54., 4)) cData = ContactData(Equilibrium("test", 54., 4))
ceq = cData.contactPhase_ ceq = cData.contactPhase_
ceq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) ceq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
...@@ -68,7 +69,7 @@ Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) ...@@ -68,7 +69,7 @@ Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
excep = False excep = False
try: try:
cData.Kin_ cData.Kin_
except RuntimeError, e: except RuntimeError:
excep = True excep = True
assert excep, "[ERROR] No kin assigned should have raised exception" assert excep, "[ERROR] No kin assigned should have raised exception"
cData.setKinematicConstraints(Id, matrix([0., 0., 1.]).T) cData.setKinematicConstraints(Id, matrix([0., 0., 1.]).T)
...@@ -77,14 +78,14 @@ cData.Kin_ ...@@ -77,14 +78,14 @@ cData.Kin_
excep = False excep = False
try: try:
cData.setKinematicConstraints(Id, matrix([0., 0., 0., 1.]).T) cData.setKinematicConstraints(Id, matrix([0., 0., 0., 1.]).T)
except RuntimeError, e: except RuntimeError:
excep = True excep = True
assert excep, "[ERROR] Miss matching matrix and vector should raise an error" assert excep, "[ERROR] Miss matching matrix and vector should raise an error"
excep = False excep = False
try: try:
cData.Ang_ cData.Ang_
except RuntimeError, e: except RuntimeError:
excep = True excep = True
assert excep, "[ERROR] No Ang_ assigned should have raised exception" assert excep, "[ERROR] No Ang_ assigned should have raised exception"
cData.setAngularConstraints(Id, matrix([0., 0., 1.]).T) cData.setAngularConstraints(Id, matrix([0., 0., 1.]).T)
...@@ -93,11 +94,11 @@ cData.Ang_ ...@@ -93,11 +94,11 @@ cData.Ang_
excep = False excep = False
try: try:
cData.setAngularConstraints(Id, matrix([0., 0., 0., 1.]).T) cData.setAngularConstraints(Id, matrix([0., 0., 0., 1.]).T)
except RuntimeError, e: except RuntimeError:
excep = True excep = True
assert excep, "[ERROR] Missmatching matrix and vector should raise an error" assert excep, "[ERROR] Missmatching matrix and vector should raise an error"
#testing constraints # testing constraints
c = Constraints() c = Constraints()
old = c.constrainAcceleration_ old = c.constrainAcceleration_
c.constrainAcceleration_ = not old c.constrainAcceleration_ = not old
...@@ -113,7 +114,7 @@ old = c.reduce_h_ ...@@ -113,7 +114,7 @@ old = c.reduce_h_
c.reduce_h_ = .235 c.reduce_h_ = .235
assert c.reduce_h_ != old assert c.reduce_h_ != old
#testing problem data # testing problem data
c = ProblemData() c = ProblemData()
nv = matrix([0., 0., 10.]).T nv = matrix([0., 0., 10.]).T
...@@ -159,23 +160,24 @@ def initContactData(pD): ...@@ -159,23 +160,24 @@ def initContactData(pD):
pD.c0_ = c0 pD.c0_ = c0
pD.dc0_ = dc0 pD.dc0_ = dc0
res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05) res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05)
#test glpk only if defined # test glpk only if defined
try: try:
test = SOLVER_GLPK test = SOLVER_GLPK
res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_GLPK) res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_GLPK)
except NameError, e: except NameError:
print "[WARNING] SOLVER_GLPK is not defined. Consider installing GLPK if you are using CROC with a force formulation" print("[WARNING] SOLVER_GLPK is not defined.")
print("Consider installing GLPK if you are using CROC with a force formulation")
res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_QUADPROG) res = computeCOMTraj(pD, matrix([0.4, 0.4, 0.4]).T, 0.05, SOLVER_QUADPROG)
#~ res = computeCOMTraj(pD,matrix([0.4,0.4,0.4]).T,0.05,SOLVER_QUADPROG_SPARSE) # res = computeCOMTraj(pD,matrix([0.4,0.4,0.4]).T,0.05,SOLVER_QUADPROG_SPARSE)
assert np.linalg.norm(res.c_of_t.derivate(1.2, 1)) < 0.00000001 assert np.linalg.norm(res.c_of_t.derivate(1.2, 1)) < 0.00000001
# non matching time step and contact phases # non matching time step and contact phases
excep = False excep = False
try: try:
res = computeCOMTraj(pD, matrix([0.4, 0.4]).T, 0.05) res = computeCOMTraj(pD, matrix([0.4, 0.4]).T, 0.05)
except RuntimeError, e: except RuntimeError:
excep = True excep = True
assert excep, "[ERROR] computeCOMTraj should have raised exception" assert excep, "[ERROR] computeCOMTraj should have raised exception"
print "all tests passed" print("all tests passed")
python/test/compare_pin_inv_dyn.py
+ 288
− 240
View file @ c9d5314c
...@@ -5,175 +5,222 @@ Created on Thu Sep 1 16:54:39 2016 ...@@ -5,175 +5,222 @@ Created on Thu Sep 1 16:54:39 2016
@author: stonneau @author: stonneau
""" """
from pinocchio_inv_dyn.optimization.solver_LP_abstract import LP_status, LP_status_string from __future__ import print_function
from pinocchio_inv_dyn.multi_contact.stability_criterion import Bunch
from pinocchio_inv_dyn.optimization.solver_LP_abstract import getNewSolver
from spline import bezier, bezier6, polynom, bernstein
from numpy import array, vstack, zeros, ones, sqrt, matrix, asmatrix, asarray, identity
from numpy import cross as X
from numpy.linalg import norm
import numpy as np
from math import atan, pi from math import atan, pi
from centroidal_dynamics import * import numpy as np
from pinocchio_inv_dyn.multi_contact.utils import generate_contacts, compute_GIWC, find_static_equilibrium_com, can_I_stop, check_static_eq from numpy import array, asarray, asmatrix
import cProfile from numpy import cross as X
from numpy import matrix, zeros
from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import BezierZeroStepCapturability ,compute_CWC
from bezier_com_traj import * from centroidal_dynamics import Equilibrium, EquilibriumAlgorithm
from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import (BezierZeroStepCapturability,
compute_CWC)
from pinocchio_inv_dyn.multi_contact.utils import (check_static_eq, find_static_equilibrium_com, generate_contacts)
from spline import bezier
__EPS = 1e-5; __EPS = 1e-5
np.set_printoptions(precision=2, suppress=True, linewidth=100); np.set_printoptions(precision=2, suppress=True, linewidth=100)
##################### #####################
# EQUILIBRIUM CHECK # # EQUILIBRIUM CHECK #
##################### #####################
def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])): def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
w1 = m * (ddc - g_vec) w1 = m * (ddc - g_vec)
return array(w1.tolist() + (X(c, w1) + dL).tolist()) return array(w1.tolist() + (X(c, w1) + dL).tolist())
def is_stable(H,c=array([0.,0.,0.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
w = compute_w(c, ddc, dL, m, g_vec) def is_stable(H,
c=array([0., 0., 0.]),
ddc=array([0., 0., 0.]),
dL=array([0., 0., 0.]),
m=54.,
g_vec=array([0., 0., -9.81]),
robustness=0.):
w = compute_w(c, ddc, dL, m, g_vec)
res = np.max(H.dot(w)) res = np.max(H.dot(w))
if(res > -robustness): if res > -robustness:
print "offset ", res, dL print("offset ", res, dL)
w = compute_w(c, ddc, array([0.,0.,0.]), m, g_vec) w = compute_w(c, ddc, array([0., 0., 0.]), m, g_vec)
print "offset2 ", np.max(H.dot(w)) print("offset2 ", np.max(H.dot(w)))
return res<=-robustness return res <= -robustness
def allZeros(t): def allZeros(t):
return zeros(3) return zeros(3)
def __check_trajectory(p0,p1,p2,p3,T,H, mass, g, time_step = 0.1, dL = allZeros):
robustness = -0.000001 def __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros):
if(time_step < 0): if time_step < 0:
time_step = 0.01 time_step = 0.01
robustness = -0.01
resolution = int(T / time_step) resolution = int(T / time_step)
wps = [p0,p1,p2,p3]; wps = matrix([pi.tolist() for pi in wps]).transpose() wps = [p0, p1, p2, p3]
wps = matrix([pi.tolist() for pi in wps]).transpose()
c_t = bezier(wps) c_t = bezier(wps)
ddc_t = c_t.compute_derivate(2) ddc_t = c_t.compute_derivate(2)
def c_tT(t): def c_tT(t):
return asarray(c_t(t/T)).flatten() return asarray(c_t(t / T)).flatten()
def ddc_tT(t): def ddc_tT(t):
return 1./(T*T) * asarray(ddc_t(t/T)).flatten() return 1. / (T * T) * asarray(ddc_t(t / T)).flatten()
def dL_tT(t): def dL_tT(t):
return 1./(T) * asarray(dL(t/T)).flatten() return 1. / (T) * asarray(dL(t / T)).flatten()
for i in range(resolution): for i in range(resolution):
t = T * float(i) / float(resolution) t = T * float(i) / float(resolution)
if not (is_stable(H,c=c_tT(t), ddc=ddc_tT(t), dL=dL_tT(t), m = mass, g_vec=g, robustness = -0.00001)): if not (is_stable(H, c=c_tT(t), ddc=ddc_tT(t), dL=dL_tT(t), m=mass, g_vec=g, robustness=-0.00001)):
if t > 0.1: if t > 0.1:
raise ValueError("trajectory is not stale ! at ", t) raise ValueError("trajectory is not stale ! at ", t)
else: else:
print is_stable(H,c=c_tT(t), ddc=ddc_tT(t), dL=asarray(dL(t/T)).flatten(), m = mass, g_vec=g, robustness = -0.00001) print(
print "failed at 0" is_stable(H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=asarray(dL(t / T)).flatten(),
m=mass,
g_vec=g,
robustness=-0.00001))
print("failed at 0")
################### ###################
# LP BEZIER TESTS # # LP BEZIER TESTS #
################### ###################
def test_continuous_cpp_vs_continuous_py(N_CONTACTS = 2, solver='qpoases', verb=0):
def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0):
DO_PLOTS = False;
PLOT_3D = False; mu = 0.5
mu = 0.5; # friction coefficient # friction coefficient
lx = 0.1; # half foot size in x direction lx = 0.1
ly = 0.07; # half foot size in y direction # half foot size in x direction
#First, generate a contact configuration ly = 0.07
CONTACT_POINT_UPPER_BOUNDS = [ 0.5, 0.5, 0.5]; # half foot size in y direction
CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]; # First, generate a contact configuration
gamma = atan(mu); # half friction cone angle CONTACT_POINT_UPPER_BOUNDS = [0.5, 0.5, 0.5]
RPY_LOWER_BOUNDS = [-2*gamma, -2*gamma, -pi]; CONTACT_POINT_LOWER_BOUNDS = [-0.5, -0.5, 0.0]
RPY_UPPER_BOUNDS = [+2*gamma, +2*gamma, +pi]; gamma = atan(mu)
MIN_CONTACT_DISTANCE = 0.3; # half friction cone angle
RPY_LOWER_BOUNDS = [-2 * gamma, -2 * gamma, -pi]
RPY_UPPER_BOUNDS = [+2 * gamma, +2 * gamma, +pi]
MIN_CONTACT_DISTANCE = 0.3
global mass global mass
global g_vector global g_vector
X_MARG = 0.07; X_MARG = 0.07
Y_MARG = 0.07; Y_MARG = 0.07
succeeded = False; succeeded = False
while(not succeeded): while (not succeeded):
(p, N) = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, p, N = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False); RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False)
X_LB = np.min(p[:,0]-X_MARG); X_LB = np.min(p[:, 0] - X_MARG)
X_UB = np.max(p[:,0]+X_MARG); X_UB = np.max(p[:, 0] + X_MARG)
Y_LB = np.min(p[:,1]-Y_MARG); Y_LB = np.min(p[:, 1] - Y_MARG)
Y_UB = np.max(p[:,1]+Y_MARG); Y_UB = np.max(p[:, 1] + Y_MARG)
Z_LB = np.max(p[:,2]+0.3); Z_LB = np.max(p[:, 2] + 0.3)
Z_UB = np.max(p[:,2]+1.5); Z_UB = np.max(p[:, 2] + 1.5)
#~ (H,h) = compute_GIWC(p, N, mu, False); # (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu); h = zeros(H.shape[0]) H = -compute_CWC(p, N, mass, mu)
(succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h); h = zeros(H.shape[0])
(succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h)
dc0 = np.random.uniform(-1, 1, size=3);
dc0 = np.random.uniform(-1, 1, size=3)
Z_MIN = np.max(p[:,2])-0.1;
Ineq_kin = zeros([3,3]); Ineq_kin[2,2] = -1 Z_MIN = np.max(p[:, 2]) - 0.1
ineq_kin = zeros(3); ineq_kin[2] = -Z_MIN Ineq_kin = zeros([3, 3])
Ineq_kin[2, 2] = -1
ineq_kin = zeros(3)
bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = [Ineq_kin,ineq_kin ]); ineq_kin[2] = -Z_MIN
eqCpp = Equilibrium("dyn_eq2", mass, 4)
eqCpp.setNewContacts(asmatrix(p),asmatrix(N),mu,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP) bezierSolver = BezierZeroStepCapturability("ss",
#~ bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver, kinematic_constraints = None); c0,
#~ stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver); dc0,
window_times = [1]+ [0.1*i for i in range(1,10)] + [0.1*i for i in range(11,21)] #try nominal time first p,
#~ window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first N,
#~ window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first mu,
#~ window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first g_vector,
#~ window_times = [0.7] mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin])
eqCpp = Equilibrium("dyn_eq2", mass, 4)
eqCpp.setNewContacts(asmatrix(p), asmatrix(N), mu, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver,
# kinematic_constraints = None);
# stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver);
window_times = [1] + [0.1 * i for i in range(1, 10)] + [0.1 * i for i in range(11, 21)] # try nominal time first
# window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
# window_times = [0.7]
found = False found = False
time_step_check = -0.2 time_step_check = -0.2
for i, el in enumerate(window_times): for i, el in enumerate(window_times):
if (found): if (found):
break break
res = bezierSolver.can_I_stop(T=el, time_step = time_step_check); res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res.is_stable): if (res.is_stable):
found = True found = True
#~ print "continuous found at ", el # print("continuous found at ", el)
__check_trajectory(bezierSolver._p0, bezierSolver._p1, res.c, res.c, el, bezierSolver._H, __check_trajectory(bezierSolver._p0,
bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose())) bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()))
if i != 0: if i != 0:
print "continuous Failed to stop at 1, but managed to stop at ", el print("continuous Failed to stop at 1, but managed to stop at ", el)
found = False found = False
time_step_check = 0.05 time_step_check = 0.05
for i, el in enumerate(window_times): for i, el in enumerate(window_times):
if (found): if (found):
break break
res2 = bezierSolver.can_I_stop(T=el, time_step = time_step_check, l0 = zeros(3)); res2 = bezierSolver.can_I_stop(T=el, time_step=time_step_check, l0=zeros(3))
if (res2.is_stable): if (res2.is_stable):
found = True found = True
#~ print "ang_momentum found at ", el # print("ang_momentum found at ", el)
__check_trajectory(bezierSolver._p0, bezierSolver._p1, res2.c, res2.c, el, bezierSolver._H, __check_trajectory(
#~ bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t) bezierSolver._p0,
bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose())) bezierSolver._p1,
res2.c,
res2.c,
el,
bezierSolver._H,
# bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
if i != 0: if i != 0:
print "ang_momentum Failed to stop at 1, but managed to stop at ", el print("ang_momentum Failed to stop at 1, but managed to stop at ", el)
#~ res2 = None # res2 = None
#~ try: # try:
#~ res2 = stabilitySolver.can_I_stop(); # res2 = stabilitySolver.can_I_stop();
#~ except Exception as e: # except Exception as e:
#~ pass # pass
if(res2.is_stable != res.is_stable ): if (res2.is_stable != res.is_stable):
if(res.is_stable): if (res.is_stable):
print "continuous won" print("continuous won")
else: else:
print "ang_momentum won" print("ang_momentum won")
return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N return res2.is_stable, res.is_stable, res2, res, c0, dc0, H, h, p, N
if __name__=="__main__": if __name__ == "__main__":
g_vector = np.array([0., 0., -9.81]); g_vector = np.array([0., 0., -9.81])
mass = 75; # mass of the robot mass = 75
from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion # mass of the robot
from matplotlib import rcParams from matplotlib import rcParams
rcParams.update({'font.size': 11}) rcParams.update({'font.size': 11})
mine_won = 0 mine_won = 0
...@@ -181,161 +228,162 @@ if __name__=="__main__": ...@@ -181,161 +228,162 @@ if __name__=="__main__":
total_stop = 0 total_stop = 0
total_not_stop = 0 total_not_stop = 0
total_disagree = 0 total_disagree = 0
margin_i_win_he_lose = [] # remaining speed margin_i_win_he_lose = [] # remaining speed
margin_he_wins_i_lost = [] # remaining acceleration margin_he_wins_i_lost = [] # remaining acceleration
curves_when_i_win = [] curves_when_i_win = []
#~ times_disagree = [] # times_disagree = []
#~ times_agree_stop = [] # times_agree_stop = []
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
from matplotlib import cm
def __plot_3d_points(ax, points, c='b'):
def __plot_3d_points(ax, points, c = 'b'):
xs = [point[0] for point in points] xs = [point[0] for point in points]
ys = [point[1] for point in points] ys = [point[1] for point in points]
zs = [point[2] for point in points] zs = [point[2] for point in points]
ax.scatter(xs[:1], ys[:1], zs[:1], c='r') ax.scatter(xs[:1], ys[:1], zs[:1], c='r')
ax.scatter(xs[1:-1], ys[1:-1], zs[1:-1], c=c) ax.scatter(xs[1:-1], ys[1:-1], zs[1:-1], c=c)
ax.scatter(xs[-1:], ys[-1:], zs[-1:], c='g') ax.scatter(xs[-1:], ys[-1:], zs[-1:], c='g')
ax.set_xlabel('X Label', fontsize = 11) ax.set_xlabel('X Label', fontsize=11)
ax.set_ylabel('Y Label', fontsize = 11) ax.set_ylabel('Y Label', fontsize=11)
ax.set_zlabel('Z Label', fontsize = 11) ax.set_zlabel('Z Label', fontsize=11)
from pinocchio_inv_dyn.geom_utils import is_vector_inside_cone, plot_inequalities def plot_support_polygon(H, h, p, N, ax, c0):
def plot_support_polygon(H,h,p,N,ax, c0): from pinocchio_inv_dyn.multi_contact.utils import compute_support_polygon
from pinocchio_inv_dyn.multi_contact.utils import generate_contacts, find_static_equilibrium_com, compute_GIWC, compute_support_polygon # (H,h) = compute_GIWC(p, N, mu);
#~ (H,h) = compute_GIWC(p, N, mu);
global mass global mass
global g_vector global g_vector
(B_sp, b_sp) = compute_support_polygon(H, h, mass, g_vector, eliminate_redundancies=False); (B_sp, b_sp) = compute_support_polygon(H, h, mass, g_vector, eliminate_redundancies=False)
X_MIN = np.min(p[:,0]); X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:,0]); X_MAX = np.max(p[:, 0])
X_MIN -= 0.5*(X_MAX-X_MIN); X_MIN -= 0.5 * (X_MAX - X_MIN)
X_MAX += 0.5*(X_MAX-X_MIN); X_MAX += 0.5 * (X_MAX - X_MIN)
Y_MIN = np.min(p[:,1]); Y_MIN = np.min(p[:, 1])
Y_MAX = np.max(p[:,1]); Y_MAX = np.max(p[:, 1])
Y_MIN -= 0.5*(Y_MAX-Y_MIN); Y_MIN -= 0.5 * (Y_MAX - Y_MIN)
Y_MAX += 0.5*(Y_MAX-Y_MIN); Y_MAX += 0.5 * (Y_MAX - Y_MIN)
num_steps = 50 num_steps = 50
dx = (X_MAX - X_MIN) / float(num_steps) dx = (X_MAX - X_MIN) / float(num_steps)
dy = (Y_MAX - Y_MIN) / float(num_steps) dy = (Y_MAX - Y_MIN) / float(num_steps)
#~ points = [(X_MIN + dx * i, Y_MAX + dy * j, 0.) for i in range(num_steps+1) for j in range(num_steps+1) if check_static_eq(H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j,0.]), g_vector) ] # points = [(X_MIN + dx * i, Y_MAX + dy * j, 0.) for i in range(num_steps+1) for j in range(num_steps+1) if
#~ points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, -0.5] for i in range(num_steps+1) for j in range(num_steps+1) if check_static_eq(H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j,0.], g_vector)] # check_static_eq(H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j,0.]), g_vector) ]
points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, 0] for i in range(num_steps+1) for j in range(num_steps+1) ] # points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, -0.5] for i in range(num_steps+1) for j in
pts2 = [] # range(num_steps+1) if check_static_eq(H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j,0.], g_vector)]
points = [c0] + [[X_MIN + dx * i, Y_MIN + dy * j, 0] for i in range(num_steps + 1)
for j in range(num_steps + 1)]
pts2 = []
for pt in points: for pt in points:
if check_static_eq(H, h, mass, pt, g_vector): if check_static_eq(H, h, mass, pt, g_vector):
pts2 += [pt] pts2 += [pt]
__plot_3d_points(ax, pts2, c="r") __plot_3d_points(ax, pts2, c="r")
#~ __plot_3d_points(ax, points2, c="r") # __plot_3d_points(ax, points2, c="r")
__plot_3d_points(ax, p, c="r") __plot_3d_points(ax, p, c="r")
#~ for i in range(num_steps): # for i in range(num_steps):
#~ for j in range(num_steps): # for j in range(num_steps):
#~ plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False); # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
#~ plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False); # plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
#~ plt.show(); # plt.show();
def plot_win_curve(n = -1, num_pts = 20): def plot_win_curve(n=-1, num_pts=20):
global curves_when_i_win global curves_when_i_win
if n > len(curves_when_i_win) -1 or n < 0: if n > len(curves_when_i_win) - 1 or n < 0:
print "n bigger than num curves or equal to -1, plotting last curve" print("n bigger than num curves or equal to -1, plotting last curve")
n = len(curves_when_i_win) -1 n = len(curves_when_i_win) - 1
c0, dc0, c_end, dc_end, t_max, c_of_t, dc_of_t, ddc_of_t, H, h, p, N, dl_of_t, L_of_t = curves_when_i_win[n] c0, dc0, c_end, dc_end, t_max, c_of_t, dc_of_t, ddc_of_t, H, h, p, N, dl_of_t, L_of_t = curves_when_i_win[n]
print "c0 ", c0 print("c0 ", c0)
print "Is c0 stable ? ", check_static_eq(H, h, mass, c0, g_vector) print("Is c0 stable ? ", check_static_eq(H, h, mass, c0, g_vector))
print "Is end stable ? ", check_static_eq(H, h, mass, c_of_t(t_max), g_vector) print("Is end stable ? ", check_static_eq(H, h, mass, c_of_t(t_max), g_vector))
w = np.zeros(6); w = np.zeros(6)
w[2] = -mass*9.81; w[2] = -mass * 9.81
w[3:] = mass*np.cross(c_of_t(t_max), g_vector); w[3:] = mass * np.cross(c_of_t(t_max), g_vector)
print 'max ', np.max(np.dot(H, w) - h) print('max ', np.max(np.dot(H, w) - h))
X_MIN = np.min(p[:,0]); X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:,0]); X_MAX = np.max(p[:, 0])
X_MIN -= 0.1*(X_MAX-X_MIN); X_MIN -= 0.1 * (X_MAX - X_MIN)
X_MAX += 0.1*(X_MAX-X_MIN); X_MAX += 0.1 * (X_MAX - X_MIN)
Y_MIN = np.min(p[:,1]); Y_MIN = np.min(p[:, 1])
Y_MAX = np.max(p[:,1]); Y_MAX = np.max(p[:, 1])
print "Is XMIN ? ", X_MIN print("Is XMIN ? ", X_MIN)
print "Is XMAX ? ", X_MAX print("Is XMAX ? ", X_MAX)
print "Is YMIN ? ", Y_MIN print("Is YMIN ? ", Y_MIN)
print "Is YMAX ? ", Y_MAX print("Is YMAX ? ", Y_MAX)
delta = t_max / float(num_pts) delta = t_max / float(num_pts)
num_pts +=1; num_pts += 1
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(221, projection='3d') ax = fig.add_subplot(221, projection='3d')
#~ ax = fig.add_subplot(221) # ax = fig.add_subplot(221)
__plot_3d_points(ax, [c_of_t(i * delta) for i in range(num_pts)]) __plot_3d_points(ax, [c_of_t(i * delta) for i in range(num_pts)])
__plot_3d_points(ax, [c0 + (c_end-c0)* i * delta for i in range(num_pts)], c = "y") __plot_3d_points(ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y")
plot_support_polygon(H,h,p,N,ax, c0) plot_support_polygon(H, h, p, N, ax, c0)
ax = fig.add_subplot(222, projection='3d') ax = fig.add_subplot(222, projection='3d')
__plot_3d_points(ax, [dc_of_t(i * delta) for i in range(num_pts)]) __plot_3d_points(ax, [dc_of_t(i * delta) for i in range(num_pts)])
__plot_3d_points(ax, [dc0 + (dc_end-dc0)* i * delta for i in range(num_pts)], c = "y") __plot_3d_points(ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)], c="y")
ax = fig.add_subplot(223, projection='3d') ax = fig.add_subplot(223, projection='3d')
#~ __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)]) # __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
#~ ax = fig.add_subplot(224, projection='3d') # ax = fig.add_subplot(224, projection='3d')
__plot_3d_points(ax, [L_of_t(i * delta) for i in range(num_pts)], c = "y") __plot_3d_points(ax, [L_of_t(i * delta) for i in range(num_pts)], c="y")
#~ __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)], c = "y") # __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)], c = "y")
ax = fig.add_subplot(224, projection='3d') ax = fig.add_subplot(224, projection='3d')
__plot_3d_points(ax, [dl_of_t(i * delta) for i in range(num_pts)]) __plot_3d_points(ax, [dl_of_t(i * delta) for i in range(num_pts)])
#~ ax = fig.add_subplot(121, projection='3d') # ax = fig.add_subplot(121, projection='3d')
#~ __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)]) # __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
#~ ax = fig.add_subplot(122, projection='3d') # ax = fig.add_subplot(122, projection='3d')
#~ __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)]) # __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)])
#~ print "cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )) # print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
#~ print "init acceleration ", ddc_of_t(0) # print("init acceleration ", ddc_of_t(0))
print "init velocity ", dc_of_t(0) print("init velocity ", dc_of_t(0))
print "end velocity ", dc_of_t(t_max) print("end velocity ", dc_of_t(t_max))
#~ print "cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) )) # print("cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0))))
#~ plt.show() # plt.show()
def plot_n_win_curves(n = -1, num_pts = 50): def plot_n_win_curves(n=-1, num_pts=50):
global curves_when_i_win global curves_when_i_win
if n > len(curves_when_i_win) -1 or n < 0: if n > len(curves_when_i_win) - 1 or n < 0:
print "n bigger than num curves or equal to -1, plotting last curve" print("n bigger than num curves or equal to -1, plotting last curve")
n = len(curves_when_i_win) -1 n = len(curves_when_i_win) - 1
for i in range(n): for i in range(n):
plot_win_curve(i, num_pts) plot_win_curve(i, num_pts)
plt.show() plt.show()
num_tested = 0. num_tested = 0.
for i in range(1000): for i in range(1000):
num_tested = i-1 num_tested = i - 1
mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = test_continuous_cpp_vs_continuous_py() mine, theirs, r_mine, r_theirs, c0, dc0, H, h, p, N = test_continuous_cpp_vs_continuous_py()
#~ mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = test_momentum_cpp_vs_momentum_py() # mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = test_momentum_cpp_vs_momentum_py()
#~ print "H test", H.shape # print("H test", H.shape)
if(mine != theirs): if (mine != theirs):
total_disagree+=1 total_disagree += 1
if(mine): if (mine):
#~ times_disagree +=[r_mine.t] # times_disagree +=[r_mine.t]
#~ raise ValueError (" BITE ME " ) # raise ValueError (" BITE ME " )
margin_i_win_he_lose+=[r_theirs.dc] margin_i_win_he_lose += [r_theirs.dc]
curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t, r_mine.dc_of_t, r_mine.ddc_of_t, H[:], h[:], p[:], N, r_mine.dL_of_t, r_mine.L_of_t)] curves_when_i_win += [
#~ print "margin when he lost: ", norm(r_theirs.dc) (c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t, r_mine.dc_of_t,
#~ else: r_mine.ddc_of_t, H[:], h[:], p[:], N, r_mine.dL_of_t, r_mine.L_of_t)
#~ times_disagree +=[r_theirs.t] ]
# print("margin when he lost: ", norm(r_theirs.dc))
# else:
# times_disagree +=[r_theirs.t]
if mine: if mine:
mine_won +=1 mine_won += 1
else: else:
mine_lose +=1 mine_lose += 1
elif(mine or theirs): elif (mine or theirs):
total_stop+=1 total_stop += 1
#~ times_agree_stop+=[r_mine.t] # times_agree_stop+=[r_mine.t]
margin_he_wins_i_lost+=[r_theirs.ddc_min] margin_he_wins_i_lost += [r_theirs.ddc_min]
#~ margin_i_win_he_lose+=[r_theirs.dc] # margin_i_win_he_lose+=[r_theirs.dc]
#~ curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t, r_mine.dc_of_t, r_mine.ddc_of_t, H[:], h[:], p[:], N)] # curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t,
#~ print "margin when he wins: ", r_theirs.ddc_min # r_mine.dc_of_t, r_mine.ddc_of_t, H[:], h[:], p[:], N)]
# print("margin when he wins: ", r_theirs.ddc_min)
else: else:
total_not_stop+=1 total_not_stop += 1
print "% of stops", 100. * float(total_stop) / num_tested, total_stop print("% of stops", 100. * float(total_stop) / num_tested, total_stop)
print "% of total_disagree", 100. * float(total_disagree) / num_tested, total_disagree print("% of total_disagree", 100. * float(total_disagree) / num_tested, total_disagree)
if total_disagree > 0: if total_disagree > 0:
print "% of wins", 100. * float(mine_won) / total_disagree print("% of wins", 100. * float(mine_won) / total_disagree)
print "% of lose", 100. * float(mine_lose) / total_disagree print("% of lose", 100. * float(mine_lose) / total_disagree)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment