Commit 16eb515b authored by Guilhem Saurel's avatar Guilhem Saurel Committed by Fanny Risbourg
Browse files

Cleaning and refactoring

parent 87a4af49
Pipeline #13704 passed with stage
in 57 seconds
.vscode/*
build/*
*.pyc
*.aux
*.glo
......
include: https://rainboard.laas.fr/project/sl1m/.gitlab-ci.yml
......@@ -22,11 +22,15 @@ FINDPYTHON()
SET(${PROJECT_NAME}_SOURCES
__init__.py
constants_and_tools.py
solver.py
generic_solver.py
fix_sparsity.py
planner_l1.py
planner.py
problem_data.py
constraints.py
constraints_biped.py
planner_generic.py
planner_biped.py
problem_definition.py
qp.py
)
SET(${PROJECT_NAME}_PLANNER_SCENARIOS_SOURCES
......@@ -49,13 +53,15 @@ SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES
__init__.py
complex1.py
constraints.py
problem_definition_talos.py
lp_complex1_path.py
lp_complex1.py
lp_ramp_path.py
lp_rubbles_path.py
lp_slalom_debris_path.py
lp_slalom_debris.py
lp_stairs_path.py
stairs.py
maze.py
ramp_noGuide.py
rubbles.py
......@@ -65,6 +71,7 @@ SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES
SET(${PROJECT_NAME}_RBPRM_SOURCES
__init__.py
narrow_convex_hull.py
constants_and_tools.py
surfaces_from_planning.py
)
......@@ -82,6 +89,7 @@ SET(${PROJECT_NAME}_TOOLS_SOURCES
obj_to_constraints.py
plot_plytopes.py
plot_utils.py
plot_tools.py
polytope_conversion_utils.py
transformations.py
)
......
# sl1m
[![Pipeline status](https://gitlab.laas.fr/$ORG/sl1m/badges/master/pipeline.svg)](https://gitlab.laas.fr/loco-3d/sl1m/commits/master)
[![Coverage report](https://gitlab.laas.fr/loco-3d/sl1m/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/loco-3d/sl1m/master/coverage/)
Python dependencies:
scipy
quadprog
......
import numpy as np
from sl1m.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities, inequalities_to_Inequalities_object
from numpy import array, asmatrix, matrix, zeros, ones
from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
from numpy import array, zeros, ones, vstack, hstack, identity, cross, concatenate
from numpy.linalg import norm
import numpy as np
from scipy.spatial import ConvexHull
from .qp import solve_lp
#~ import eigenpy
#from curves import bezier3
from random import random as rd
from random import randint as rdi
from numpy import squeeze, asarray
from sl1m.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities, inequalities_to_Inequalities_object
# --------------------------------- CONSTANTS ---------------------------------------------------------------
EPSILON = 0.000001
EPSILON_EQ = 0.
Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
g = array([0.,0.,-9.81])
g6 = array([0.,0.,-9.81,0.,0.,0.])
IDENTITY = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
GRAVITY = array([0., 0., -9.81])
GRAVITY_6 = array([0., 0., -9.81, 0., 0., 0.])
mu = 0.45
X = array([1., 0., 0.])
Y = array([0., 1., 0.])
Z = array([0., 0., 1.])
x = array([1.,0.,0.])
z = array([0.,0.,1.])
zero3 = zeros(3)
# --------------------------------- METHODS ---------------------------------------------------------------
eps =0.000001
def normalize(Ab):
A = Ab[0]
b = Ab[1]
A_normalized = zeros(A.shape)
b_normalized = zeros(b.shape)
for i in range(A.shape[0]):
n = norm(A[i, :])
if n <= EPSILON:
n = 1.
A_normalized[i, :] = A[i, :] / n
b_normalized[i] = b[i] / n
return A_normalized, b_normalized
#### surface to inequalities ####
def convert_surface_to_inequality(s):
#TODO does normal orientation matter ?
#it will for collisions
n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0])
def convert_surface_to_inequality(s, eq_as_ineq):
# TODO does normal orientation matter ? It will for collisions
n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0])
if n[2] <= 0.:
for i in range(3):
n[i] = -n[i]
n /= norm(n)
return surfacePointsToIneq(s, n)
def replace_surfaces_with_ineq_in_phaseData(phase):
phase["S"] = [convert_surface_to_inequality(S) for S in phase["S"]]
def replace_surfaces_with_ineq_in_problem(pb):
[ replace_surfaces_with_ineq_in_phaseData(phase) for phase in pb ["phaseData"]]
norm_n = norm(n)
if norm_n > 1e-6:
n /= norm(n)
else:
# FIXME: to something better here
n = array([0, 0, 1])
return surface_points_to_inequalities(s, n, eq_as_ineq)
def normal_from_ineq(s_ineq):
n = s_ineq[0][-1]
if n[2] < 0:
n = -n
return n
def replace_surfaces_with_ineq_in_phaseData(phase, eq_as_ineq):
phase["S"] = [convert_surface_to_inequality(S, eq_as_ineq) for S in phase["S"]]
def replace_surfaces_with_ineq_in_problem(pb, eq_as_ineq=False):
[replace_surfaces_with_ineq_in_phaseData(phase, eq_as_ineq) for phase in pb["phaseData"]]
def ineqQHull(hull):
A = hull.equations[:,:-1]
b = -hull.equations[:,-1]
return A,b
def vectorProjection (v, n):
v = v / norm(v)
n = n / norm(n)
proj = v - np.dot(v,n)*n
return proj/norm(proj)
def addHeightConstraint(K,k, val):
K1 = vstack([K, -z])
k1 = concatenate([k, -ones(1) * val]).reshape((-1,))
return K1, k1
def default_transform_from_pos_normal_(transform, pos, normal):
#return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] ) # FIXME : temp stuff, only work on flat floor
# FIXME : there is something wrong the the code above
#~ print "pos ", pos
#~ print "normal ", normal
#align the x-axis of the foot to the root heading direction
f = x
xp = np.dot(transform, x).tolist()
t = vectorProjection(xp, normal)
v = np.cross(f, t)
c = np.dot(f, t)
if abs(c) > 0.99 :
return vstack( [hstack([transform,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] )
else:
u = v/norm(v)
h = (1. - c)/(1. - c**2)
A = hull.equations[:, :-1]
b = -hull.equations[:, -1]
return A, b
vx, vy, vz = v
rot1 =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
[h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
[h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
#align the z-axis of the foot to the surface normal
f = z
t = array(normal)
t = t / norm(t)
v = np.cross(f, t)
c = np.dot(f, t)
if abs(c) > 0.99 :
rot2 = identity(3)
else:
u = v/norm(v)
h = (1. - c)/(1. - c**2)
vx, vy, vz = v
rot2 =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
[h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
[h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
rot = np.dot(rot1,rot2)
return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] )
def default_transform_from_pos_normal(pos, normal):
f = array([0.,0.,1.])
def default_transform_from_pos_normal(pos, normal, transform=IDENTITY):
"""
transform matrix is a rotation matrix from the root trajectory
used to align the foot yaw (z-axis) orientation
surface normal is used to align the foot roll and pitch (x- and y- axes) orientation
"""
f = Z
t = array(normal)
t = t / norm(t)
v = np.cross(f, t)
c = np.dot(f, t)
if abs(c) > 0.99 :
rot = identity(3)
c = np.dot(f, t)
if abs(c) > 0.99:
rot = identity(3)
else:
u = v/norm(v)
h = (1. - c)/(1. - c**2)
s = (1. - c**2)
assert s > 0
s = np.sqrt(s)
ux, uy, uz = u
rot = array([[c + h*ux**2, h*ux*uy - uz*s, h*ux*uz + uy*s],
[h*ux*uy + uz*s, c + h*ux**2, h*uy*uz - ux*s],
[h*ux*uz - uy*s, h*uy*uz + ux*s, c + h*uz**2]])
vx, vy, vz = v
rot =array([[c + h*vx**2, h*vx*vy - vz, h*vx*vz + vy],
[h*vx*vy+vz, c+h*vy**2, h*vy*vz-vx],
[h*vx*vz - vy, h*vy*vz + vx, c+h*vz**2]])
return vstack( [hstack([rot,pos.reshape((-1,1))]), [ 0. , 0. , 0. , 1. ] ] )
rot = np.dot(transform, rot)
return vstack([hstack([rot, pos.reshape((-1, 1))]), [0., 0., 0., 1.]])
#last is equality
def surfacePointsToIneq(S, normal):
def surface_points_to_inequalities(S, normal, eq_as_ineq):
n = array(normal)
tr = default_transform_from_pos_normal(array([0.,0.,0.]),n)
trinv = tr.copy(); trinv[:3,:3] = tr[:3,:3].T;
trpts = [tr[:3,:3].dot(s)[:2] for s in S.T]
tr = default_transform_from_pos_normal(array([0., 0., 0.]), n)
trinv = tr.copy()
trinv[:3, :3] = tr[:3, :3].T
trpts = [tr[:3, :3].dot(s)[:2] for s in S.T]
hull = ConvexHull(array(trpts))
A,b = ineqQHull(hull)
A = hstack([A,zeros((A.shape[0],1))])
ine = inequalities_to_Inequalities_object(A,b)
A, b = ineqQHull(hull)
A = hstack([A, zeros((A.shape[0], 1))])
ine = inequalities_to_Inequalities_object(A, b)
ine = rotate_inequalities(ine, trinv)
#adding plane constraint
#plane equation given by first point and normal for instance
d = array([n.dot(S[:,0])])
A = vstack([ine.A, n , -n ])
b = concatenate([ine.b, d, -d]).reshape((-1,))
A = vstack([ine.A, n])
b = concatenate([ine.b, d]).reshape((-1,))
d = array([n.dot(S[:, 0])])
if eq_as_ineq:
A = vstack([ine.A, n, -n])
b = concatenate([ine.b, d + EPSILON_EQ, -d + EPSILON_EQ]).reshape((-1,))
else:
A = vstack([ine.A, n , -n ])
b = concatenate([ine.b, d, -d]).reshape((-1,))
A = vstack([ine.A, n])
b = concatenate([ine.b, d]).reshape((-1,))
return A, b
############ BENCHMARKING ###############
try:
from time import perf_counter as clock
except ImportError:
from time import clock
def timMs(t1, t2):
"""
Expresses the duration between t1 and t2 in milliseconds
"""
return (t2-t1) * 1000.
import numpy as np
# Implementation of the class constraints, which implements the constraints used by the generic planner
#
# The problem is a LP with these variables for each phase:
# [ com_x, com_y, com_z_1, com_z_2, p_i_x, p_i_y, p_i_z, {a_i} ]
# [ 1 , 1 , 1 , 1 , 3 , 0 if n_surfaces == 1, else n_surfaces]
#
# Under the following constraints :
# - fixed_foot_com: Ensures the COM is 'above' the fixed feet
# - foot_relative_distance: Ensures the moving feet is close enough to the other feet
# - surface: Each foot belongs to one surface
# - slack_positivity: The slack variables are positive
# - com_weighted_equality: Fix the horizontal position of the COm at the barycenter of the contact points (fixed feet)
class Constraints:
def __init__(self, n_effectors):
self.n_effectors = n_effectors
self.default_n_variables = 7
self.WEIGHTS = [1./float(n_effectors - 1) for _ in range(n_effectors)]
self.com_xy = self.__expression_matrix(2, 0)
self.com_1 = self.__expression_matrix(3, 0)
self.com_2 = self.__expression_matrix(3, 0)
self.com_2[2, 2] = 0
self.com_2[2, 3] = 1
self.com_1_z = self.__expression_matrix(1, 2)
self.com_2_z = self.__expression_matrix(1, 3)
self.foot = self.__expression_matrix(3, 4)
self.foot_xy = self.__expression_matrix(2, 4)
def __expression_matrix(self, size, j):
"""
Generate a selection matrix for a given variable
@param size number of rows of the variable
@param x position of the variable in the phase variables
@return a (size, number of default variables (without slacks)) matrix with identity at column j
"""
M = np.zeros((size, self.default_n_variables))
M[:, j:j+size] = np.identity(size)
return M
def __fixed_foot_com_2_kinematic(self, pb, phase, G, h, i_start, js, feet_phase):
"""
The COM 2 must belong to a reachable polytope above each end-effector of the phase
For each effector id , K_id (c2 - p_id) <= k_id
@param pb The problem specific data
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param js List of column corresponding to the start of each phase
@param feet_phase List of the feet las moving phase, -1 if they haven't moved
@return i_start + the number of rows used by the constraint
"""
i = i_start
j = js[-1]
for foot, (K, k) in enumerate(phase["K"]):
if foot == phase["Moving"]:
l = k.shape[0]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2 - self.foot)
h[i:i + l] = k
i += l
elif feet_phase[foot] != -1:
l = k.shape[0]
j_foot = js[feet_phase[foot]]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
h[i:i + l] = k
i += l
else:
l = k.shape[0]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
foot_pose = pb["p0"][foot]
h[i:i + l] = k + K.dot(foot_pose)
i += l
return i
def __fixed_foot_com_1_kinematic(self, pb, phase, G, h, i_start, js, feet_phase):
"""
The COM 1 must belong to a reachable polytope above each end-effector of the previous phase
For each effector id , K_id (c1- p_id(t-1)) <= k_id
This constraint should not be added at the first phase.
@param pb The problem specific data
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param js List of column corresponding to the start of each phase
@param feet_phase List of the feet las moving phase, -1 if they haven't moved
@return i_start + the number of rows used by the constraint
"""
i = i_start
j = js[-1]
for foot, (K, k) in enumerate(phase["K"]):
if foot == phase["Moving"]:
if feet_phase[foot] != -1:
l = k.shape[0]
j_foot = js[feet_phase[foot]]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_1)
G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
h[i:i + l] = k
i += l
else:
l = k.shape[0]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_1)
foot_pose = pb["p0"][foot]
h[i:i + l] = k + K.dot(foot_pose)
i += l
elif feet_phase[foot] != -1:
l = k.shape[0]
j_foot = js[feet_phase[foot]]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
G[i:i + l, j_foot:j_foot + self.default_n_variables] = -K.dot(self.foot)
h[i:i + l] = k
i += l
else:
l = k.shape[0]
G[i:i + l, j:j + self.default_n_variables] = K.dot(self.com_2)
foot_pose = pb["p0"][foot]
h[i:i + l] = k + K.dot(foot_pose)
i += l
return i
def fixed_foot_com(self, pb, phase, G, h, i_start, js, phase_id, feet_phase):
"""
The COM must belong to a reachable polytope above each end-effector
For each effector id , K_id (c(t) - p_id(t-1)) <= k_id
This constraint should not be added at the first phase.
@param pb The problem specific data
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param js List of column corresponding to the start of each phase
@param phase_id Phase number
@param feet_phase List of the feet las moving phase, -1 if they haven't moved
@return i_start + the number of rows used by the constraint
"""
i = i_start
if phase_id != 0:
i = self.__fixed_foot_com_1_kinematic(pb, phase, G, h, i_start, js, feet_phase)
return self.__fixed_foot_com_2_kinematic(pb, phase, G, h, i, js, feet_phase)
def foot_relative_distance(self, pb, phase, G, h, i_start, js, feet_phase):
"""
The distance between the moving effector and the other ones is limited
For i = moving_foot, For j !=i, Ki (pj - pi) <= ki
@param pb The problem specific data
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param js List of column corresponding to the start of each phase
@param feet_phase List of the feet las moving phase, -1 if they haven't moved
@return i_start + the number of rows used by the constraint
"""
i = i_start
j = js[-1]
constraints = phase["allRelativeK"][phase["Moving"]]
for (foot, (K, k)) in constraints:
l = k.shape[0]
G[i:i + l, j:j + self.default_n_variables] = -K.dot(self.foot)
if feet_phase[foot] != -1:
j_foot = js[feet_phase[foot]]
G[i:i + l, j_foot:j_foot + self.default_n_variables] = K.dot(self.foot)
h[i:i + l] = k
else:
foot_pose = pb["p0"][foot]
h[i:i + l] = k - K.dot(foot_pose)
i += l
return i
def slack_positivity(self, phase, G, h, i_start, j):
"""
The slack variables (alpha) should be positive
Sl for each surface s, -alpha_s <= 0
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param j Column corresponding to this phase variables
@return i_start + the number of rows used by the constraint
"""
i = i_start
n_surfaces = len(phase["S"])
if n_surfaces > 1:
for s in range(n_surfaces):
G[i+s, j + self.default_n_variables + s] = -1
i += n_surfaces
return i
def surface_inequality(self, phase, G, h, i_start, j):
"""
The moving foot must belong to one surface
For each surface l: Sl pi - alphal <= sl
@param phase The phase specific data
@param G The inequality constraint matrix
@param h The inequality constraint vector
@param i_start Initial row to use
@param j Column corresponding to this phase variables
@return i_start + the number of rows used by the constraint
"""
i = i_start
n_surfaces = len(phase["S"])
j_alpha = self.default_n_variables
for (S, s) in phase["S"]:
l = S.shape[0]
G[i:i + l, j:j + self.default_n_variables] = S.dot(self.foot)
h[i:i + l] = s
if n_surfaces > 1:
G[i:i + l, j + j_alpha] = -np.ones(l)
j_alpha += 1
i += l
return i
def __com_weighted_equality(self, pb, phase, C, d, i_start, js, feet_phase):
"""
The 2D position of the com should be the baricenter of the fixed feet locations
0 = sum(fixed_foot i) WEIGHT * p_i_x_y - c_x_y
@param pb The problem specific data
@param phase The phase specific data
@param C The equality constraint matrix
@param d The equality constraint vector
@param i_start Initial row to use
@param js List of column corresponding to the start of each phase
@param feet_phase List of the feet las moving phase, -1 if they haven't moved
@return i_start + the number of rows used by the constraint
"""
i = i_start
j = js[-1]
for foot in range(self.n_effectors):
if foot == phase["Moving"]:
continue
if feet_phase[foot] != -1:
j_foot = js[feet_phase[foot]]
C[i:i + 2, j_foot:j_foot + self.default_n_variables] = self.WEIGHTS[foot] * self.foot_xy
else:
foot_pose = pb["p0"][foot]
d[i:i + 2] -= self.WEIGHTS[foot] * foot_pose[:2]
C[i:i + 2, j:j + self.default_n_variables] = -self.com_xy
i += 2
return i
def __com_equality_init(self, pb, C, d, i_start):
"""
The initial com position is defined in the problem
@param pb The problem specific data
@param C The equality constraint matrix
@param d The equality constraint vector
@param i_start Initial row to use
@return i_start + the number of rows used by the constraint
"""
initial_position = pb["c0"]
i = i_start
C[i:i + 2, :self.default_n_variables] = self.com_xy
d[i:i + 2] = initial_position[:2]
i += 2
return i
def com(self, pb, phase, C, d, i_start, js, phase_id, feet_phase):
"""
The com position is defined
@param pb The problem specific data
@param phase The phase specific data
@param C The equality constraint matrix
@param d The equality constraint vector