diff --git a/.gitignore b/.gitignore index 2631868..d4bfb12 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ +.vscode/* +build/* *.pyc *.aux *.glo diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000..a8a0b79 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1 @@ +include: https://rainboard.laas.fr/project/sl1m/.gitlab-ci.yml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..f16ae48 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,8 @@ +repos: +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: check-json + - id: check-symlinks + - id: check-toml + - id: check-yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 269185f..dd707c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,67 +22,72 @@ 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 +SET(${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_SOURCES __init__.py - complex1.py complex.py - escaliers.py - flat_ground.py - lp_complex1.py + ) +SET(${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_PATH_SOURCES lp_complex_path.py - lp_complex.py - lp_stair_bauzil_10.py - lp_stair_bauzil_hrp2_path_10.py lp_stair_bauzil_hrp2_path.py - lp_stair_bauzil.py - stairs_10cm.py + lp_stair_bauzil_hrp2_path_10.py ) SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES __init__.py - - complex1.py - constraints.py - lp_complex1_path.py - lp_complex1.py + complex.py + stairs.py + rubbles.py + ) +SET(${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_PATH_SOURCES + lp_complex_path.py lp_ramp_path.py lp_rubbles_path.py - lp_slalom_debris_path.py - lp_slalom_debris.py - maze.py - ramp_noGuide.py - rubbles.py - rubble_stairs.py - slalom_debris.py + lp_stairs_path.py ) SET(${PROJECT_NAME}_RBPRM_SOURCES __init__.py + narrow_convex_hull.py constants_and_tools.py surfaces_from_planning.py ) SET(${PROJECT_NAME}_STANDALONE_SCENARIOS_SOURCES __init__.py - complex.py - constraints.py - escaliers.py + hrp2_complex.py + hrp2_stairs.py + talos_ramp.py + talos_rubble_stairs.py + problem_definition_talos.py + problem_definition_hrp2.py ) + SET(${PROJECT_NAME}_STANDALONE_SURFACES_SOURCES + __init__.py + stair_surfaces.py + complex_surfaces.py + flat_ground.py + ramp_surfaces.py + inclined_surfaces.py + ) SET(${PROJECT_NAME}_TOOLS_SOURCES __init__.py geom_utils.py obj_to_constraints.py - plot_plytopes.py plot_utils.py - polytope_conversion_utils.py + plot_tools.py + heightmap_tools.py transformations.py ) @@ -97,11 +102,26 @@ ENDFOREACH(src ${PROJECT_NAME}_PLANNER_SCENARIOS_SOURCES) FOREACH(src ${${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES}) PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/planner_scenarios/talos ${src}) ENDFOREACH(src ${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_SOURCES) +FOREACH(src ${${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_PATH_SOURCES}) + PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/planner_scenarios/talos/path_files ${src}) +ENDFOREACH(src ${PROJECT_NAME}_PLANNER_SCENARIOS_TALOS_PATH_SOURCES) + +FOREACH(src ${${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_SOURCES}) + PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/planner_scenarios/hrp2 ${src}) +ENDFOREACH(src ${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_SOURCES) + +FOREACH(src ${${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_PATH_SOURCES}) + PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/planner_scenarios/hrp2/path_files/ ${src}) +ENDFOREACH(src ${PROJECT_NAME}_PLANNER_SCENARIOS_HRP2_PATH_SOURCES) FOREACH(src ${${PROJECT_NAME}_RBPRM_SOURCES}) PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/rbprm ${src}) ENDFOREACH(src ${PROJECT_NAME}_RBPRM_SOURCES) +FOREACH(src ${${PROJECT_NAME}_STANDALONE_SURFACES_SOURCES}) + PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/stand_alone_scenarios/surfaces ${src}) +ENDFOREACH(src ${PROJECT_NAME}_STANDALONE_SURFACES_SOURCES) + FOREACH(src ${${PROJECT_NAME}_STANDALONE_SCENARIOS_SOURCES}) PYTHON_INSTALL_ON_SITE(${PROJECT_NAME}/stand_alone_scenarios ${src}) ENDFOREACH(src ${PROJECT_NAME}_STANDALONE_SCENARIOS_SOURCES) diff --git a/README.md b/README.md index 97afd7d..e8ba808 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,71 @@ -Python dependencies: - scipy - quadprog - matplotlib - pycddlib - cvxpy +# 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/) + +## Install + +> 📝 Upon installing via the `robotpkg` *ppa* please ensure the correct version of + python is noted, e.g. `sudo apt install robotpkg-py38-quadprog` means you + install the `quadprog` software with `Python3.8` as it's dependency. + +### Python dependencies: + + + python -m pip install scipy + sudo apt install robotpkg-py38-quadprog + python -m pip install matplotlib + python -m pip install pycddlib + python -m pip install cvxpy :warning: this package requires gurobi :warning: A free academic license can be obtained at https://www.gurobi.com + +### Demo dependencies: + +These are the dependencies for the robot and rbprm server. +``` +sudo apt install -y robotpkg-py38-hpp-rbprm-corba +``` + +### Installation troubleshooting: + +- If you realize you a python package such as `rbprm`, `anymal_rbprm`, + `solo_rbprm`, or `talos_rbprm` Please so the following: + + ``` + sudo apt install -y robotpkg-py38-hpp-rbprm-corba \ + robotpkg-py38-hpp-rbprm \ + robotpkg-py38-anymal-rbprm \ + robotpkg-py38-solo-rbprm \ + robotpkg-py38-talos-rbprm + ``` + +## Run demo: + +``` +# Source you install prefix. +source ~/devel/workspace_sl1m/install/setup.bash +# Locate where the Solo12 specific data are. +export SOLO3D_ENV_DIR=/home/mnaveau/Documents/cloud_laas/Shared/Solo3D/ +# Point toward you HPP installation folder +export INSTALL_HPP_DIR=/opt/openrobots/share +# Add the data folder to the ROS environment for urdf parsing. +export ROS_PACKAGE_PATH=/opt/openrobots/share/example-robot-data/robots:${SOLO3D_ENV_DIR}:$ROS_PACKAGE_PATH +``` + +Execute the scripts: + +```bash +python sl1m/stand_alone_scenarios/anymal_stairs.py +python sl1m/stand_alone_scenarios/anymal_trot.py +python sl1m/stand_alone_scenarios/data.pickle +python sl1m/stand_alone_scenarios/hrp2_complex.py +python sl1m/stand_alone_scenarios/hrp2_stairs.py +python sl1m/stand_alone_scenarios/problem_definition_hrp2.py +python sl1m/stand_alone_scenarios/problem_definition_talos.py +python sl1m/stand_alone_scenarios/solo_stairs.py +python sl1m/stand_alone_scenarios/solo_trot.py +python sl1m/stand_alone_scenarios/talos_ramp.py +python sl1m/stand_alone_scenarios/talos_rubble_stairs.py +``` diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..c43b494 --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + sl1m + 0.1.1 + Implementation of the sl1m solver for multi contact planning + + Guilhem Saurel + BSD + + Steve Tonneau + + + catkin + cmake + python + python3 + python-numpy + python3-numpy + python-matplotlib + python3-matplotlib + python3-cvxpy-pip + python3-glpk-pip + python3-gurobipy-pip + + cmake + + diff --git a/sl1m/constants_and_tools.py b/sl1m/constants_and_tools.py index b24db12..bfaabc1 100644 --- a/sl1m/constants_and_tools.py +++ b/sl1m/constants_and_tools.py @@ -1,157 +1,162 @@ 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.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, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) +GRAVITY = array([0.0, 0.0, -9.81]) +GRAVITY_6 = array([0.0, 0.0, -9.81, 0.0, 0.0, 0.0]) -mu = 0.45 +X = array([1.0, 0.0, 0.0]) +Y = array([0.0, 1.0, 0.0]) +Z = array([0.0, 0.0, 1.0]) -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.0 + 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]) - if n[2] <= 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.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) - - 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.]) + A = hull.equations[:, :-1] + b = -hull.equations[:, -1] + return A, b + + +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) - - 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. ] ] ) - - -#last is equality -def surfacePointsToIneq(S, normal): + u = v / norm(v) + h = (1.0 - c) / (1.0 - c**2) + s = 1.0 - 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], + ] + ) + + rot = np.dot(transform, rot) + return vstack([hstack([rot, pos.reshape((-1, 1))]), [0.0, 0.0, 0.0, 1.0]]) + + +def surface_points_to_inequalities(S, normal, eq_as_ineq): + # Homogenous tranformation matrix from world frame to surface frame. + # Normal vector corresponds to z-axis. + # 1st vertex is the Origin. 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(S.T[0], n) + + # Projection of vertices on the surface local frame --> z = 0. (2D) + # P_world = R_ @ P_local + T_ + # --> P_local = R_-1 @ (P_world - T_) + trpts = [np.dot(tr[:3, :3].T , (pos - tr[:-1,-1]))[:2] for pos 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) - 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,)) + A, b = ineqQHull(hull) + # Increase inequality matrix with z = 0 column + A = hstack([A, zeros((A.shape[0], 1))]) + # In local frame : A_l @ P_local <= b_l + # --> A_l @ R-1 @ P_world <= b_l + A_l @ R_-1 @ T_ + A_world = np.dot(A,tr[:3,:3].T) + b_world = b + np.dot(np.dot(A,tr[:3,:3].T), tr[:-1,-1]) + ine = inequalities_to_Inequalities_object(A_world, b_world) + + 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): - return (t2-t1) * 1000. + """ + Expresses the duration between t1 and t2 in milliseconds + """ + return (t2 - t1) * 1000.0 diff --git a/sl1m/constraints.py b/sl1m/constraints.py new file mode 100644 index 0000000..1c7f9d0 --- /dev/null +++ b/sl1m/constraints.py @@ -0,0 +1,426 @@ +import numpy as np + + +class Constraints: + """ + 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_ij} ] + [ 1 , 1 , 1 , 1 , 3 * phase_n_moving , (0 if n_surfaces == 1, else n_surfaces) * phase_n_moving] + + Under the following constraints : + - fixed_foot_com: Ensures the COM is 'above' the fixed feet + - foot_relative_distance: Ensures the moving feet are 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) + """ + + def __init__(self, n_effectors, com=True): + self.n_effectors = n_effectors + + self.default_n_variables = 4 * int(com) + + self.M = 50.0 + + def _default_n_variables(self, phase): + """ + @param phase phase concerned + @return the number of non slack variables in phase + """ + return self.default_n_variables + 3 * len(phase.moving) + + def _expression_matrix(self, size, _default_n_variables, j): + """ + Generate a selection matrix for a given variable + @param size number of rows of the variable + @param _default_n_variables number of non slack variables in the phase + @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, _default_n_variables)) + M[:, j : j + size] = np.identity(size) + return M + + def com_xy(self, phase): + """ + Generate a selection matrix for the com x and y components + @param phase phase data + @return a (2, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(2, self._default_n_variables(phase), 0) + + def com_1(self, phase): + """ + Generate a selection matrix for the com_1 + @param phase phase data + @return a (3, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(3, self._default_n_variables(phase), 0) + + def com_2(self, phase): + """ + Generate a selection matrix for the com_2 + @param phase phase data + @return a (3, phase number of variables (without slacks)) matrix + """ + M = self._expression_matrix(3, self._default_n_variables(phase), 0) + M[2, 2] = 0 + M[2, 3] = 1 + return M + + def com_1_z(self, phase): + """ + Generate a selection matrix for the com_1 z component + @param phase phase data + @return a (1, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(1, self._default_n_variables(phase), 2) + + def com_2_z(self, phase): + """ + Generate a selection matrix for the com_2 z component + @param phase phase data + @return a (1, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(3, self._default_n_variables(phase), 3) + + def foot(self, phase, foot=None, id=None): + """ + Generate a selection matrix for a given foot + @param phase phase data + @param foot foot to select + @param id id of the foot in the moving feet list + @return a (3, number of variables (without slacks)) selection matrix + """ + if foot is not None: + id = np.argmax(phase.moving == foot) + elif id is None: + print("Error in foot selection matrix: you must specify either foot or id") + j = self.default_n_variables + 3 * id + return self._expression_matrix(3, self._default_n_variables(phase), j) + + def foot_xy(self, phase, foot=None, id=None): + """ + Generate a selection matrix for a given foot x and y coordinates + @param phase phase data + @param foot foot to select + @param id id of the foot in the moving feet list + @return a (3, number of variables (without slacks)) selection matrix + """ + if foot is not None: + id = np.argmax(phase.moving == foot) + elif id is None: + print("Error in foot selection matrix: you must specify either foot or id") + j = self.default_n_variables + 3 * id + return self._expression_matrix(2, self._default_n_variables(phase), j) + + 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 stance foot 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 in phase.stance: + l = k.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = K.dot( + self.com_2(phase) + ) + h[i : i + l] = k + if foot in phase.moving: + G[i : i + l, j : j + self._default_n_variables(phase)] -= K.dot( + self.foot(phase, foot) + ) + elif feet_phase[foot] != -1: + j_f = js[feet_phase[foot]] + phase_f = pb.phaseData[feet_phase[foot]] + G[ + i : i + l, + j_f : j_f + self._default_n_variables(phase_f), + ] = -K.dot(self.foot(phase_f, foot)) + else: + foot_pose = pb.p0[foot] + h[i : i + l] += 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 stance foot of the previous phase + For each effector id in stance phase, 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 phase.id == 0 and pb.p0[foot] is not None: + l = k.shape[0] + foot_pose = pb.p0[foot] + G[i : i + l, j : j + self._default_n_variables(phase)] = K.dot( + self.com_1(phase) + ) + h[i : i + l] = k + K.dot(foot_pose) + i += l + elif foot in pb.phaseData[phase.id - 1].stance: + l = k.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = K.dot( + self.com_1(phase) + ) + h[i : i + l] = k + if feet_phase[foot] != -1: + j_f = js[feet_phase[foot]] + phase_f = pb.phaseData[feet_phase[foot]] + G[ + i : i + l, + j_f : j_f + self._default_n_variables(phase_f), + ] -= K.dot(self.foot(phase_f, foot)) + else: + foot_pose = pb.p0[foot] + h[i : i + l] += K.dot(foot_pose) + i += l + return i + + def fixed_foot_com(self, pb, phase, G, h, i_start, js, 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 feet_phase List of the feet last 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, 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 stance feet is limited + For i in moving_foot, For j !=i, Ki (pj - pi) <= ki + The distane between the moving effector and the previous phase stance feet (or initial + contacts for the fist phase) is also limited + @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 in phase.moving: + constraints = phase.allRelativeK[foot] + for (other, (K, k)) in constraints: + if other in phase.stance: + l = k.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = -K.dot( + self.foot(phase, foot) + ) + h[i : i + l] = k + if other in phase.moving: + G[i : i + l, j : j + self._default_n_variables(phase)] += K.dot( + self.foot(phase, other) + ) + elif feet_phase[other] != -1: + j_f = js[feet_phase[other]] + phase_f = pb.phaseData[feet_phase[other]] + G[ + i : i + l, + j_f : j_f + self._default_n_variables(phase_f), + ] = K.dot(self.foot(phase_f, other)) + else: + foot_pose = pb.p0[other] + h[i : i + l] -= K.dot(foot_pose) + i += l + elif phase.id == 0: + l = k.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = -K.dot( + self.foot(phase, foot) + ) + h[i : i + l] = k + h[i : i + l] -= K.dot(pb.p0[other]) + i += l + elif other in pb.phaseData[phase.id - 1].stance: + l = k.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = -K.dot( + self.foot(phase, foot) + ) + h[i : i + l] = k + if feet_phase[other] != -1: + j_f = js[feet_phase[other]] + phase_f = pb.phaseData[feet_phase[other]] + G[ + i : i + l, + j_f : j_f + self._default_n_variables(phase_f), + ] = K.dot(self.foot(phase_f, other)) + else: + foot_pose = pb.p0[other] + h[i : i + l] -= 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 + j_alpha = j + self._default_n_variables(phase) + for n_surface in phase.n_surfaces: + if n_surface > 1: + G[i : i + n_surface, j_alpha : j_alpha + n_surface] = -np.identity( + n_surface + ) + j_alpha += n_surface + i += n_surface + return i + + def surface_inequality(self, phase, G, h, i_start, j): + """ + Each 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 + j_alpha = self._default_n_variables(phase) + for id, surfaces in enumerate(phase.S): + for S, s in surfaces: + l = S.shape[0] + G[i : i + l, j : j + self._default_n_variables(phase)] = S.dot( + self.foot(phase, id=id) + ) + h[i : i + l] = s + if phase.n_surfaces[id] > 1: + G[i : i + l, j + j_alpha] = -self.M * np.ones(l) + j_alpha += 1 + i += l + return i + + def slack_equality(self, phase, C, d, i_start, j): + """ + The slack variables (alpha) sum should be equal to the number of surfaces -1 + Sl for each moving foot, sum(alpha_s) = n_surfaces - 1 + @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 j Column corresponding to this phase variables + @return i_start + the number of rows used by the constraint + """ + i = i_start + j_alpha = j + self._default_n_variables(phase) + for n_surface in phase.n_surfaces: + if n_surface > 1: + C[i, j_alpha : j_alpha + n_surface] = np.ones(n_surface) + d[i] = n_surface - 1 + j_alpha += n_surface + i += 1 + 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] + + weight = 1.0 / len(phase.stance) + + for foot in range(self.n_effectors): + if foot in phase.stance: + if feet_phase[foot] != -1: + j_f = js[feet_phase[foot]] + phase_f = pb.phaseData[feet_phase[foot]] + C[ + i : i + 2, + j_f : j_f + self._default_n_variables(phase_f), + ] = weight * self.foot_xy(phase_f, foot) + elif pb.p0[foot] is not None: + foot_pose = pb.p0[foot] + d[i : i + 2] -= weight * foot_pose[:2] + C[i : i + 2, j : j + self._default_n_variables(phase)] = -self.com_xy(phase) + i += 2 + return i + + def _com_equality_init(self, pb, phase, C, d, i_start): + """ + The initial com position is defined in the problem + @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 + @return i_start + the number of rows used by the constraint + """ + i = i_start + C[i : i + 2, : self._default_n_variables(phase)] = self.com_xy(phase) + d[i : i + 2] = pb.c0[:2] + i += 2 + return i + + def com(self, pb, phase, C, d, i_start, js, feet_phase): + """ + The com position is defined by the initial data or as the barycenter of fixed feet positions + @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 + """ + if phase.id == 0 and pb.c0 is not None: + return self._com_equality_init(pb, phase, C, d, i_start) + elif len(phase.stance) > 0: + return self._com_weighted_equality(pb, phase, C, d, i_start, js, feet_phase) + else: + return i_start diff --git a/sl1m/constraints_biped.py b/sl1m/constraints_biped.py new file mode 100644 index 0000000..8bb7399 --- /dev/null +++ b/sl1m/constraints_biped.py @@ -0,0 +1,257 @@ +import numpy as np + + +class BipedConstraints: + """ + Implementation of the class constraints, which implements the constraints used by the generic planner + + The problem is a has these variables for each phase: + [ p_i_x, p_i_y, p_i_z, com_z, {alpha_0_i, alpha_1_i} ] + [ 1 , 1 , 1 , 1 , 0 if n_surfaces == 1, else 2 * n_surfaces] + + alpha_0_i, slack variable for inequality, alpha_1_i slack for equality constraint + Extra constraints are alpha_1_i - alpha_0_i <= 0 ; -alpha_1_i - alpha_0_i <= 0; alpha_0_i >= 0 is implied by the first 2 + + 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: Inequality constraints on the slack variables + """ + + def __init__(self): + self.default_n_variables = 4 + self.slack_scale = 10.0 + + self.n_slack_per_surface = 2 + self.n_ineq_per_surface = 2 + + self.foot = self._expression_matrix(3, 0) + self.foot_xy = self.foot[:2, :] + self.foot_z = self.foot[2:, :] + self.com_z = self._expression_matrix(1, 3) + self.com = self._expression_matrix(3, 0) + self.com[2, :] = self.com_z + + def _fixed(self, moving_foot): + """ + Get the index of the fixed foot given the moving foot index + @param moving_foot moving foot index + @return fixed foot index + """ + return (moving_foot + 1) % 2 + + 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(self, pb, id, phase, A, b, j_previous, j, i): + """ + The COM must be in the reachable polytope of the fixed foot (whose position is stored in "p0") + K_fixed (cz - p_fixed_z) <= k_fixed + :param: id Phase 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 j_startumn corresponding to the start of each phase + return i_start + the number of rows used by the constraint + """ + fixed_foot = self._fixed(phase.moving) + K, k = phase.K[fixed_foot] + l = K.shape[0] + + if id == 0: + if pb.p0 is not None: + fixed_foot_z = pb.p0[fixed_foot][-1:] + A[i : i + l, j : j + self.default_n_variables] = K[:, -1:].dot( + self.com_z + ) + b[i : i + l] = k + K[:, -1:].dot(fixed_foot_z) + else: + A[i : i + l, j : j + self.default_n_variables] = K[:, -1:].dot(self.com_z) + A[i : i + l, j_previous : j_previous + self.default_n_variables] = -K[ + :, -1: + ].dot(self.foot_z) + b[i : i + l] = k + + return i + l + + def moving_foot_com(self, pb, id, phase, A, b, j_previous, j, i): + """ + The COM must be in the reachable polytope of the moving foot (whose position is stored in "p0") + K_moving_z * cz - K_moving * (p_moving - p_fixed) <= k_moving + This contraint is only valid at the first phase + :param: id Phase 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 j_startumn corresponding to the start of each phase + return i_start + the number of rows used by the constraint + """ + moving_foot = phase.moving + fixed_foot = self._fixed(moving_foot) + K, k = phase.K[moving_foot] + l = K.shape[0] + + if id == 0: + if pb.p0 is not None: + com_xy = pb.p0[fixed_foot][:2] + A[i : i + l, j : j + self.default_n_variables] = K[:, -1:].dot( + self.com_z + ) - K.dot(self.foot) + b[i : i + l] = k - K[:, :2].dot(com_xy[:2]) + else: + A[i : i + l, j : j + self.default_n_variables] = K[:, -1:].dot( + self.com_z + ) - K.dot(self.foot) + A[i : i + l, j_previous : j_previous + self.default_n_variables] = K[ + :, :2 + ].dot(self.foot_xy) + b[i : i + l] = k + + return i + l + + def feet_relative_distance(self, pb, id, phase, A, b, j_previous, j, i): + """ + The distance between the fixed and moving effectors is limited + K_moving (pj - pi) <= k_moving + This contraint is only valid at the first phase + :param: id Phase 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 j_startumn corresponding to the start of each phase + return i_start + the number of rows used by the constraint + """ + moving_foot = phase.moving + fixed_foot, (K, k) = phase.allRelativeK[moving_foot][0] + l = K.shape[0] + + if id == 0: + if pb.p0 is not None: + fixed_foot_position = pb.p0[fixed_foot] + A[i : i + l, j : j + self.default_n_variables] = K.dot(self.foot) + b[i : i + l] = k + K.dot(fixed_foot_position) + return i + l + else: + A[i : i + l, j : j + self.default_n_variables] = K.dot(self.foot) + A[i : i + l, j_previous : j_previous + self.default_n_variables] = -K.dot( + self.foot + ) + b[i : i + l] = k + + return i + l + + def surface_inequality(self, phase, A, b, j, i_start): + """ + The moving foot must belong to one surface + For each surface l: Sl pi - alphal <= sl + :param: phase The phase specific data + :param: C The equality constraint matrix + :param: d The equality constraint vector + :param: i Initial row to use + :param: j Column corresponding to this phase variables + return i + the number of rows used by the constraint + """ + n_surfaces = len(phase.S) + j_slack = self.default_n_variables + i = i_start + for S, s in phase.S: + l = S.shape[0] - 1 + A[i : i + l, j : j + self.default_n_variables] = S[:-1, :].dot(self.foot) + b[i : i + l] = s[:-1] + if n_surfaces > 1: + A[i : i + l, j + j_slack] = -np.ones(l) * self.slack_scale + j_slack += self.n_slack_per_surface + i += l + return i + + def surface_equality(self, phase, E, e, j, i_start): + """ + The moving foot must belong to one surface + For each surface l: di * p_i = e_i + beta_i + :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: j Column corresponding to this phase variables + return i_start + the number of rows used by the constraint + """ + n_surfaces = len(phase.S) + i = i_start + if n_surfaces == 1: + E[i, j : j + self.default_n_variables] = phase.S[0][0][-1].dot(self.foot) + e[i] = phase.S[0][1][-1] + return i + 1 + else: + j_slack = self.default_n_variables + 1 + for S, s in phase.S: + E[i, j : j + self.default_n_variables] = S[-1, :].dot(self.foot) + E[i, j + j_slack] = -1 * self.slack_scale + e[i] = s[-1] + j_slack += self.n_slack_per_surface + i += 1 + return i + + def slack_equality(self, phase, C, d, i_start, j): + """ + The slack variables (alpha) sum should be equal to the number of surfaces -1 + Sl for each moving foot, sum(alpha_s) = n_surfaces - 1 + @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 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: + j_slack = self.default_n_variables + for _ in range(0, n_surfaces): + C[i, j + j_slack] = 1 + j_slack += self.n_slack_per_surface + d[i] = n_surfaces - 1 + i += 1 + return i + + def slack_positivity(self, phase, A, b, j_start, i_start): + """ + The slack variables (alpha) should be positive + for each surface s: + -alpha_s + beta_s <= 0 + -alpha_s - beta_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 + """ + n_surfaces = len(phase.S) + i = i_start + j = j_start + if n_surfaces > 1: + j += self.default_n_variables + for j_slack in range( + 0, + n_surfaces * self.n_slack_per_surface, + self.n_slack_per_surface, + ): + A[i, j + j_slack : j + j_slack + 2] = [-1, 1] + A[i + 1, j + j_slack : j + j_slack + 2] = [-1, -1] + i += self.n_ineq_per_surface + return i diff --git a/sl1m/fix_sparsity.py b/sl1m/fix_sparsity.py index d3f5e47..1f42a2e 100644 --- a/sl1m/fix_sparsity.py +++ b/sl1m/fix_sparsity.py @@ -1,165 +1,360 @@ import numpy as np +import itertools +import copy +from sl1m.solver import call_QP_solver, call_LP_solver -from sl1m.constants_and_tools import * -from sl1m import planner_l1 as pl1 -from sl1m import planner as pl - -from . import qp - - -# try to import mixed integer solver -MIP_OK = False -try: - import gurobipy - import cvxpy as cp - MIP_OK = True - -except ImportError: - pass - - - -np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) - - - -### This solver is called when the sparsity is fixed. It assumes the first contact surface for each phase -### is the one used for contact creation. -def solve(pb,surfaces, draw_scene = None, plot = True ): - - t1 = clock() - A, b, E, e = pl.convertProblemToLp(pb) - C = identity(A.shape[1]) - c = zeros(A.shape[1]) - t2 = clock() - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - t3 = clock() - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = pl.retrieve_points_from_res(pb, res) - - plot = plot and draw_scene is not None - if plot: - ax = draw_scene(surfaces) - pl.plotQPRes(pb, res, ax=ax) - - return pb, coms, footpos, allfeetpos, res - - -### Calls the sl1m solver. Brute-forcedly tries to solve non fixed sparsity by handling the combinatorial. -### Ultimately calls solve which provides the approriate cost function -def solveL1(pb, surfaces, draw_scene = None, plot = True): - A, b, E, e = pl1.convertProblemToLp(pb) - C = identity(A.shape[1]) * 0.00001 - c = pl1.slackSelectionMatrix(pb) - - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - - ok = pl1.isSparsityFixed(pb, res) - solutionIndices = None - solutionComb = None - if not ok: - pbs = pl1.generateAllFixedScenariosWithFixedSparsity(pb, res) - - t3 = clock() - - for (pbComb, comb, indices) in pbs: - A, b, E, e = pl1.convertProblemToLp(pbComb, convertSurfaces = False) - C = identity(A.shape[1]) * 0.00001 - c = pl1.slackSelectionMatrix(pbComb) - try: - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - if pl1.isSparsityFixed(pbComb, res): - coms, footpos, allfeetpos = pl1.retrieve_points_from_res(pbComb, res) - pb = pbComb - ok = True - solutionIndices = indices[:] - solutionComb = comb - if plot: - ax = draw_scene(surfaces) - pl1.plotQPRes(pb, res, ax=ax) - break - except: - print("unfeasible problem") - pass - - t4 = clock() - - print("time to solve combinatorial ", timMs(t3,t4)) - - if ok: - surfacesret, indices = pl1.bestSelectedSurfaces(pb, res) - for i, phase in enumerate(pb["phaseData"]): - phase["S"] = [surfaces[i][indices[i]]] - if solutionIndices is not None: - for i, idx in enumerate(solutionIndices): - pb["phaseData"][idx]["S"] = [surfaces[idx][solutionComb[i]]] - - return solve(pb,surfaces, draw_scene = draw_scene, plot = True ) - - -############### MIXED-INTEGER SOLVER ############### - -def tovals(variables): - return array([el.value for el in variables]) - -def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True): - if not MIP_OK: - print("Mixed integer formulation requires gurobi packaged in cvxpy") - raise ImportError - - gurobipy.setParam('LogFile', '') - gurobipy.setParam('OutputFlag', 0) - - A, b, E, e = pl1.convertProblemToLp(pb) - slackMatrix = pl1.slackSelectionMatrix(pb) - - rdim = A.shape[1] - varReal = cp.Variable(rdim) - constraints = [] - constraintNormalIneq = A * varReal <= b - constraintNormalEq = E * varReal == e - - constraints = [constraintNormalIneq, constraintNormalEq] - #creating boolean vars - - slackIndices = [i for i,el in enumerate (slackMatrix) if el > 0] - numSlackVariables = len([el for el in slackMatrix if el > 0]) - boolvars = cp.Variable(numSlackVariables, boolean=True) - obj = cp.Minimize(slackMatrix * varReal) - - if MIP: - constraints = constraints + [varReal[el] <= 100. * boolvars[i] for i, el in enumerate(slackIndices)] - - currentSum = [] - previousL = 0 - for i, el in enumerate(slackIndices): - if i!= 0 and el - previousL > 2.: - assert len(currentSum) > 0 - constraints = constraints + [sum(currentSum) == len(currentSum) -1 ] - currentSum = [boolvars[i]] - elif el !=0: - currentSum = currentSum + [boolvars[i]] - previousL = el - if len(currentSum) > 1: - constraints = constraints + [sum(currentSum) == len(currentSum) -1 ] - obj = cp.Minimize(ones(numSlackVariables) * boolvars) - prob = cp.Problem(obj, constraints) - t1 = clock() - res = prob.solve(solver=cp.GUROBI, verbose=False ) - t2 = clock() - res = tovals(varReal) - print("time to solve MIP ", timMs(t1,t2)) - - - plot = plot and draw_scene is not None - if plot: - ax = draw_scene(surfaces) - pl1.plotQPRes(pb, res, ax=ax) - - return timMs(t1,t2) - +from sl1m.problem_data import ProblemData + +ALPHA_THRESHOLD = 0.00001 +MAX_COMB_GENERATED = 100 + + +def optimize_sparse_L1(planner, pb, costs, QP_SOLVER, LP_SOLVER): + """ + This solver is called when the sparsity is fixed. + It assumes the only contact surface for each phase is the one used for contact creation. + Solve the problem with a specific solver + @param P, q, G, h, C, d problem datalse + @return None if wrong SOLVER, else ResultData + """ + G, h, C, d = planner.convert_pb_to_LP(pb, False) + P, q = planner.compute_costs(costs) + if costs != {}: + result = call_QP_solver(P, q, G, h, C, d, QP_SOLVER) + else: + result = call_LP_solver(q, G, h, C, d, LP_SOLVER) + + if result.success: + coms, moving_foot_pos, all_feet_pos = planner.get_result(result.x) + return ProblemData(True, result.time, coms, moving_foot_pos, all_feet_pos) + else: + print("optimize_sparse_L1 failed to solve the QP") + return ProblemData(False, result.time) + + +def fix_sparsity_combinatorial_gait(planner, pb, LP_SOLVER): + """ + Calls the sl1m solver. Tries to solve non fixed sparsity by handling the combinatorial. + Ultimately calls solve which provides the appropriate cost function + @param planner + @param pb problem data + @param surfaces potential surfaces + @param SOLVER Solver choice + @return true if the problem was solved, fixed surfaces problem, surface_indices and time + """ + G, h, C, d = planner.convert_pb_to_LP(pb) + q = 100.0 * planner.alphas + + result = call_LP_solver(q, G, h, C, d, LP_SOLVER) + t = result.time + if not result.success: + print("Initial LP solver fails in fix_sparsity_combinatorial") + return False, pb, [], t + + alphas = planner.get_alphas(result.x) + print( + " Nb surfaces per phase : ", + get_number_surfaces_per_phase_gait(alphas), + ) + + if is_sparsity_fixed_gait(pb, alphas): + print(" -> Sparsity fixed directly") + selected_surfaces = planner.selected_surfaces(alphas) + for i, phase in enumerate(pb.phaseData): + for j in range(len(phase.n_surfaces)): + phase.S[j] = [phase.S[j][selected_surfaces[i][j]]] + phase.n_surfaces[j] = 1 + return True, pb, selected_surfaces, t + + undecided_surfaces, decided_surfaces = get_undecided_surfaces_gait(pb, alphas) + pbs = generate_fixed_sparsity_problems_gait(pb, alphas) + if pbs is None: + print("No combinatorial problems was found") + return False, pb, [], t + + # Handle the combinatorial + sparsity_fixed = False + i = 0 + for (fixed_pb, combination) in pbs: + G, h, C, d = planner.convert_pb_to_LP(fixed_pb, False) + q = 100.0 * planner.alphas + result = call_LP_solver(q, G, h, C, d, LP_SOLVER) + t += result.time + if result.success: + alphas = planner.get_alphas(result.x) + if is_sparsity_fixed_gait(fixed_pb, alphas): + print( + " -> Sparsity fixed in comb at try ", + i + 1, + " / ", + len(pbs), + ) + sparsity_fixed = True + pb = fixed_pb + break + i += 1 + + if not sparsity_fixed: + print(" -> Sparsity could not be fixed") + return False, pb, [], t + + # Get ID of selected surfaces + k = 0 + surface_indices = [] + # Fill empty surface_indices + for i, phase in enumerate(pb.phaseData): + len_effectors = len(phase.n_surfaces) + surface_indices.append([0] * len_effectors) + # Fill it with decided surfaces + for i_phase, i_effector, index_surface_selected in decided_surfaces: + # Update surface_indices + surface_indices[i_phase][i_effector] = index_surface_selected + # Fill it with succeeding combinatorial + for i, undecided_surface in enumerate(undecided_surfaces): + i_phase = undecided_surface[0] + i_effector = undecided_surface[1] + index_surface_selected = combination[i] + # Update surface_indices + surface_indices[i_phase][i_effector] = index_surface_selected + return sparsity_fixed, pb, surface_indices, t + + +def fix_sparsity_combinatorial(planner, pb, LP_SOLVER): + """ + Calls the sl1m solver. Tries to solve non fixed sparsity by handling the combinatorial. + Ultimately calls solve which provides the appropriate cost function + @param planner + @param pb problem data + @param surfaces potential surfaces + @param SOLVER Solver choice + @return true if the problem was solved, fixed surfaces problem, surface_indices and time + """ + G, h, C, d = planner.convert_pb_to_LP(pb) + q = 100.0 * planner.alphas + + result = call_LP_solver(q, G, h, C, d, LP_SOLVER) + t = result.time + if not result.success: + print("Initial LP solver fails in fix_sparsity_combinatorial") + return False, pb, [], t + + alphas = planner.get_alphas(result.x) + if is_sparsity_fixed(pb, alphas): + surface_indices = planner.selected_surfaces(alphas) + for i, phase in enumerate(pb.phaseData): + phase.S = [phase.S[surface_indices[i]]] + phase.n_surfaces = 1 + return True, pb, surface_indices, t + + undecided_surfaces, decided_surfaces = get_undecided_surfaces(pb, alphas) + pbs = generate_fixed_sparsity_problems(pb, alphas) + if pbs is None: + print("No combinatorial problems was found") + return False, pb, [], t + + # Handle the combinatorial + sparsity_fixed = False + i = 0 + for (fixed_pb, combination) in pbs: + G, h, C, d = planner.convert_pb_to_LP(fixed_pb, False) + q = 100.0 * planner.alphas + result = call_LP_solver(q, G, h, C, d, LP_SOLVER) + t += result.time + if result.success: + alphas = planner.get_alphas(result.x) + if is_sparsity_fixed(fixed_pb, alphas): + sparsity_fixed = True + pb = fixed_pb + break + i += 1 + + if not sparsity_fixed: + print("Sparsity could not be fixed") + return False, pb, [], t + + # Get ID of selected surfaces + k = 0 + surface_indices = [] + # Fill empty surface_indices + for i, phase in enumerate(pb.phaseData): + surface_indices.append(0) + # Fill it with decided surfaces + for ( + decided_surface + ) in ( + decided_surfaces + ): # decided surfaces : [i_phase, i_effector, i_surface_selected] + i_phase = decided_surface[0] + index_surface_selected = decided_surface[1] + # Update surface_indices + surface_indices[i_phase] = index_surface_selected + # Fill it with succeeding combinatorial + for i, undecided_surface in enumerate(undecided_surfaces): + i_phase = undecided_surface[0] + index_surface_selected = combination[i] + # Update surface_indices + surface_indices[i_phase] = index_surface_selected + return sparsity_fixed, pb, surface_indices, t + + +def get_undecided_surfaces_gait(pb, alphas): + """ + Get the surfaces and indices of all the undecided surfaces + @param planner the planner + @param pb the problem data + @return a list of phase indices, and sorted surface indices + """ + undecided_surfaces = [] + decided_surfaces = [] + for i, phase in enumerate(pb.phaseData): + for j, n_surface in enumerate(phase.n_surfaces): + if n_surface > 1 and np.array(alphas[i][j]).min() > ALPHA_THRESHOLD: + undecided_surfaces.append([i, j, np.argsort(alphas[i][j])]) + else: + decided_surfaces.append( + [i, j, np.argmin(alphas[i][j])] + ) # argmin(None)=0 + return undecided_surfaces, decided_surfaces + + +def get_undecided_surfaces(pb, alphas): + """ + Get the surfaces and indices of all the undecided surfaces + @param planner the planner + @param pb the problem data + @return a list of phase indices, and sorted surface indices + """ + undecided_surfaces, decided_surfaces = [], [] + for i, phase in enumerate(pb.phaseData): + if phase.n_surfaces > 1 and np.array(alphas[i]).min() > ALPHA_THRESHOLD: + undecided_surfaces.append([i, np.argsort(alphas[i])]) + else: + decided_surfaces.append( + [i, np.argmin(alphas[i]) if len(alphas[i]) > 0 else 0] + ) + return undecided_surfaces, decided_surfaces + + +def is_sparsity_fixed_gait(pb, alphas): + """ + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return true if the sparsity is fixed (ie there is one and only one alpha~0 per phase) + """ + undecided_surfaces, _ = get_undecided_surfaces_gait(pb, alphas) + return len(undecided_surfaces) == 0 + + +def is_sparsity_fixed(pb, alphas): + """ + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return true if the sparsity is fixed (ie there is one and only one alpha~0 per phase) + """ + undecided_surfaces, _ = get_undecided_surfaces(pb, alphas) + return len(undecided_surfaces) == 0 + + +def generate_fixed_sparsity_problems_gait(pb, alphas): + """ + Check if the combinatorial is not too big, if not return all the problems + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return the list of problems + """ + undecided_surfaces, decided_surfaces = get_undecided_surfaces_gait(pb, alphas) + all_len = [len(undecided_surface[-1]) for undecided_surface in undecided_surfaces] + n_pbs = 1 + for l in all_len: + n_pbs *= l + if n_pbs > 1000: + print(" Problem probably too big to handle combinatorial", n_pbs) + return generate_combinatorials_gait(pb, undecided_surfaces, decided_surfaces) + + +def generate_fixed_sparsity_problems(pb, alphas): + """ + Check if the combinatorial is not too big, if not return all the problems + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return the list of problems + """ + undecided_surfaces, decided_surfaces = get_undecided_surfaces(pb, alphas) + all_len = [len(undecided_surface[-1]) for undecided_surface in undecided_surfaces] + n_pbs = 1 + for l in all_len: + n_pbs *= l + if n_pbs > 1000: + print(" Problem probably too big to handle combinatorial", n_pbs) + return generate_combinatorials(pb, undecided_surfaces, decided_surfaces) + + +def generate_combinatorials(pb, undecided_surfaces, decided_surfaces): + """ + Generate all the problems with only one potential surface per undecided phase + fix all decided surfaces + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return the list of problems, the phase indices, and the indices of the selected surfaces + """ + pbs = [] + # Fix all decided surfaces + pb_fixed = copy.deepcopy(pb) + for i, decided_surface in enumerate(decided_surfaces): + phase = pb_fixed.phaseData[decided_surface[0]] + phase.S = [phase.S[decided_surface[-1]]] + phase.n_surfaces = 1 + # Generate combinatorics + sorted_combinations = list(itertools.product(*[s[-1] for s in undecided_surfaces])) + for combination in sorted_combinations[ + 0 : min(MAX_COMB_GENERATED, len(sorted_combinations)) + ]: + pb_comb = copy.deepcopy(pb_fixed) + for i, undecided_surface in enumerate(undecided_surfaces): + phase = pb_comb.phaseData[undecided_surface[0]] + phase.S = [phase.S[combination[i]]] + phase.n_surfaces = 1 + pbs.append([pb_comb, combination]) + return pbs + + +def generate_combinatorials_gait(pb, undecided_surfaces, decided_surfaces): + """ + Generate all the problems with only one potential surface per undecided phase + fix all decided surfaces + @param pb the problem data + @param alphas the list of slack variables found by the planner + @return the list of fixed problems with the indices of the selected surfaces + """ + pbs = [] + # Fix all decided surfaces + pb_fixed = copy.deepcopy(pb) + for i, decided_surface in enumerate(decided_surfaces): + phase = pb_fixed.phaseData[decided_surface[0]] + effector = decided_surface[1] + phase.S[effector] = [phase.S[effector][decided_surface[-1]]] + phase.n_surfaces[effector] = 1 + # Generate combinatorics + sorted_combinations = list(itertools.product(*[s[-1] for s in undecided_surfaces])) + for combination in sorted_combinations[ + 0 : min(MAX_COMB_GENERATED, len(sorted_combinations)) + ]: + pb_comb = copy.deepcopy(pb_fixed) + for i, undecided_surface in enumerate(undecided_surfaces): + phase = pb_comb.phaseData[undecided_surface[0]] + effector = undecided_surface[1] + phase.S[effector] = [phase.S[effector][combination[i]]] + phase.n_surfaces[effector] = 1 + pbs.append([pb_comb, combination]) + return pbs + + +def get_number_surfaces_per_phase_gait(alphas): + nb_surfaces_per_phase = [] + for alpha_phase in alphas: + nb_surface_phase = [] + for alpha_effector in alpha_phase: + nb_surface_phase.append( + len(alpha_effector) if alpha_effector is not None else 1 + ) + nb_surfaces_per_phase.append(nb_surface_phase) + return nb_surfaces_per_phase diff --git a/sl1m/generic_solver.py b/sl1m/generic_solver.py new file mode 100644 index 0000000..4c72755 --- /dev/null +++ b/sl1m/generic_solver.py @@ -0,0 +1,156 @@ +import numpy as np +from sl1m.planner_biped import BipedPlanner +from sl1m.planner_generic import Planner +from sl1m.solver import call_MIP_solver, Solvers, solve_MIP_gurobi_cost +from sl1m.fix_sparsity import ( + fix_sparsity_combinatorial, + fix_sparsity_combinatorial_gait, + optimize_sparse_L1, +) +from sl1m.problem_data import ProblemData + + +# ----------------------- L1 ----------------------------------------------------------------------- + + +def solve_L1_combinatorial( + pb, lp_solver=Solvers.GUROBI, qp_solver=Solvers.GUROBI, costs={}, com=True +): + """ + Solve the problem by first chosing the surfaces with a L1 norm minimization problem handling the + combinatorial if necesary, and then optimizing the feet positions with a QP + @param pb problem to solve + @surfaces surfaces to choose from + @lp_solver solver to use for the LP + @qp_solver solver to use for the QP + @costs cost dictionary specifying the cost functions to use and their parameters + @return ProblemData storing the result + """ + planner = Planner(mip=False, com=com) + sparsity_fixed, pb, surface_indices, t = fix_sparsity_combinatorial_gait( + planner, pb, lp_solver + ) + if sparsity_fixed: + print(" Surfaces selected : ", surface_indices) + pb_data = optimize_sparse_L1(planner, pb, costs, qp_solver, lp_solver) + pb_data.surface_indices = surface_indices + else: + return ProblemData(False, t) + pb_data.time += t + return pb_data + + +def solve_L1_combinatorial_biped( + pb, lp_solver=Solvers.GUROBI, qp_solver=Solvers.GUROBI, costs={} +): + """ + Solve the problem for a biped by first chosing the surfaces with a L1 norm minimization problem + handling the combinatorial if necesary, and then optimizing the feet positions with a QP + @param pb problem to solve + @surfaces surfaces to choose from + @lp_solver solver to use for the LP + @qp_solver solver to use for the QP + @costs cost dictionary specifying the cost functions to use and their parameters + @return ProblemData storing the result + """ + planner = BipedPlanner() + sparsity_fixed, pb, surface_indices, t = fix_sparsity_combinatorial( + planner, pb, lp_solver + ) + if sparsity_fixed: + pb_data = optimize_sparse_L1(planner, pb, costs, qp_solver, lp_solver) + pb_data.surface_indices = surface_indices + else: + return ProblemData(False, t) + pb_data.time += t + return pb_data + + +# ----------------------- MIP ----------------------------------------------------------------------- + + +def solve_MIP(pb, costs={}, solver=Solvers.GUROBI, com=False): + """ + Solve the problem with a MIP solver + @param pb problem to solve + @surfaces surfaces to choose from + @costs cost dictionary specifying the cost functions to use and their parameters + @solver MIP solver to use + @return ProblemData storing the result + """ + planner = Planner(mip=True, com=com) + G, h, C, d = planner.convert_pb_to_LP(pb) + slack_selection_vector = planner.alphas + P = None + + # If no combinatorial call directly a QP + if solver == Solvers.CVXPY and np.linalg.norm(slack_selection_vector) < 1: + return optimize_sparse_L1(planner, pb, costs) + + q = None + if costs != None: + P, q = planner.compute_costs(costs) + result = call_MIP_solver(slack_selection_vector, P, q, G, h, C, d, solver=solver) + + if costs != None and solver == Solvers.CVXPY: + alphas = planner.get_alphas(result.x) + selected_surfaces = planner.selected_surfaces(alphas) + for i, phase in enumerate(pb.phaseData): + for j in range(len(phase.n_surfaces)): + phase.S[j] = [phase.S[j][selected_surfaces[i][j]]] + phase.n_surfaces[j] = 1 + return optimize_sparse_L1( + planner, + pb, + costs, + QP_SOLVER=Solvers.CVXPY, + LP_SOLVER=Solvers.CVXPY, + ) + + if result.success: + alphas = planner.get_alphas(result.x) + coms, moving_foot_pos, all_feet_pos = planner.get_result(result.x) + surface_indices = planner.selected_surfaces(alphas) + return ProblemData( + True, + result.time, + coms, + moving_foot_pos, + all_feet_pos, + surface_indices, + ) + return ProblemData(False, result.time) + + +def solve_MIP_biped(pb, costs={}, solver=Solvers.GUROBI): + """ + Solve the problem with a MIP solver for a biped + @param pb problem to solve + @surfaces surfaces to choose from + @costs cost dictionary specifying the cost functions to use and their parameters + @solver MIP solver to use + @return ProblemData storing the result + """ + planner = BipedPlanner(mip=True) + G, h, C, d = planner.convert_pb_to_LP(pb) + slack_selection_vector = planner.alphas + + if costs != None: + P, q = planner.compute_costs(costs) + result = solve_MIP_gurobi_cost(slack_selection_vector, P, q, G, h, C, d) + else: + result = call_MIP_solver(slack_selection_vector, G, h, C, d, solver=solver) + + if result.success: + alphas = planner.get_alphas(result.x) + coms, moving_foot_pos, all_feet_pos = planner.get_result(result.x) + surface_indices = planner.selected_surfaces(alphas) + return ProblemData( + True, + result.time, + coms, + moving_foot_pos, + all_feet_pos, + surface_indices, + ) + return ProblemData(False, result.time) diff --git a/sl1m/planner.py b/sl1m/planner.py deleted file mode 100644 index 99dc985..0000000 --- a/sl1m/planner.py +++ /dev/null @@ -1,216 +0,0 @@ - -############### Problem definition ############# -from sl1m.problem_definition import * - -#~ NUM VARIABLES - -def getTotalNumVariablesAndIneqConstraints(pb): - cols = sum([phase["S"][0].shape[1] + 1 for phase in pb["phaseData"] ]) - rows = sum( [sum([k.shape[0] for (_, k) in phase["K"][0] ]) + phase["relativeK"][0][1].shape[0] for phase in pb["phaseData"][1:] ]) - rowsOld = sum( [sum([k.shape[0] for (_, k) in phase["K"][0] ]) for phase in pb["phaseData"] ]) - rows += cols #positivity constraint - return rows, cols - -def getTotalNumEqualityConstraints(pb): - return pb["nphases"] - -def numVariablesForPhase(phase): - return phase["S"][0].shape[1] + 1 - -#~ FOOT AND COM EXPRESSION MATRICES - -# z position relative to support foot -def comZExpr(phaseDataT, startCol, endCol): - ret = zeros((3, endCol - startCol)) - ret[-1,phaseDataT["S"][0].shape[1]] = 1. - return ret - -#com is foot position at last phase + z from this phase -def comExpr(phaseDataT, fixedFootMatrix, startCol, endCol): - comZMatrix = comZExpr(phaseDataT, startCol, endCol) - ret = zeros((3,comZMatrix.shape[1]+fixedFootMatrix.shape[1])) - ret[:2,:fixedFootMatrix.shape[1]] = fixedFootMatrix[:-1] - ret[ 2, fixedFootMatrix.shape[1]:fixedFootMatrix.shape[1]+comZMatrix.shape[1]] = comZMatrix[-1,:] - return ret - -# Foot position matrix -# used to retrieve foot position in the previous phase essentially -def footMatrixExpr(phaseDataT, startCol, endCol): - lenVar = endCol - startCol - ret = zeros((3, lenVar)) - ret[:,:phaseDataT["S"][0].shape[1]] = phaseDataT["S"][0][:,:] - return ret - - -#~ CONSTRAINTS -def footFrameConstraint(pb, phaseDataT, constrainedMatrix, frameMatrix, Kk, A, b, endCol,startRow): - K, k = Kk[0], Kk[1] - idRow = startRow + K.shape[0] - A[startRow:idRow, endCol - constrainedMatrix.shape[1]:endCol] = K.dot(constrainedMatrix) - A[startRow:idRow, endCol - frameMatrix.shape[1]:endCol] -= K.dot(frameMatrix) - b[startRow:idRow ] = k - return idRow - -#ensure alpha vector defines a convex combination -def ConvexConstraint(phaseDataT, E, e, startCol, endCol, startRow): - ret = ones((1, endCol - startCol)) - ret[-1,-1] = 0 - idRow = startRow + 1 - E[startRow:idRow, startCol:endCol] = ret - e[startRow:idRow ] = 1 - return idRow - -def addInitialPosPhase(pb): - if pb["p0"] is not None: - phaseNext = pb["phaseData"][0] - phaseNextNext = pb["phaseData"][1] - phase0 = {"S" : [array([pb["p0"][-1]]).T], "moving" : phaseNext["fixed"], "fixed" : phaseNext["moving"], "relativeK" : phaseNextNext["relativeK"], "K" : phaseNext["K"] } - pb["phaseData"] = [phase0] + pb["phaseData"] - pb["p0"] = None - pb["nphases"] = pb["nphases"] +1 - -def convertProblemToLp(pb): - addInitialPosPhase(pb) - #define first problem - #A u <= b - nIneq, nvars = getTotalNumVariablesAndIneqConstraints(pb) - A = zeros((nIneq, nvars)); b = zeros(nIneq) - #E u = b - nEq = getTotalNumEqualityConstraints(pb) - E = zeros((nEq, nvars)); e = zeros(nEq) - - startRow = 0; - startCol = 0; - previousStartCol = 0; - startRowEq = 0 - - # initialisation. Add equality constraint on first position - phaseDataT = pb["phaseData"][0] - endCol = startCol + numVariablesForPhase(phaseDataT) - startRowEq = ConvexConstraint(phaseDataT, E, e, startCol, endCol, startRowEq) - fixedFootMatrix = footMatrixExpr(phaseDataT, startCol, endCol) - startCol = endCol - - for i, phaseDataT in enumerate(pb["phaseData"][1:]): - #inequality - fixed = phaseDataT["fixed"] - moving = phaseDataT["moving"] - nVarPhase = numVariablesForPhase(phaseDataT) - endCol = startCol + nVarPhase - comMatrix = comExpr(phaseDataT, fixedFootMatrix, startCol, endCol); - footMatrix = footMatrixExpr(phaseDataT, startCol, endCol) - fixedFootMatrix = hstack([fixedFootMatrix,zeros((fixedFootMatrix.shape[0], nVarPhase))]) # offset fixedfootmatrix - - startRow = footFrameConstraint(pb, phaseDataT, comMatrix , fixedFootMatrix, phaseDataT["K"][0][fixed ], A, b, endCol, startRow) - startRow = footFrameConstraint(pb, phaseDataT, comMatrix , footMatrix , phaseDataT["K"][0][moving], A, b, endCol, startRow) - startRow = footFrameConstraint(pb, phaseDataT, footMatrix, fixedFootMatrix, phaseDataT["relativeK"][0], A, b, endCol, startRow) - - #equality - startRowEq = ConvexConstraint(phaseDataT, E, e, startCol, endCol, startRowEq) - - # for next phase - fixedFootMatrix = footMatrix - startCol = endCol - - #add positivity constraint - A[startRow:startRow+nvars, : ] = -identity(nvars) - b[-nvars: ] = zeros(nvars) - - startRow += nvars - assert startRowEq == E.shape[0] - assert startRow == A.shape[0] - - return (A,b,E,e) - -###########################" PLOTTING ################" - -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D - -def retrieve_points_from_res(pb, res): - coms = [] - if pb["c0"] is not None: - coms = [pb["c0"]] - - footPos = [[],[]] - allFeetPos = [] - - col = 0 - for i, phaseDataT in enumerate(pb["phaseData"]): - moving = phaseDataT["moving"] - fixed = phaseDataT["fixed"] - nvarPhase = numVariablesForPhase(phaseDataT) - posMatrix = footMatrixExpr(phaseDataT, 0, nvarPhase) - pos = posMatrix.dot(res[col:col + nvarPhase]) - footPos[moving] = footPos[moving] + [pos] - if len(footPos[moving]) <2: - footPos[moving] = footPos[moving] + [pos] - com = zeros(3); - com[2] = res[col + nvarPhase - 1] - if len(footPos[fixed]) > 0: - footPos[fixed] = footPos[fixed] + [footPos[fixed][-1]] #duplicate fixed foot - com[:2] = footPos[fixed][-1][:2] - coms += [com] - allFeetPos += [pos] - col += nvarPhase - return coms, footPos, allFeetPos - -def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2): - x = array(wps)[:,0] - y = array(wps)[:,1] - if(D3): - z = array(wps)[:,2] - ax.scatter(x, y, z, c=color, marker='o', linewidth = 5) - else: - ax.scatter(x,y,color=color, linewidth = linewidth) - -def plotQPRes(pb, res, linewidth=2, ax = None, show = True): - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - ax.grid(False) - ax.view_init(elev=8.776933438381377, azim=-99.32358055821186) - - plotPoints(ax, coms, color = "b") - plotPoints(ax, footpos[RF], color = "r") - plotPoints(ax, footpos[LF], color = "g") - - cx = [c[0] for c in coms] - cy = [c[1] for c in coms] - cz = [c[2] for c in coms] - ax.plot(cx, cy, cz) - px = [c[0] for c in allfeetpos] - py = [c[1] for c in allfeetpos] - pz = [c[2] for c in allfeetpos] - ax.plot(px, py, pz) - - if show: - plt.draw() - plt.show() - - -####################### MAIN ###################" - -if __name__ == '__main__': - from sl1m.stand_alone_scenarios.escaliers import gen_stair_pb, draw_scene - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - C= identity(A.shape[1]) - c = zeros(A.shape[1]) - t2 = clock() - res = qp.solve_least_square(C,c,A,b,E,e) - t3 = clock() - - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - ax = draw_scene(None) - plotQPRes(pb, res, ax = ax) - - diff --git a/sl1m/planner_biped.py b/sl1m/planner_biped.py new file mode 100644 index 0000000..84ff794 --- /dev/null +++ b/sl1m/planner_biped.py @@ -0,0 +1,445 @@ +import numpy as np +from sl1m.constants_and_tools import ( + replace_surfaces_with_ineq_in_problem, + normalize, +) +from sl1m.constraints_biped import BipedConstraints + + +class BipedPlanner: + """ + Implements an optimization problem to find the next surfaces to use for a biped robot + + The problem is a has these variables for each phase: + [ p_i_x, p_i_y, p_i_z, com_z, {alpha_i, beta_i} ] + [ 1 , 1 , 1 , 1 , 0 if n_surfaces == 1, else 2 * 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 and beta is between -alpha and alpha + """ + + def __init__(self, mip=False): + self.mip = mip + self.default_n_variables = 4 + self.slack_scale = 10.0 + + self.n_slack_per_surface = 2 + self.n_ineq_per_surface = 2 + + self.foot = self._expression_matrix(3, 0) + self.com = self._expression_matrix(3, 0) + self.com[2, 2] = 0 + self.com[2, 3] = 1 + self.com_xy = self.com[:2, :] + self.foot_xy = self.foot[:2, :] + + self.cost_dict = { + "final_com": self.end_com_cost, + "effector_positions": self.end_effectors_position_cost, + "coms": self.com_cost, + "posture": self.posture_cost, + "step_size": self.step_size_cost, + } + + 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 _slack_selection_vector(self): + """ + Get the selection matrix of the slack variables + @return the selection matrix + """ + n_variables = self._total_n_variables() + selection_vector = np.zeros(n_variables) + i = 0 + for phase in self.pb.phaseData: + phase_n_variables = self._phase_n_variables(phase) + n_slacks = phase_n_variables - self.default_n_variables + i_start = i + self.default_n_variables + for i_slack in range(0, n_slacks, self.n_slack_per_surface): + selection_vector[i_start + i_slack] = 1 + i += phase_n_variables + return selection_vector + + def set_problem(self, pb): + """ + Set the problem data + @param pb new problem data + """ + self.pb = pb + self.alphas = self._slack_selection_vector() + + def _phase_n_variables(self, phase): + """ + Counts the number of variables in a phase + @param phase concerned phase + @return number of variables in the phase + """ + n_variables = self.default_n_variables + n_surfaces = len(phase.S) + if n_surfaces > 1: + n_variables += n_surfaces * self.n_slack_per_surface + return n_variables + + def _total_n_variables(self): + """ + Counts the number of variables, inequalities constraints in the problem + @return the number of variables of the problem + """ + return sum([self._phase_n_variables(phase) for phase in self.pb.phaseData]) + + def _phase_n_ineq(self, phase): + """ + Counts the dimension of the inequalities in a phase + - COM Kinematic constraints: summation over all effectors, times 2 because there are 2 height possible for the transition + - Relative kinematic constraints between each effectors + - Inequalities relative to each contact surface + @param phase concerned phase + """ + # COM kinematic constraints + n_ineq = sum([k.shape[0] for (_, k) in phase.K]) + + # Foot relative distance + _, Ks = phase.allRelativeK[phase.moving][0] + n_ineq += Ks[0].shape[0] + + # Surfaces + n_ineq += sum([S[1].shape[0] - 1 for S in phase.S]) + + # Slack positivity + n_surfaces = len(phase.S) + if n_surfaces > 1: + n_ineq += n_surfaces * self.n_ineq_per_surface + return n_ineq + + def _total_n_ineq(self): + """ + Counts the number of inequality constraints + @return the number of inequality constraints of the problem + """ + return sum([self._phase_n_ineq(phase) for phase in self.pb.phaseData]) + + def _phase_n_eq(self, phase): + """ + Counts the dimension of the equality constraints of a phase + @param phase concerned phase + """ + n_eq = len(phase.S) + if self.mip: + for phase in self.pb.phaseData: + n_surface = len(phase.S) + if n_surface > 1: + n_eq += 1 + return n_eq + + def _total_n_eq(self): + """ + Counts the number of equality constraints + @return the number of equality constraints of the problem + """ + return sum([self._phase_n_eq(phase) for phase in self.pb.phaseData]) + + def convert_pb_to_LP(self, pb, convert_surfaces=True): + """ + Compute the constraints: + G x <= h + C x = d + + @param pb problem data + @return G, h, C, d + """ + if convert_surfaces: + replace_surfaces_with_ineq_in_problem(pb, True) + + self.set_problem(pb) + + n_variables = self._total_n_variables() + n_ineq = self._total_n_ineq() + n_eq = self._total_n_eq() + + A = np.zeros((n_ineq, n_variables)) + b = np.zeros(n_ineq) + E = np.zeros((n_eq, n_variables)) + e = np.zeros(n_eq) + + i_start = 0 + i_start_eq = 0 + j = 0 + j_previous = 0 + + constraints = BipedConstraints() + for i, phase in enumerate(pb.phaseData): + # inequality + i_start = constraints.fixed_foot_com( + pb, i, phase, A, b, j_previous, j, i_start + ) + i_start = constraints.feet_relative_distance( + pb, i, phase, A, b, j_previous, j, i_start + ) + i_start = constraints.moving_foot_com( + pb, i, phase, A, b, j_previous, j, i_start + ) + i_start = constraints.surface_inequality(phase, A, b, j, i_start) + i_start = constraints.slack_positivity(phase, A, b, j, i_start) + + # equality + i_start_eq = constraints.surface_equality(phase, E, e, j, i_start_eq) + + if self.mip: + i_start_eq = constraints.slack_equality(phase, E, e, i_start_eq, j) + + j_previous = j + j += self._phase_n_variables(phase) + + A, b = normalize([A, b]) + E, e = normalize([E, e]) + return (A, b, E, e) + + def selected_surfaces(self, alphas): + """ + : param: alphas the list of slack variables found by the planner + Return the list of selected surfaces indices in the problem + """ + indices = [] + for id, phase in enumerate(self.pb.phaseData): + if phase.n_surfaces == 1: + index = 0 + else: + index = np.argmin(alphas[id]) + indices.append(index) + return indices + + def get_alphas(self, result): + """ + Retrieve the alphas from the result + @param result vector of variables + @return list of alpha values for each phase + """ + alphas = [] + + j = 0 + for phase in self.pb.phaseData: + phase_alphas = [] + for j_slack in range( + self.default_n_variables, + self._phase_n_variables(phase), + self.n_slack_per_surface, + ): + phase_alphas.append(result[j + j_slack]) + alphas.append(phase_alphas) + + j += self._phase_n_variables(phase) + return alphas + + def get_result(self, result): + """ + Retrieve the com and feet positions from the result + @param result vector of variables + @return com positions, moving foot positions and all feet positions + """ + coms = [] + moving_foot_pos = [] + all_feet_pos = [[self.pb.p0[0]], [self.pb.p0[1]]] + + j = 0 + for i, phase in enumerate(self.pb.phaseData): + moving_foot = phase.moving + fixed_foot = (moving_foot + 1) % 2 + + # CoM + coms.append(self.com.dot(result[j : j + self.default_n_variables])) + + # Moving foot + moving_foot_pos.append( + self.foot.dot(result[j : j + self.default_n_variables]) + ) + + # All feet + all_feet_pos[moving_foot].append(moving_foot_pos[i]) + all_feet_pos[fixed_foot].append(all_feet_pos[fixed_foot][-1]) + + j += self._phase_n_variables(phase) + + all_feet_pos[0].pop(0) + all_feet_pos[1].pop(0) + + return coms, moving_foot_pos, all_feet_pos + + def com_cost(self, coms): + """ + Compute a cost to keep the com close to a target com at each phase + @param coms list of target positions for the com + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for id, phase in enumerate(self.pb.phaseData): + A = np.zeros((2, n_variables)) + A[:, j : j + self.default_n_variables] = self.com + b = coms[id][:2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + + return P, q + + def end_com_cost(self, com): + """ + Compute a cost to keep the final CoM position close to a target one + @param com Target final com position + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + n_phases = self.pb["n_phases"] + j = 0 + for id, phase in enumerate(self.pb.phaseData): + if id == n_phases - 1: + A = np.zeros((2, n_variables)) + A[:, j : j + self.default_n_variables] = self.com_xy + b = com[:2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + j += self._phase_n_variables(phase) + + return P, q + + def end_effectors_position_cost(self, effector_positions): + """ + Compute a cost to keep the final end effectors positions closed to target ones + @param effector_positions list of each effector's final position + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + n_phases = self.pb["n_phases"] + j = 0.0 + for id, phase in enumerate(self.pb.phaseData): + if id >= n_phases - 2: + A = np.zeros((3, n_variables)) + A[:, j : j + self.default_n_variables] = self.foot + b = effector_positions[phase.moving][:3] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + j += self._phase_n_variables(phase) + + return P, q + + def posture_cost(self): + """ + Compute a cost to keep the feet relative positions as close as possible from the initial ones + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + relative_positions = [self.pb.p0[1] - self.pb.p0[0]] + + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j_previous = 0 + j = 0 + for id, phase in enumerate(self.pb.phaseData): + A = np.zeros((3, n_variables)) + b = np.zeros(3) + if id == 0: + if phase.moving == 0: + A[:, j : j + self.default_n_variables] = -self.foot + b = -self.pb.p0[1] + else: + A[:, j : j + self.default_n_variables] = self.foot + b = self.pb.p0[0] + else: + if phase.moving == 0: + A[:, j : j + self.default_n_variables] = -self.foot + A[:, j_previous : j_previous + self.default_n_variables] = self.foot + else: + A[:, j : j + self.default_n_variables] = self.foot + A[ + :, j_previous : j_previous + self.default_n_variables + ] = -self.foot + b += relative_positions + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j_previous = j + j += self._phase_n_variables(phase) + + return P, q + + def step_size_cost(self, step_size): + """ + Compute a cost to keep the step sizes as close as possible of a target step size + @param step_size desired size of the steps + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j_previous = 0 + j = 0 + for id, phase in enumerate(self.pb.phaseData): + foot = phase.moving + A = np.zeros((2, n_variables)) + b = step_size + A[:, j : j + self.default_n_variables] = self.foot_xy + if id == 0: + b += self.pb.p0[foot][:2] + else: + A[:, j_previous : j_previous + self.default_n_variables] = -self.foot_xy + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j_previous = j + j += self._phase_n_variables(phase) + + return P, q + + def compute_costs(self, costs): + """ + This function computes the P and q cost matrix and vector given a cost dictionary + + @param costs the cost dictionary. The keys should match keys from self.cost_dict + @return P, q the cost matrix and vector of the QP problem + """ + n_variables = self._total_n_variables() + + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + if costs == {}: + P += np.identity(n_variables) + else: + for key, val in [(k, v) for k, v in costs.items() if k in self.cost_dict]: + if key in self.cost_dict.keys(): + P_, q_ = self.cost_dict[key](*val[1:]) + P += P_ * val[0] + q += q_ * val[0] + else: + print("Unknown cost to add to the biped problem") + + return P, q diff --git a/sl1m/planner_generic.py b/sl1m/planner_generic.py new file mode 100644 index 0000000..b0447b7 --- /dev/null +++ b/sl1m/planner_generic.py @@ -0,0 +1,676 @@ +import numpy as np +from sl1m.constants_and_tools import * +from sl1m.constraints import Constraints +from copy import deepcopy + + +class Planner: + """ + Implements an optimization problem to find the next surfaces to use + + 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 , 1 , 1 , 1 , 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) + """ + + def __init__(self, mip=False, com=False): + self.mip = mip + self.com = com + + self.default_n_equality_constraints = 2 * int(self.com) + self.default_n_variables = 4 * int(self.com) + + self.cost_dict = { + "final_com": self.end_com_cost, + "end_effector_positions": self.end_effectors_position_cost, + "effector_positions": self.effector_position_cost, + "coms_xy": self.com_cost_xy, + "coms_z": self.com_cost_z, + "coms_3D": self.com_cost_3D, + "posture": self.posture_cost, + "step_size": self.step_size_cost, + } + + self.com_costs = ["final_com", "coms_xy", "coms_z", "coms_3D"] + + def _default_n_variables(self, phase): + """ + @param phase phase concerned + @return the number of non slack variables in phase + """ + return self.default_n_variables + 3 * len(phase.moving) + + def _expression_matrix(self, size, _default_n_variables, j): + """ + Generate a selection matrix for a given variable + @param size number of rows of the variable + @param _default_n_variables number of non slack variables in the phase + @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, _default_n_variables)) + M[:, j : j + size] = np.identity(size) + return M + + def com_xy(self, phase): + """ + Generate a selection matrix for the com x and y components + @param phase phase data + @return a (2, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(2, self._default_n_variables(phase), 0) + + def com_z(self, phase): + """ + Generate a selection matrix for the com x and y components + @param phase phase data + @return a (2, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(2, self._default_n_variables(phase), 2) + + def com_1(self, phase): + """ + Generate a selection matrix for the com_1 + @param phase phase data + @return a (3, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(3, self._default_n_variables(phase), 0) + + def com_2(self, phase): + """ + Generate a selection matrix for the com_2 + @param phase phase data + @return a (3, phase number of variables (without slacks)) matrix + """ + M = self._expression_matrix(3, self._default_n_variables(phase), 0) + M[2, 2] = 0 + M[2, 3] = 1 + return M + + def com_xyz(self, phase): + """ + Generate a selection matrix for the height of com_2 + @param phase phase data + @return a (1, phase number of variables (without slacks)) matrix + """ + return self._expression_matrix(4, self._default_n_variables(phase), 0) + + def foot(self, phase, foot=None, id=None): + """ + Generate a selection matrix for a given foot + @param phase phase data + @param foot foot to select + @param id id of the foot in the moving feet list + @return a (3, number of variables (without slacks)) selection matrix + """ + if foot is not None: + id = np.argmax(phase.moving == foot) + elif id is None: + print("Error in foot selection matrix: you must specify either foot or id") + j = self.default_n_variables + 3 * id + return self._expression_matrix(3, self._default_n_variables(phase), j) + + def foot_xy(self, phase, foot=None, id=None): + """ + Generate a selection matrix for a given foot x and y coordinates + @param phase phase data + @param foot foot to select + @param id id of the foot in the moving feet list + @return a (3, number of variables (without slacks)) selection matrix + """ + if foot is not None: + id = np.argmax(phase.moving == foot) + elif id is None: + print("Error in foot selection matrix: you must specify either foot or id") + j = self.default_n_variables + 3 * id + return self._expression_matrix(2, self._default_n_variables(phase), j) + + def _slack_selection_vector(self): + """ + Get the selection vector of the slack variables + @return the selection vector + """ + n_variables = self._total_n_variables() + selection_vector = np.zeros(n_variables) + i = 0 + for phase in self.pb.phaseData: + n_variables_phase = self._phase_n_variables(phase) + n_slacks = n_variables_phase - self._default_n_variables(phase) + i_start = i + self._default_n_variables(phase) + selection_vector[i_start : i_start + n_slacks] = np.ones(n_slacks) + i += n_variables_phase + return selection_vector + + def set_problem(self, pb): + """ + Set the problem data + @param pb new problem data + """ + self.pb = pb + self.n_effectors = pb.n_effectors + self.alphas = self._slack_selection_vector() + + def _feet_last_moving_phase(self, phase_id): + """ + Ids of the last phase each foot moved at a given phase + @param phase_id id of the phase + @return a list of n_effetors terms corresponding to the last phase the feet moved, -1 if it hasn't moved yet + """ + feet_last_moving_phase = [-1] * self.n_effectors + for phase in self.pb.phaseData: + if phase.id >= phase_id: + break + for foot in phase.moving: + feet_last_moving_phase[foot] = phase.id + return feet_last_moving_phase + + def _phase_n_variables(self, phase): + """ + Counts the number of variables in a phase + @param phase concerned phase + @return number of variables in the phase + """ + n_variables = self._default_n_variables(phase) + for n_surface in phase.n_surfaces: + if n_surface > 1: + n_variables += n_surface + return n_variables + + def _total_n_variables(self): + """ + Counts the number of variables, inequalities constraints in the problem + @return the number of variables of the problem + """ + return sum([self._phase_n_variables(phase) for phase in self.pb.phaseData]) + + def _phase_n_ineq(self, phase): + """ + Counts the dimension of the inequalities in a phase + - COM Kinematic constraints: summation over all effectors, times 2 because there are 2 height possible for the transition + - Relative kinematic constraints between each effectors + - Inequalities relative to each contact surface + @param phase concerned phase + """ + n_ineq = 0 + # COM kinematic constraints + if self.com: + for foot, (K, _) in enumerate(phase.K): + if foot in phase.stance: + n_ineq += K.shape[0] + if phase.id == 0: + n_ineq += K.shape[0] + elif foot in self.pb.phaseData[phase.id - 1].stance: + n_ineq += K.shape[0] + + # Foot relative distance + for foot in phase.moving: + for (other, Ks) in phase.allRelativeK[foot]: + if other in phase.stance: + n_ineq += Ks[0].shape[0] + elif phase.id == 0: + n_ineq += Ks[0].shape[0] + elif other in self.pb.phaseData[phase.id - 1].stance: + n_ineq += Ks[0].shape[0] + + # Surfaces + n_ineq += sum( + [sum([S[1].shape[0] for S in foot_surfaces]) for foot_surfaces in phase.S] + ) + + # Slack positivity + for n_surface in phase.n_surfaces: + if n_surface > 1: + n_ineq += n_surface + + return n_ineq + + def _total_n_ineq(self): + """ + Counts the number of inequality constraints + @return the number of inequality constraints of the problem + """ + return sum([self._phase_n_ineq(phase) for phase in self.pb.phaseData]) + + def _total_n_eq(self): + """ + Counts the number of equality constraints + @return the number of equality constraints of the problem + """ + n_eq = self.default_n_equality_constraints * self.pb.n_phases + if self.mip: + for phase in self.pb.phaseData: + for n_surface in phase.n_surfaces: + if n_surface > 1: + n_eq += 1 + return n_eq + + def convert_pb_to_LP(self, pb, convert_surfaces=False): + """ + Compute the constraints: + G x <= h + C x = d + + @param pb problem data + @return G, h, C, d + """ + if convert_surfaces: + replace_surfaces_with_ineq_in_problem(pb, False) + + self.set_problem(pb) + + n_variables = self._total_n_variables() + n_ineq = self._total_n_ineq() + n_eq = self._total_n_eq() + + G = np.zeros((n_ineq, n_variables)) + h = np.zeros(n_ineq) + C = np.zeros((n_eq, n_variables)) + d = np.zeros(n_eq) + + i_start = 0 + i_start_eq = 0 + js = [0] + cons = Constraints(self.n_effectors, self.com) + for phase in self.pb.phaseData: + feet_phase = self._feet_last_moving_phase(phase.id) + + i_start = cons.foot_relative_distance( + self.pb, phase, G, h, i_start, js, feet_phase + ) + i_start = cons.surface_inequality(phase, G, h, i_start, js[-1]) + i_start = cons.slack_positivity(phase, G, h, i_start, js[-1]) + + if self.com: + i_start = cons.fixed_foot_com( + self.pb, phase, G, h, i_start, js, feet_phase + ) + # i_start_eq = cons.com(self.pb, phase, C, d, i_start_eq, js, feet_phase) + + if self.mip: + i_start_eq = cons.slack_equality(phase, C, d, i_start_eq, js[-1]) + + js.append(js[-1] + self._phase_n_variables(phase)) + + G, h = normalize([G, h]) + C, d = normalize([C, d]) + return G, h, C, d + + def selected_surfaces(self, alphas): + """ + :param: alphas nested list of slack variables for each foot in each phase + Return the list of selected surfaces indices in the problem + """ + indices = [] + for phase in self.pb.phaseData: + phase_indices = [] + for i, n_surface in enumerate(phase.n_surfaces): + if n_surface == 1: + phase_indices.append(0) + else: + phase_indices.append(np.argmin(alphas[phase.id][i])) + indices.append(phase_indices) + return indices + + def get_alphas(self, result): + """ + Retrieve the alphas from the result + @param result vector of variables + @return list of alpha values for each phase + """ + alphas = [] + j_alpha = 0 + for phase in self.pb.phaseData: + alpha_phase = [] + j_alpha += self._default_n_variables(phase) + for n_surface in phase.n_surfaces: + if n_surface == 1: + alpha_phase.append(None) + else: + alpha_phase.append(result[j_alpha : j_alpha + n_surface]) + j_alpha += n_surface + alphas.append(alpha_phase) + return alphas + + def get_result(self, result): + """ + Retrieve the com and feet positions from the result + @param result vector of variables + @return com positions, moving foot positions and all feet positions + """ + if self.com: + coms = [] + else: + coms = None + moving_feet_pos = [] + all_feet_pos = [[self.pb.p0[i]] for i in range(self.n_effectors)] + + j = 0 + for i, phase in enumerate(self.pb.phaseData): + if self.com: + coms.append( + self.com_2(phase).dot( + result[j : j + self._default_n_variables(phase)] + ) + ) + phase_moving_feet = [] + for foot in phase.moving: + phase_moving_feet.append( + self.foot(phase, foot).dot( + result[j : j + self._default_n_variables(phase)] + ) + ) + moving_feet_pos.append(phase_moving_feet) + + for foot in range(self.n_effectors): + if foot in phase.moving: + id = np.argmax(phase.moving == foot) + all_feet_pos[foot].append(moving_feet_pos[i][id]) + elif foot in phase.stance: + all_feet_pos[foot].append(all_feet_pos[foot][-1]) + else: + all_feet_pos[foot].append(None) + + j += self._phase_n_variables(phase) + + return coms, moving_feet_pos, all_feet_pos + + def com_cost_xy(self, coms): + """ + Compute a cost to keep the com in X,Y axis close to a target com at each phase + @param coms list of target positions for the com + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for phase in self.pb.phaseData: + A = np.zeros((2, n_variables)) + A[:, j : j + self._default_n_variables(phase)] = self.com_xy(phase) + b = coms[phase.id][:2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + + return P, q + + def com_cost_z(self, coms): + """ + Compute a cost to keep the com in Z axis close to a target com at each phase + @param coms list of target positions for the com + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for phase in self.pb.phaseData: + A = np.zeros((2, n_variables)) + b = np.zeros(2) + A[:, j : j + self._default_n_variables(phase)] = self.com_z(phase) + b[0] = coms[phase.id][2] + b[1] = coms[phase.id][2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + + return P, q + + def com_cost_3D(self, coms): + """ + Compute a cost to keep the com close to a target com in X,Y,Z axis at each phase + @param coms list of target positions for the com + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for phase in self.pb.phaseData: + A = np.zeros((4, n_variables)) + b = np.zeros(4) + A[:, j : j + self._default_n_variables(phase)] = self.com_xyz(phase) + b[:3] = coms[phase.id][:] + b[3] = coms[phase.id][2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + + return P, q + + def end_com_cost(self, com): + """ + Compute a cost to keep the final CoM position close to a target one + @param com Target final com position + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for phase in self.pb.phaseData: + if phase.id == self.pb.n_phases - 1: + A = np.zeros((2, n_variables)) + A[:, j : j + self._default_n_variables(phase)] = self.com_xy(phase) + b = com[:2] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + j += self._phase_n_variables(phase) + + return P, q + + def end_effectors_position_cost(self, effector_positions): + """ + Compute a cost to keep the final end effectors positions closed to target ones + @param effector_positions list of each effector's final position + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + js = [0] + for phase in self.pb.phaseData: + if phase.id < self.pb.n_phases - 1: + js.append(js[-1] + self._phase_n_variables(phase)) + + feet_phase = self._feet_last_moving_phase(self.pb.n_phases - 1) + for foot in range(self.n_effectors): + phase = self.pb.phaseData[-1] + if foot in phase.moving: + j = js[phase.id] + A = np.zeros((3, n_variables)) + A[:, j : j + self._default_n_variables(phase)] = self.foot(phase, foot) + b = effector_positions[foot][:3] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + elif feet_phase[foot] != -1: + phase = self.pb.phaseData[feet_phase[foot]] + j = js[phase.id] + A = np.zeros((3, n_variables)) + A[:, j : j + self._default_n_variables(phase)] = self.foot(phase, foot) + b = effector_positions[foot][:3] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + return P, q + + def posture_cost(self): + """ + Compute a cost to keep the feet relative positions as close as possible from the initial ones + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + relative_positions = [ + [None for _ in range(self.n_effectors)] for _ in range(self.n_effectors) + ] + for foot in range(self.n_effectors): + for other in range(self.n_effectors): + if ( + foot != other + and self.pb.p0[foot] is not None + and self.pb.p0[other] is not None + ): + relative_positions[foot][other] = np.array( + self.pb.p0[foot] - self.pb.p0[other] + ) + + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + js = [0] + j = 0 + for id, phase in enumerate(self.pb.phaseData): + feet_phase = self._feet_last_moving_phase(id) + + for foot in range(self.n_effectors): + for other in range(self.n_effectors): + if other > foot and relative_positions[foot][other] is not None: + A = np.zeros((3, n_variables)) + b = relative_positions[foot][other] + if foot in phase.moving: + A[:, j : j + self._default_n_variables(phase)] = self.foot( + phase, foot + ) + elif feet_phase[foot] != -1: + jf = js[feet_phase[foot]] + phase_f = self.pb.phaseData[feet_phase[foot]] + A[ + :, jf : jf + self._default_n_variables(phase_f) + ] = self.foot(phase_f, foot) + else: + b -= self.pb.p0[foot] + + if other in phase.moving: + A[:, j : j + self._default_n_variables(phase)] = -self.foot( + phase, other + ) + elif feet_phase[other] != -1: + jf = js[feet_phase[other]] + phase_f = self.pb.phaseData[feet_phase[other]] + A[ + :, jf : jf + self._default_n_variables(phase_f) + ] = -self.foot(phase_f, other) + else: + b += self.pb.p0[other] + + P += np.dot(A.T, A) + q += -np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + js.append(j) + + return P, q + + def step_size_cost(self, step_size): + """ + Compute a cost to keep the step sizes as close as possible of a target step size + @param step_size desired size of the steps + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + js = [0] + j = 0 + for phase in self.pb.phaseData: + feet_phase = self._feet_last_moving_phase(phase.id) + for foot in phase.moving: + A = np.zeros((2, n_variables)) + b = deepcopy(step_size) + A[:, j : j + self._default_n_variables(phase)] = self.foot_xy( + phase, foot + ) + if feet_phase[foot] != -1: + j_f = js[feet_phase[foot]] + phase_f = self.pb.phaseData[feet_phase[foot]] + A[ + :, j_f : j_f + self._default_n_variables(phase_f) + ] = -self.foot_xy(phase_f, foot) + elif self.pb.p0[foot] is not None: + b += self.pb.p0[foot][:2] + else: + continue + + P += np.dot(A.T, A) + q -= np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + js.append(j) + + return P, q + + def effector_position_cost(self, end_effector_positions): + """ + Compute a cost to keep the effectors with a specified distance to the shoulders + @param end_effector_positions desired effector positions + @return P matrix and q vector s.t. we minimize x' P x + q' x + """ + n_variables = self._total_n_variables() + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + j = 0 + for phase in self.pb.phaseData: + # feet_phase = self._feet_last_moving_phase(phase.id) + for foot in phase.moving: + A = np.zeros((2, n_variables)) + A[:, j : j + self._default_n_variables(phase)] = self.foot_xy( + phase, foot + ) + b = end_effector_positions[foot][phase.id] + + P += np.dot(A.T, A) + q -= np.dot(A.T, b).reshape(A.shape[1]) + + j += self._phase_n_variables(phase) + + return P, q + + def compute_costs(self, costs): + """ + This function computes the P and q cost matrix and vector given a cost dictionary + @param costs the cost dictionary. The keys should match keys from self.cost_dict + @return P, q the cost matrix and vector of the QP problem + """ + n_variables = self._total_n_variables() + + P = np.zeros((n_variables, n_variables)) + q = np.zeros(n_variables) + + if costs == {}: + P += np.identity(n_variables) + else: + for key, val in [(k, v) for k, v in costs.items() if k in self.cost_dict]: + if not self.com and key in self.com_costs: + print( + "COM cost given, but no COM variable in planner. The cost is ignored" + ) + elif key in self.cost_dict.keys(): + P_, q_ = self.cost_dict[key](*val[1:]) + P += P_ * val[0] + q += q_ * val[0] + else: + print("Unknown cost to add to the biped problem") + return P, q diff --git a/sl1m/planner_l1.py b/sl1m/planner_l1.py deleted file mode 100644 index 9d890c8..0000000 --- a/sl1m/planner_l1.py +++ /dev/null @@ -1,530 +0,0 @@ - - -############### Problem definition ############# - -from sl1m.problem_definition import * - - -#### constraint specification #### - -DEFAULT_NUM_VARS = 4 -SLACK_SCALE = 10. - -#extra variables when multiple surface: a0, variable for inequality, a1 slack for equality constraint, then a2 = |a1| -#so vars are x, y, z, zcom, a0, a1 -#extra constraints are a1 - a0 <= 0 ; -a1 - a0 <= 0; a0 >= 0, a0 >=0 is implied by the first 2 -NUM_SLACK_PER_SURFACE = 2 -NUM_INEQ_SLACK_PER_SURFACE = 2 # - -def numVariablesForPhase(phase): - ret = DEFAULT_NUM_VARS - numSurfaces =len(phase["S"]) - if numSurfaces > 1: - ret += numSurfaces * NUM_SLACK_PER_SURFACE - return ret - -def numIneqForPhase(phase): - #surface -one because last is equality - ret = sum([k.shape[0] for (_, k) in phase["K"][0] ]) + phase["relativeK"][0][1].shape[0] + sum([S[1].shape[0]-1 for S in phase["S"]]) - numSurfaces =len(phase["S"]) - if numSurfaces > 1: - ret += numSurfaces * NUM_INEQ_SLACK_PER_SURFACE - return ret - -def getTotalNumVariablesAndIneqConstraints(pb): - nphases = pb["nphases"] - cols = sum([numVariablesForPhase(phase) for phase in pb["phaseData"]]) - rows = sum( [numIneqForPhase(phase) for phase in pb["phaseData"] ]) - return rows, cols - -def numEqForPhase(phase): - return len(phase["S"]) - -def getTotalNumEqualityConstraints(pb): - return sum( [numEqForPhase(phase) for phase in pb["phaseData"] ]) - - - -#### Selection matrices #### - -# COM position given by the first two parameters (x, y of foot) and the fourth one (height) -def _COMExpressionMatrix(): - ret = zeros((3, DEFAULT_NUM_VARS)) - ret[:2,:2] = identity(2) - ret[2, -1] = 1 - return ret - - -# Foot position given by the first three parameters (x, y, z of foot) -def _footExpressionMatrix(): - return identity(3) - -def _footExpressionMatrixXY(): - return identity(2) - -def _footExpressionMatrixz(): - ret = zeros((1,3)) - ret[0,-1] = 1 - return ret - -# z position of COM -def _COMzmatrix(): - ret = zeros((1, DEFAULT_NUM_VARS)) - ret[-1,-1:] = [1] - return ret - -COMExpressionMatrix = _COMExpressionMatrix() -footExpressionMatrix = _footExpressionMatrix() -footExpressionMatrixz = _footExpressionMatrixz() -footExpressionMatrixXY = _footExpressionMatrixXY() -COMzmatrix = _COMzmatrix() - -# com is always above foot, so we lony care about the z value -def FixedFootCOMKinConstraintInitPhase(pb, phaseDataT, A, b, startCol, endCol): - fixed = phaseDataT["fixed"] - pos = pb["p0"][fixed] - K, k = phaseDataT["K"][0][fixed] - idRow = K.shape[0] - comOff = COMzmatrix.shape[1] - - A[:idRow, startCol:startCol+comOff] = K[:,-1:].dot(COMzmatrix) - b[:idRow ] = k + K[:,-1:].dot(pos[-1:]) - return idRow - -def FixedFootCOMKinConstraintVarPhase(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow): - fixed = phaseDataT["fixed"] - K, k = phaseDataT["K"][0][fixed] - idRow = startRow + K.shape[0] - offCOm = COMzmatrix.shape[1] - offFoot = footExpressionMatrixz.shape[1] - A[startRow:idRow, startCol:startCol+offCOm] = K[:,-1:].dot(COMzmatrix) - A[startRow:idRow, previousStartCol:previousStartCol+offFoot] = -K[:,-1:].dot(footExpressionMatrixz) - b[startRow:idRow ] = k - - return idRow - - -def FixedFootCOMConstraint(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow): - if startRow == 0: - if pb["p0"] is None: - fixed = phaseDataT["fixed"] #0 constraints - K, k = phaseDataT["K"][0][fixed] - return K.shape[0] - else: - return FixedFootCOMKinConstraintInitPhase(pb, phaseDataT, A, b, startCol, endCol) - else: - return FixedFootCOMKinConstraintVarPhase (pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow) - - -#com position at this phase is contrained by moving foot position -def MovingFootCOMConstraintInitPhase(pb, phaseDataT, A, b, startCol, endCol, startRow): - fixed = phaseDataT["fixed"] - pos = pb["p0"][fixed] - moving = phaseDataT["moving"] - K, k = phaseDataT["K"][0][moving] - idRow = startRow + K.shape[0] - offCOmZ = COMzmatrix.shape[1] - offFoot = footExpressionMatrix.shape[1] - A[startRow:idRow, startCol:startCol+offCOmZ] = K[:,-1:].dot(COMzmatrix) - A[startRow:idRow, startCol:startCol+offFoot] -= K.dot(footExpressionMatrix) - b[startRow:idRow ] = k - K.dot(pos) - return idRow - - -def MovingFootCOMConstraintVarPhase(phaseDataT, A, b, previousStartCol, startCol, endCol, startRow): - moving = phaseDataT["moving"] - K, k = phaseDataT["K"][0][moving] - idRow = startRow + K.shape[0] - offCOmZ = COMzmatrix.shape[1] - offFootXY = footExpressionMatrixXY.shape[1] - offFoot = footExpressionMatrix.shape[1] - A[startRow:idRow, previousStartCol:previousStartCol+offFootXY] = K[:,:2].dot(footExpressionMatrixXY) - A[startRow:idRow, startCol:startCol+offCOmZ] = K[:,-1:].dot(COMzmatrix) - A[startRow:idRow, startCol:startCol+offFoot] = -K.dot(footExpressionMatrix) - b[startRow:idRow ] = k - return idRow - -def MovingFootCOMConstraint(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow, first): - if first: - if pb["p0"] is None: - moving = phaseDataT["moving"] #0 constraints - K, k = phaseDataT["K"][0][moving] - return startRow + K.shape[0] - else: - return MovingFootCOMConstraintInitPhase(pb, phaseDataT, A, b, startCol, endCol, startRow) - else: - return MovingFootCOMConstraintVarPhase (phaseDataT, A, b, previousStartCol, startCol, endCol, startRow) - - -def FixedFootRelativeDistanceConstraintInitPhase(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow): - fixed = phaseDataT["fixed"] - K, k = phaseDataT["relativeK"][0] - footMatrix = currentFootExpressionMatrix(phaseDataT, startCol, endCol) - pos = pb["p0"][fixed] - idRow = K.shape[0] - offFoot = footMatrix.shape[1] - A[:idRow, startCol:startCol+offFoot] = K.dot(footMatrix) - b[:idRow ] = k + K.dot(pos) - return idRow + startRow - -def FixedFootRelativeDistanceConstraintVarPhase(pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow): - K, k = phaseDataT["relativeK"][0] - idRow = startRow + K.shape[0] - offFoot = footExpressionMatrix.shape[1] - A[startRow:idRow, startCol:startCol+offFoot] = K.dot(footExpressionMatrix) - A[startRow:idRow, previousStartCol:previousStartCol+offFoot] = -K.dot(footExpressionMatrix) - b[startRow:idRow ] = k - return idRow - - -def FixedFootConstraintRelativeDistance(pb, phaseDataT, A, b, previousCol, startCol, endCol, startRow, first): - if first: - if True: - K, k = phaseDataT["relativeK"][0] #0 constraints - return startRow + K.shape[0] - else: - return FixedFootRelativeDistanceConstraintInitPhase(pb, phaseDataT, A, b, startCol, endCol, startRow) - else: - return FixedFootRelativeDistanceConstraintVarPhase (pb, phaseDataT, A, b, previousCol, startCol, endCol, startRow) - - -def SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow): - sRow = startRow - nSurfaces = len(phaseDataT["S"]) - idS = DEFAULT_NUM_VARS - for (S,s) in phaseDataT["S"]: - idRow = sRow + S.shape[0]-1 - A[sRow:idRow, startCol:startCol+footExpressionMatrix.shape[1]] = S[:-1,:].dot(footExpressionMatrix) - b[sRow:idRow ] = s[:-1] - if nSurfaces >1: - A[sRow:idRow, startCol+idS] = -ones(idRow-sRow) * SLACK_SCALE - sRow = idRow - idS += NUM_SLACK_PER_SURFACE - return idRow - -def SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow): - numSurfaces =len(phaseDataT["S"]) - idRow = startRow - if numSurfaces > 1: - varOffset = DEFAULT_NUM_VARS #foot and com positions - for row in range(startRow, startRow + numSurfaces * NUM_INEQ_SLACK_PER_SURFACE, NUM_INEQ_SLACK_PER_SURFACE): - i = row - startRow - col = (int) (startCol+varOffset + i / NUM_INEQ_SLACK_PER_SURFACE * NUM_SLACK_PER_SURFACE) - A[row , col:col+2] = [-1, 1]; # - a0 + a1 <= 0 - A[row+1, col:col+2] = [-1, -1]; # -a0 - a1 <= 0 - idRow = row + NUM_INEQ_SLACK_PER_SURFACE - return idRow - -def EqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq): - sRow = startRowEq - nSurfaces = len(phaseDataT["S"]) - if nSurfaces == 1: - E[sRow, startCol:startCol+footExpressionMatrix.shape[1]] = phaseDataT["S"][0][0][-1] - e[sRow] = phaseDataT["S"][0][1][-1] - return sRow+1 - else: - idS = DEFAULT_NUM_VARS - for (S,s) in phaseDataT["S"]: - E[sRow, startCol:startCol+footExpressionMatrix.shape[1]] = S[-1,:].dot(footExpressionMatrix) - #~ E[sRow, startCol+idS+1] = -1 # E x + a1 = e - E[sRow, startCol+idS+1] = -1 * SLACK_SCALE # E x + a1 = e - e[sRow] = s[-1] - idS += NUM_SLACK_PER_SURFACE - sRow += 1 - return sRow - -def convertProblemToLp(pb, convertSurfaces = True): - if convertSurfaces: - replace_surfaces_with_ineq_in_problem(pb) - #define first problem - #A u <= b - nIneq, nvars = getTotalNumVariablesAndIneqConstraints(pb) - A = zeros((nIneq, nvars)); b = zeros(nIneq) - #E u = b - nEq = getTotalNumEqualityConstraints(pb) - E = zeros((nEq, nvars)); e = zeros(nEq) - - startRow = 0; - startRowEq = 0; - startCol = 0; - previousStartCol = 0; - endCol = 0; - #~ fixedFootMatrix = None; - - for i, phaseDataT in enumerate(pb["phaseData"]): - #inequality - endCol = startCol + numVariablesForPhase(phaseDataT) - startRow = FixedFootCOMConstraint (pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow) - startRow = FixedFootConstraintRelativeDistance (pb, phaseDataT, A, b, previousStartCol, startCol, endCol, startRow, first = i == 0) - startRow = MovingFootCOMConstraint(pb,phaseDataT, A, b, previousStartCol, startCol, endCol, startRow, first = i == 0) - startRow = SurfaceConstraint(phaseDataT, A, b, startCol, endCol, startRow) - startRow = SlackPositivityConstraint(phaseDataT, A, b, startCol, endCol, startRow) - startRowEq = EqualityConstraint(phaseDataT, E, e, startCol, endCol, startRowEq) - previousStartCol = startCol - startCol = endCol - - assert startRowEq == E.shape[0] - assert startRow == A.shape[0] - - A,b = normalize([A,b]) - E,e = normalize([E,e]) - return (A,b,E,e) - -def slackSelectionMatrix(pb): - nvars = getTotalNumVariablesAndIneqConstraints(pb)[1] - c = zeros(nvars) - cIdx = 0 - for i, phase in enumerate(pb["phaseData"]): - phaseVars = numVariablesForPhase(phase) - nslacks = phaseVars - DEFAULT_NUM_VARS - startIdx = cIdx + DEFAULT_NUM_VARS - for i in range (0,nslacks,NUM_SLACK_PER_SURFACE): - c[startIdx + i] = 1 - cIdx += phaseVars - assert cIdx == nvars - return c - -def num_non_zeros(pb, res): - nvars = getTotalNumVariablesAndIneqConstraints(pb)[1] - indices = [] - cIdx = 0 - wrongsurfaces = [] - for i, phase in enumerate(pb["phaseData"]): - numSurfaces = len(phase["S"]) - phaseVars = numVariablesForPhase(phase) - if numSurfaces > 1: - startIdx = cIdx + DEFAULT_NUM_VARS - betas = [res[startIdx+j] for j in range(0,numSurfaces*2,2) ] - if array(betas).min() > 0.01: - #~ print "wrong ", i, array(betas).min() - indices += [i] - sorted_surfaces = np.argsort(betas) - #~ print "sorted_surfaces ",sorted_surfaces - wrongsurfaces += [[[phase["S"][idx]] for idx in sorted_surfaces] ] - #~ print "lens ", len([[phase["S"][idx]] for idx in sorted_surfaces] ) - cIdx += phaseVars - return indices, wrongsurfaces - -def isSparsityFixed(pb, res): - indices, wrongsurfaces = num_non_zeros(pb, res) - return len(indices) == 0 - -def genOneComb(pb,indices, surfaces, res): - pb1 = pb.copy() - for i, idx in enumerate(indices): - pb1["phaseData"][idx]["S"] = surfaces[i][0] - res += [pb1] - -import itertools -import copy - -def genCombinatorialRec(pb, indices, wrongsurfaces, res): - lenss = [len(surfs) for surfs in wrongsurfaces] - all_indexes = [[el for el in range(lens)] for lens in [len(surfs) for surfs in wrongsurfaces]] - combs = [el for el in itertools.product(*all_indexes)] - for comb in combs: - pb1 = copy.deepcopy(pb) - for i, idx in enumerate(indices): - pb1["phaseData"][idx]["S"] = wrongsurfaces[i][comb[i]] - res += [[pb1, comb, indices]] - - - -def generateAllFixedScenariosWithFixedSparsity(pb, res): - indices, wrongsurfaces = num_non_zeros(pb, res) - all_len = [len(surfs) for surfs in wrongsurfaces] - comb = 1 - for el in all_len: - comb *= el - res = [] - if comb >1000: - print("problem probably too big ", comb) - else: - genCombinatorialRec(pb, indices, wrongsurfaces, res) - return res - - - -def bestSelectedSurfaces(pb, res): - surfaces = [] - indices = [] - cIdx = 0 - nvars = getTotalNumVariablesAndIneqConstraints(pb)[1] - for i, phase in enumerate(pb["phaseData"]): - numSurfaces = len(phase["S"]) - phaseVars = numVariablesForPhase(phase) - if numSurfaces == 1: - surfaces = surfaces + [phase["S"][0]] - indices = indices + [0] - else: - startIdx = cIdx + DEFAULT_NUM_VARS - betas = [res[startIdx+j] for j in range(0,numSurfaces*2,2) ] - assert min(betas) >= -0.00000001 - bestIdx = betas.index(array(betas).min()) - surfaces = surfaces + [phase["S"][bestIdx]] - - indices = indices + [bestIdx] - cIdx += phaseVars - assert cIdx == nvars - return surfaces, indices - -###########################" PLOTTING ################" - -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D - -def retrieve_points_from_res(pb, res): - coms = [] - if pb["c0"] is not None: - coms = [pb["c0"]] - - if pb["p0"] is not None: - footPos = [[pb["p0"][LF]],[pb["p0"][RF]]] - allFeetPos = [footPos[0][0], footPos[1][0]] - else: - footPos = [[],[]] - allFeetPos = [] - - col = 0 - for i, phaseDataT in enumerate(pb["phaseData"]): - moving = phaseDataT["moving"] - fixed = phaseDataT["fixed"] - pos = footExpressionMatrix.dot(res[col:col + footExpressionMatrix.shape[1]]) - footPos[moving] = footPos[moving] + [pos] - com = zeros(3); - com[2] = res[col + 3] - if len(footPos[moving]) <2: - footPos[moving] = footPos[moving] + [pos] - if len(footPos[fixed]) > 0: - footPos[fixed] = footPos[fixed] + [footPos[fixed][-1]] #duplicate fixed foot - com[:2] = footPos[fixed][-1][:2] - coms += [com] - allFeetPos += [pos] - col += numVariablesForPhase(phaseDataT) - return coms, footPos, allFeetPos - - -def plotPoints(ax, wps, color = "b", D3 = True, linewidth=2): - x = array(wps)[:,0] - y = array(wps)[:,1] - if(D3): - z = array(wps)[:,2] - ax.scatter(x, y, z, c=color, marker='o', linewidth = 5) - else: - ax.scatter(x,y,color=color, linewidth = linewidth) - - -def plotConstraints(ax, pb, allfeetpos, coms): - from tools.plot_plytopes import plot_polytope_H_rep - for i, phase in enumerate(pb["phaseData"][:]): - if i <1 : - continue - fixed = phase["fixed"] - moving = phase["moving"] - oldK, oldk = pb["phaseData"][i-1]["K"][0][fixed] - oldK = oldK.copy() - oldk = oldk.copy() - oldk += oldK.dot(allfeetpos[i-1]) - K, k = phase["K"][0][moving] - K = K.copy() - k = k.copy() - pos = allfeetpos[i] - com = coms[i] - relK, relk = pb["phaseData"][i-1]["relativeK"][0] - relK = relK.copy() - relk = relk.copy() - relk += relK.dot(allfeetpos[i-1]) - - k = k + K.dot(pos) - resK = vstack([oldK,K]) - resk = concatenate([oldk, k]).reshape((-1,)) - if True: - #~ if i %2 ==0: - #~ if i %2 ==1: - try : - #~ plot_polytope_H_rep(resK,resk.reshape((-1,1)), ax = ax) - plot_polytope_H_rep(relK,relk.reshape((-1,1)), ax = ax) - #~ plot_polytope_H_rep(K,k.reshape((-1,1)), ax = ax) - except: - print("qhullfailed") - - -def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show = True): - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - ax.grid(False) - - ax.set_autoscale_on(False) - ax.view_init(elev=8.776933438381377, azim=-99.32358055821186) - - #~ plotPoints(ax, coms, color = "b") - plotPoints(ax, footpos[RF], color = "r") - plotPoints(ax, footpos[LF], color = "g") - - cx = [c[0] for c in coms] - cy = [c[1] for c in coms] - cz = [c[2] for c in coms] - #~ ax.plot(cx, cy, cz) - px = [c[0] for c in allfeetpos] - py = [c[1] for c in allfeetpos] - pz = [c[2] for c in allfeetpos] - ax.plot(px, py, pz) - - if show: - plt.ion() - plt.show() - - -####################### MAIN ###################" - -if __name__ == '__main__': - from sl1m.stand_alone_scenarios.complex import gen_stair_pb, draw_scene - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - - A,b = normalize([A,b]) - C = identity(A.shape[1]) * 0.00001 - c = slackSelectionMatrix(pb) * 100. - t2 = clock() - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - t3 = clock() - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - ax = draw_scene(None) - plotQPRes(pb, res, ax=ax, plot_constraints = False) - - surfaces, indices = bestSelectedSurfaces(pb, res) - for i, phase in enumerate(pb["phaseData"]): - phase["S"] = [surfaces[i]] - - t1 = clock() - A, b, E, e = convertProblemToLp(pb, False) - - - C = identity(A.shape[1]) - c = zeros(A.shape[1]) - t2 = clock() - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - t3 = clock() - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - ax = draw_scene(None) - plotQPRes(pb, res, ax=ax, plot_constraints = False) - - - - - - diff --git a/sl1m/planner_scenarios/bauzil.sh b/sl1m/planner_scenarios/bauzil.sh deleted file mode 100644 index 705bb59..0000000 --- a/sl1m/planner_scenarios/bauzil.sh +++ /dev/null @@ -1,12 +0,0 @@ -#! /bin/bash - - -gepetto-gui & -hpp-rbprm-server & -cp lp_stair_bauzil.py lp_stair_bauzil_hrp2_path.py /media/data/dev/linux/hpp/src/hpp-rbprm-corba/script/scenarios/demos -#~ ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py talos_circle -ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py lp_stair_bauzil - -pkill -f 'gepetto-gui' -pkill -f 'hpp-rbprm-server' - diff --git a/sl1m/planner_scenarios/bauzil_10.sh b/sl1m/planner_scenarios/bauzil_10.sh deleted file mode 100644 index 9b21159..0000000 --- a/sl1m/planner_scenarios/bauzil_10.sh +++ /dev/null @@ -1,12 +0,0 @@ -#! /bin/bash - - -gepetto-gui & -hpp-rbprm-server & -cp lp_stair_bauzil_10.py lp_stair_bauzil_hrp2_path_10.py /media/data/dev/linux/hpp/src/hpp-rbprm-corba/script/scenarios/demos -#~ ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py talos_circle -ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py lp_stair_bauzil_10 - -pkill -f 'gepetto-gui' -pkill -f 'hpp-rbprm-server' - diff --git a/sl1m/planner_scenarios/complex.py b/sl1m/planner_scenarios/complex.py deleted file mode 100644 index 3c7ff99..0000000 --- a/sl1m/planner_scenarios/complex.py +++ /dev/null @@ -1,143 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * - - -from plot_plytopes import * - -floor2 = [[-3., 0.4 , 0. ], [-2.7 , 0.4, 0. ], [-2.7 , 0.1, 0. ], [-03., 0.1, 0. ], ] -floor = [[-0.30, 0.54 , 0. ], [0.01 , 0.54, 0. ], [0.01 , -0.46, 0. ], [-0.30, -0.46, 0. ], ] -step1 = [[ 0.01, 0.54 , 0.1 ], [0.31 , 0.54, 0.1], [0.31 , -0.46, 0.1 ], [ 0.01, -0.46, 0.1 ], ] -step2 = [[ 0.31, 0.54 , 0.2 ], [0.61 , 0.54, 0.2], [0.61 , -0.46, 0.2 ], [ 0.31, -0.46, 0.2 ], ] -step3 = [[ 0.61, 0.54 , 0.3 ], [0.91 , 0.54, 0.3], [0.91 , -0.46, 0.3 ], [ 0.61, -0.46, 0.3 ], ] -step4 = [[ 0.91, 0.54 , 0.4 ], [1.21 , 0.54, 0.4], [1.21 , -0.46, 0.4 ], [ 0.91, -0.46, 0.4 ], ] -step5 = [[ 1.21, 0.54 , 0.5 ], [1.51 , 0.54, 0.5], [1.51 , -0.46, 0.5 ], [ 1.21, -0.46, 0.5 ], ] -step6 = [[ 1.51, 0.54 , 0.6 ], [1.81 , 0.54, 0.6], [1.81 , -0.46, 0.6 ], [ 1.51, -0.46, 0.6 ], ] -#~ step7 = [[ 1.51, 0.94 , 0.6 ], [2.51 , 0.94, 0.6], [2.51 , -1.06, 0.6 ], [ 1.51, -1.06, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -#~ platfo = [[-1.49, -0.06 , 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.06, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -#~ step8 = [[-1.49, -0.06 , 0.45], [-1.49, 0.24, 0.45],[-2.49, 0.24, 0.45], [-2.49, -0.06, 0.45], ] -#~ step9 = [[-1.49, 0.24 , 0.30], [-1.49, 0.54, 0.30],[-2.49, 0.54, 0.30], [-2.49, 0.24, 0.30], ] -#~ step10= [[-1.49, 0.54 , 0.15], [-1.49, 0.84, 0.15],[-2.49, 0.84, 0.15], [-2.49, 0.54, 0.15], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.30 , 0.05 ], [-2.45 , 0.30, 0.05 ], [ -2.45, 0.53, 0.05 ], [-2.11, 0.53, 0.05 ], ] -rub3 = [[ -1.91, -0.15 , 0.1 ], [-2.25 , -0.15, 0.1 ], [ -2.25, 0.18, 0.1 ], [-1.91, 0.18, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.53, 0.15 ], [-1.69, 0.53, 0.15 ], ] -rub5 = [[ -1.49, -0.15 , 0.2 ], [-1.83 , -0.15, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.53, 0.2 ], [-1.29, 0.53, 0.2 ], ] -rub7 = [[ -1.09, -0.15 , 0.15 ], [-1.43 , -0.15, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.53, 0.1], [-0.89, 0.53, 0.1 ], ] -rub8 = [[ -0.89, -0.15 , 0.025 ], [-1.02 , -0.15, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.15 , 0.025 ], [-0.86 , -0.15, 0.025], [-0.86, 0.52, 0.025 ], [ -0.35, 0.52, 0.025], ] -rub8 = [[ -0.89, -0.15 , 0.05 ], [-1.02 , -0.15, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.35, -0.15 , 0.05 ], [-0.86 , -0.15, 0.05], [-0.86, 0.52, 0.05 ], [ -0.35, 0.52, 0.05], ] - -all_surfaces = [floor2, floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -#~ arub1 = array(rub1).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -#~ astep8 = array(step8).T -#~ astep9 = array(step9).T -#~ astep10 = array(step10).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] -allsteps = [astep2,astep1,astep3,astep4,astep5,astep6,astep7] -allstepsandfloor = allsteps + [arub9,afloor] - - -#~ surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],allrub,allrub,allrub,[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1], [astep2,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6],[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] - -surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],allrub,allrub,allrub,[arub75] ,allstepsandfloor,allstepsandfloor,allstepsandfloor,allsteps, allsteps,allsteps,allsteps, allsteps,[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo]] - -surfaces1 = [ [abridge], [abridge],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[aplatfo],[aplatfo],[aplatfo],[aplatfo]] -surfaces = surfaces0 + surfaces1 - -def gen_stair_pb(): - #~ for i in range(nphases) - #~ kinematicConstraints = genKinematicConstraints(min_height = 0.6) - kinematicConstraints = genKinematicConstraints(min_height = None) - relativeConstraints = genFootRelativeConstraints() - - #~ arrayAllSurf = [array(el).T for el in all_surfaces] - #~ surfaces = [[arub2,arub3],[arub3,arub2]] + [ arrayAllSurf for _ in range(30)] + [[aplatfo],[aplatfo]] - - nphases = len(surfaces) - - p0 = [array([-2.7805096486250154, 0.33499999999999996, 0.]), array([-2.7805096486250154, 0.145,0.])]; - - #~ res = { "p0" : p0, "c0" : None, "nphases": nphases} - res = { "p0" : None, "c0" : None, "nphases": nphases} - - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -if __name__ == '__main__': - - from sl1m.fix_sparsity import solveL1 - - pb = gen_stair_pb() - - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - #~ ax = draw_scene() - #~ plotQPRes(pb, res, ax=ax, plot_constraints = False) - - diff --git a/sl1m/planner_scenarios/complex.sh b/sl1m/planner_scenarios/complex.sh deleted file mode 100644 index 16219b6..0000000 --- a/sl1m/planner_scenarios/complex.sh +++ /dev/null @@ -1,12 +0,0 @@ -#! /bin/bash - - -gepetto-gui & -hpp-rbprm-server & -cp lp_complex.py lp_complex_path.py /media/data/dev/linux/hpp/src/hpp-rbprm-corba/script/scenarios/demos -#~ ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py talos_circle -ipython -i --no-confirm-exit $DEVEL_HPP_DIR/src/multicontact-locomotion-planning/scripts/run_mlp.py lp_complex - -pkill -f 'gepetto-gui' -pkill -f 'hpp-rbprm-server' - diff --git a/sl1m/planner_scenarios/complex1.py b/sl1m/planner_scenarios/complex1.py deleted file mode 100644 index f4cfb19..0000000 --- a/sl1m/planner_scenarios/complex1.py +++ /dev/null @@ -1,262 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * - - -from sl1m.plot_plytopes import * -from sl1m.planner_l1 import * - -floor2 = [[-3., 0.4 , 0. ], [-2.7 , 0.4, 0. ], [-2.7 , 0.1, 0. ], [-03., 0.1, 0. ], ] -floor = [[-0.23, 0.54 , 0. ], [-0.1 , 0.54, 0. ], [-0.1 , -0.46, 0. ], [-0.23, -0.46, 0. ], ] -step1 = [[ 0.05, 0.54 , 0.1 ], [0.25 , 0.54, 0.1], [0.25 , -0.46, 0.1 ], [ 0.05, -0.46, 0.1 ], ] -step2 = [[ 0.35, 0.54 , 0.2 ], [0.55 , 0.54, 0.2], [0.55 , -0.46, 0.2 ], [ 0.35, -0.46, 0.2 ], ] -step3 = [[ 0.65, 0.54 , 0.3 ], [0.85 , 0.54, 0.3], [0.85 , -0.46, 0.3 ], [ 0.65, -0.46, 0.3 ], ] -step4 = [[ 0.95, 0.54 , 0.4 ], [1.15 , 0.54, 0.4], [1.15 , -0.46, 0.4 ], [ 0.95, -0.46, 0.4 ], ] -step5 = [[ 1.25, 0.54 , 0.5 ], [1.45 , 0.54, 0.5], [1.45 , -0.46, 0.5 ], [ 1.25, -0.46, 0.5 ], ] -step6 = [[ 1.55, 0.54 , 0.6 ], [1.75 , 0.54, 0.6], [1.75 , -0.46, 0.6 ], [ 1.55, -0.46, 0.6 ], ] -#~ step7 = [[ 1.51, 0.94 , 0.6 ], [2.51 , 0.94, 0.6], [2.51 , -1.06, 0.6 ], [ 1.51, -1.06, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -#~ platfo = [[-1.49, -0.06 , 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.06, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -#~ step8 = [[-1.49, -0.06 , 0.45], [-1.49, 0.24, 0.45],[-2.49, 0.24, 0.45], [-2.49, -0.06, 0.45], ] -#~ step9 = [[-1.49, 0.24 , 0.30], [-1.49, 0.54, 0.30],[-2.49, 0.54, 0.30], [-2.49, 0.24, 0.30], ] -#~ step10= [[-1.49, 0.54 , 0.15], [-1.49, 0.84, 0.15],[-2.49, 0.84, 0.15], [-2.49, 0.54, 0.15], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.19 , 0.05 ], [-2.45 , 0.19, 0.05 ], [ -2.45, 0.45, 0.05 ], [-2.11, 0.45, 0.05 ], ] -rub3 = [[ -1.91, -0.05 , 0.1 ], [-2.25 , -0.05, 0.1 ], [ -2.25, 0.18, 0.1 ], [-1.91, 0.18, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.45, 0.15 ], [-1.69, 0.45, 0.15 ], ] -rub5 = [[ -1.49, -0.05 , 0.2 ], [-1.83 , -0.05, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.45, 0.2 ], [-1.29, 0.45, 0.2 ], ] -rub7 = [[ -1.09, -0.05 , 0.15 ], [-1.43 , -0.05, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.45, 0.1], [-0.89, 0.45, 0.1 ], ] -rub8 = [[ -0.89, -0.05 , 0.025 ], [-1.02 , -0.05, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.05 , 0.025 ], [-0.86 , -0.05, 0.025], [-0.86, 0.45, 0.025 ], [ -0.35, 0.45, 0.025], ] -rub8 = [[ -0.89, -0.05 , 0.05 ], [-1.02 , -0.05, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.45, -0.05 , 0.05 ], [-0.86 , -0.05, 0.05], [-0.86, 0.45, 0.05 ], [ -0.45, 0.45, 0.05], ] - -all_surfaces = [floor2, floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -#~ arub1 = array(rub1).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -#~ astep8 = array(step8).T -#~ astep9 = array(step9).T -#~ astep10 = array(step10).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] -allsteps = [astep2,astep1,astep3,astep4,astep5,astep6,astep7] -allstepsandfloor = allsteps + [arub9,afloor] - - -#~ surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],allrub,allrub,allrub,[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1], [astep2,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6],[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] - -#surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],allrub,allrub,allrub,[arub75] ,allstepsandfloor,allstepsandfloor,allstepsandfloor,allsteps, allsteps,allsteps,allsteps, allsteps,[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo]] - -#surfaces1 = [ [abridge], [abridge],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[aplatfo],[aplatfo],[aplatfo],[aplatfo]] -#surfaces = surfaces0 + surfaces1 -surfaces = [[arub2],[arub3],[arub4],[arub5],[arub6],[arub7],[arub75] ,[arub9],[arub9],[afloor],[afloor],[astep1],[astep1],[astep2],[astep2],[astep3],[astep3], [astep4], [astep4],[astep5],[astep5],[astep6],[astep6]] - -def gen_stair_pb(): - #~ for i in range(nphases) - #~ kinematicConstraints = genKinematicConstraints(min_height = 0.6) - kinematicConstraints = genKinematicConstraints(min_height = None) - relativeConstraints = genFootRelativeConstraints() - - #~ arrayAllSurf = [array(el).T for el in all_surfaces] - #~ surfaces = [[arub2,arub3],[arub3,arub2]] + [ arrayAllSurf for _ in range(30)] + [[aplatfo],[aplatfo]] - - nphases = len(surfaces) - - p0 = [array([-2.7805096486250154, 0.31499999999999996, 0.]), array([-2.7805096486250154, 0.125,0.])]; - - res = { "p0" : p0, "c0" : None, "nphases": nphases} - - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -def min_dist(size): - C = zeros((size,size)) - for i in range(size%4): - C[i:i+3, :3] = identity(3) - C[i:i+3,4:7] = -identity(3) - c = zeros(size) - c =-dot(C.T, c).reshape(c.shape[0]) - C = dot(C.T, C) - C += identity(size) * 0.00001 - return C, c - - -def solveL1(): - #~ draw_rectangle(l,ax) - #~ plt.show() - - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - A,b = normalize([A,b]) - - #~ C, c = least_square_cost_function (pb, A.shape[1], comTarget = array([30.,0.,0.5]), reg_term = 0.0000001) - C = identity(A.shape[1]) * 0.00001 - #~ C = identity(A.shape[1]) * 0.1 - c = slackSelectionMatrix(pb) - #~ c = zeros(A.shape[1]) - - - t2 = clock() - - - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - ax = draw_scene() - plotQPRes(pb, res, ax=ax, plot_constraints = False) - - - ok = isSparsityFixed(pb, res) - solutionIndices = None - solutionComb = None - if not ok: - pbs = generateAllFixedScenariosWithFixedSparsity(pb, res) - #~ pbs.reverse() - for (pbComb, comb, indices) in pbs: - A, b, E, e = convertProblemToLp(pbComb, convertSurfaces = False) - C = identity(A.shape[1]) * 0.00001 - c = slackSelectionMatrix(pbComb) - try: - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - if isSparsityFixed(pbComb, res): - print("solved sparsity") - - coms, footpos, allfeetpos = retrieve_points_from_res(pbComb, res) - ax = draw_scene() - plotQPRes(pbComb, res, ax=ax, plot_constraints = False) - pb = pbComb - ok = True - solutionIndices = indices - solutionComb = comb - break - else: - print("not olved yet") - except: - print("unfeasible problem") - - - if ok: - surfacesret, indices = bestSelectedSurfaces(pb, res) - for i, phase in enumerate(pb["phaseData"]): - phase["S"] = [surfaces[i][indices[i]]] - for i, idx in enumerate(solutionIndices): - pb["phaseData"][idx]["S"] = [surfaces[i][solutionComb[i]]] - - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - - A,b = normalize([A,b]) - C = identity(A.shape[1]) - c = zeros(A.shape[1]) - #~ c = slackSelectionMatrix(pb) - #~ C, c = min_dist(A.shape[1]) - #~ c += slackSelectionMatrix(pb) * 1000. - t2 = clock() - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - #~ resx = qp.solve_lp( c,A,b,E,e) - #~ res = resx["x"] - t3 = clock() - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - ax = draw_scene() - plotQPRes(pb, res, ax=ax, plot_constraints = False) - - return pb, coms, footpos, allfeetpos, res - -def solve(): - - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - - A,b = normalize([A,b]) - C = identity(A.shape[1]) - c = zeros(A.shape[1]) - t2 = clock() - res = qp.quadprog_solve_qp(C, c,A,b,E,e) - t3 = clock() - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - ax = draw_scene() - plotQPRes(pb, res, ax=ax, plot_constraints = False) - return pb, coms, footpos, allfeetpos, res - -if __name__ == '__main__': - - pb, coms, footpos, allfeetpos, res = solve() - #ax = draw_scene() - #plotQPRes(pb, res, ax=ax, plot_constraints = False) - - diff --git a/sl1m/planner_scenarios/escaliers.py b/sl1m/planner_scenarios/escaliers.py deleted file mode 100644 index 460c27a..0000000 --- a/sl1m/planner_scenarios/escaliers.py +++ /dev/null @@ -1,92 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * - - -from plot_plytopes import * - -floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ] - -step1 = [[0.3, 0.6, 0.15],[0.3, -0.16, 0.15],[0.6, -0.16, 0.15],[0.6, 0.6, 0.15]] -step2 = [[0.6, 0.6, 0.3 ],[0.6, -0.16, 0.3 ],[0.9, -0.16, 0.3 ],[0.9, 0.6, 0.3 ]] -step3 = [[0.9, 0.6, 0.45],[0.9, -0.16, 0.45],[1.2, -0.16, 0.45],[1.2, 0.6, 0.45]] -step4 = [[1.2, 0.6, 0.6 ],[1.2, -0.16, 0.6 ],[1.5, -0.16, 0.6 ],[1.5, 0.6, 0.6 ]] - - -floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ] - -step1 = [[0.3, 0.6, 0.1],[0.3, -0.16, 0.1],[0.6, -0.16, 0.1],[0.6, 0.6, 0.1]] -step2 = [[0.6, 0.6, 0.2 ],[0.6, -0.16, 0.2],[0.9, -0.16, 0.2 ],[0.9, 0.6, 0.2 ]] -step3 = [[0.9, 0.6, 0.3],[0.9, -0.16, 0.3],[1.2, -0.16, 0.3],[1.2, 0.6, 0.3]] -step4 = [[1.2, 0.6, 0.4 ],[1.2, -0.16, 0.4 ],[1.5, -0.16, 0.4 ],[1.5, 0.6, 0.4 ]] - - -all_surfaces = [floor, step1, step2, step3, step4] - -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T - -surfaces = [[afloor], [afloor], [astep1,astep2,astep3],[astep2,astep3,astep1], [astep3,astep2,astep1,astep4], [astep3,astep4], [astep4],[astep4]] - -def gen_stair_pb(): - #~ kinematicConstraints = genKinematicConstraints(min_height = 0.6) - kinematicConstraints = genKinematicConstraints(min_height = None) - relativeConstraints = genFootRelativeConstraints() - nphases = len(surfaces) - p0 = None - p0 = [array([0.,0., 0.]), array([0.,0., 0.])]; - res = { "p0" : p0, "c0" : None, "nphases": nphases} - - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(surfaces, ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -if __name__ == '__main__': - - from sl1m.fix_sparsity import solveL1 - - pb = gen_stair_pb() - - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene, plot = True) - - - diff --git a/sl1m/planner_scenarios/flat_ground.py b/sl1m/planner_scenarios/flat_ground.py deleted file mode 100644 index e815c40..0000000 --- a/sl1m/planner_scenarios/flat_ground.py +++ /dev/null @@ -1,102 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * - - -from sl1m.plot_plytopes import * - - -all_surfaces = [] - -left = [[-50.,0.04,0.], [50.,0.04,0.], [50.,40.,0.], [-50.,40.,0.]] -right = [[-50.,-40,0.], [50.,-40.,0.], [50.,-0.04,0.], [-50.,-0.04,0.]] -allsurf = [[-50.,-40,0.], [50.,-40.,0.], [50.,40,0.], [-50.,40,0.]] - -#~ all_surfaces = [left, right] -all_surfaces = [allsurf] - -# generate problem for flat ground scenarios, from init pos for feet p0LF and p0RF -# this method also defines the template definition for defining contact plan -def genFlatGroundProblem(p0LF, p0RF, nphases = 5): - #~ for i in range(nphases) - #~ kinematicConstraints = genKinematicConstraints(min_height = 0.4) - kinematicConstraints = genKinematicConstraints() - relativeConstraints = genFootRelativeConstraints() - surfaces = [None, None]; - p0 = [[0., -0.06,0.], [0., 0.06,0.]]; - #~ surfaces[LF] = array(right).T # for the moment arbitrary surfaces to separate foot - surfaces[LF] = [array(allsurf).T] # for the moment arbitrary surfaces to separate foot - #~ surfaces[LF] = array([[-50.,-40,0.], [50.,-40.,0.], [50.,-0.04,0.], [-50.,-0.04,0.] ]).T - surfaces[RF] = [array(allsurf).T] - p0 [LF] = p0LF; - p0 [RF] = p0RF; - - res = { "p0" : p0, "c0" : array([0., -0.06,0.5]), "nphases": nphases} - #~ res = { "p0" : None, "nphases": nphases} - - #TODO in non planar cases, K must be rotated - #relative k is the constraint with respect to the previous frame - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i%2]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i%2]))], "S" : surfaces[i%2] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -if __name__ == '__main__': - pb = genFlatGroundProblem([0.,0.05,0.],[0.,-0.05,0.], 20) - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - - print('A.shape', A.shape) - - C, c = least_square_cost_function (pb, A.shape[1], comTarget = array([3.,10.,0.2])) - t2 = clock() - res = qp.solve_least_square(C,c,A,b,E,e) - t3 = clock() - - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - - plotQPRes(pb, res, plot_constraints = False) - #~ plotQPRes(pb, res, plot_constraints = True) - - diff --git a/sl1m/planner_scenarios/hrp2/__init__.py b/sl1m/planner_scenarios/hrp2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/sl1m/planner_scenarios/hrp2/complex.py b/sl1m/planner_scenarios/hrp2/complex.py new file mode 100644 index 0000000..a216707 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/complex.py @@ -0,0 +1,85 @@ +import numpy as np +import os +import sl1m.tools.plot_tools as plot +import matplotlib.pyplot as plt + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.stand_alone_scenarios.problem_definition_hrp2 import ( + Problem as HRP2Problem, +) +from sl1m.problem_definition import Problem +from sl1m.generic_solver import * + +from time import perf_counter as clock + +import sl1m.stand_alone_scenarios + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_BIPED_PLANNER = False +USE_MIP = True +USE_COM = True + +paths = [ + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", +] +limbs = ["LF", "RF"] + +if __name__ == "__main__": + t_init = clock() + + from sl1m.planner_scenarios.hrp2 import lp_complex_path as tp + + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + tp.rbprmBuilder, tp.ps, tp.afftool, tp.pathId, tp.v, 0.7, False + ) + t_2 = clock() + + p0 = [ + np.array(tp.q_init[:3]) + [0, 0.095, -0.5], + np.array(tp.q_init[:3]) + [0, -0.095, -0.5], + ] + t_3 = clock() + + if USE_BIPED_PLANNER: + pb = HRP2Problem() + pb.generate_problem(R, surfaces, [0, 1], p0) + t_4 = clock() + if USE_MIP: + result = solve_MIP_biped(pb, surfaces) + else: + result = solve_L1_combinatorial_biped(pb, surfaces) + else: + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem(limb_names=limbs, constraint_paths=paths) + pb.generate_problem(R, surfaces_gait, GAIT, p0, tp.q_init[:3]) + t_4 = clock() + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_complex.py b/sl1m/planner_scenarios/hrp2/path_files/lp_complex.py new file mode 100644 index 0000000..33611a0 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_complex.py @@ -0,0 +1,439 @@ +from hpp.corbaserver.rbprm.hrp2 import Robot +from hpp.gepetto import Viewer +from tools.display_tools import * +import time + +print("Plan guide trajectory ...") +from . import lp_complex_path as tp + +print("Done.") +import time + +pId = tp.ps.numberPaths() - 1 +fullBody = Robot() + +# Set the bounds for the root +fullBody.setJointBounds("root_joint", [-10.135, 2, -20, 20, 0, 2.8]) +# fullBody.setConstrainedJointsBounds() +# fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) +# fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) +# add the 6 extraDof for velocity and acceleration (see *_path.py script) +fullBody.client.robot.setDimensionExtraConfigSpace(0) +# ~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) +ps = tp.ProblemSolver(fullBody) +# ~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) +# ~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) +# load the viewer +v = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True) + +# load a reference configuration +# ~ q_ref = fullBody.referenceConfig[::]+[0]*6 +q_ref = fullBody.referenceConfig[::] +q_init = q_ref[::] +fullBody.setReferenceConfig(q_ref) +fullBody.setCurrentConfig(q_init) +fullBody.setPostureWeights(fullBody.postureWeights[::]) +# ~ fullBody.setPostureWeights(fullBody.postureWeights[::]) +# fullBody.usePosturalTaskContactCreation(True) + +print("Generate limb DB ...") +tStart = time.time() +# generate databases : + +nbSamples = 10000 +fullBody.addLimb( + fullBody.rLegId, + fullBody.rleg, + fullBody.rfoot, + fullBody.rLegOffset, + fullBody.rLegNormal, + fullBody.rLegx, + fullBody.rLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.rLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) +fullBody.addLimb( + fullBody.lLegId, + fullBody.lleg, + fullBody.lfoot, + fullBody.lLegOffset, + fullBody.rLegNormal, + fullBody.lLegx, + fullBody.lLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.lLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) + + +tGenerate = time.time() - tStart +print("Done.") +print("Databases generated in : " + str(tGenerate) + " s") + +# define initial and final configurations : +configSize = ( + fullBody.getConfigSize() - fullBody.client.robot.getDimensionExtraConfigSpace() +) + + +# ~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation +q_init[:3] = [0.1, -0.82, 0.648702] +q_init[7:] = [ + 0.0, + 0.0, + 0.0, + 0.0, # CHEST HEAD 7-10 + 0.261799388, + 0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # LARM 11-17 + 0.261799388, + -0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # RARM 18-24 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # LLEG 25-30 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # RLEG 31-36 +] +v(q_init) + +# fullBody.resetJointsBounds() +from hpp.corbaserver.rbprm import rbprmstate +from hpp.corbaserver.rbprm import state_alg + + +def getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]): + s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs) + rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) + lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) + return { + Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]), + Robot.lLegId: (lLegPn[0][0], lLegPn[1][0]), + } + + +# ~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) + + +from sl1m.planner_scenarios.complex import * + +from hpp.corbaserver.rbprm.rbprmstate import StateHelper + +pb, coms, footpos, allfeetpos, res = solve() + + +def gen_state(s, pId, com, num_max_sample=1, first=False, normal=z, newContact=True): + # ~ pId = 6 + phase = pb["phaseData"][pId] + moving = phase["moving"] + movingID = fullBody.lLegId + if moving == RF: + movingID = fullBody.rLegId + pos = allfeetpos[pId] + pos[2] += 0.01 + if not first: + com[2] += 0.5 + # ~ com[2] += 0.4 + # ~ print "com" , com + # ~ print "pos" , pos.tolist() + # ~ q = fullBody.getCurrentConfig() + # ~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) + if newContact: + sres, succ = state_alg.addNewContact( + s, movingID, pos.tolist(), normal.tolist(), num_max_sample=200 + ) + else: + sres, succ = StateHelper.cloneState(s) + if not succ: + print("succes ?", succ) + succCom = False + ite = 0 + # ~ if first: + # ~ print "FIRST " + # ~ com[2] -= 0.25 + # ~ while(not succCom and ite < 11): + while not succCom and ite < 30: + succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) + com[2] -= 0.05 + ite += 1 + if succCom: + q = sres.q() + q[3:7] = [0.0, 0.0, 0.0, 1.0] + q[3:] = fullBody.referenceConfig[3:] + sres.setQ(q) + succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) + if not succCom: + print("refail") + v(sres.q()) + return sres + + +q = fullBody.getCurrentConfig() +q[:3] = [-2.69, 0.24, 0.649702] +v(q) +s = rbprmstate.State(fullBody, q=q, limbsIncontact=[fullBody.lLegId, fullBody.rLegId]) + +idfirst = 2 +coms[0] = array(s.getCenterOfMass()) +# ~ coms[0] = coms[1] +# ~ coms[1] = array(s.getCenterOfMass()) + + +def normal(phase): + s = phase["S"][0] + n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0]) + n /= norm(n) + if n[2] < 0.0: + for i in range(3): + n[i] = -n[i] + # ~ print "normal ", n + return n + + +all_states = [s] +sprev = s +# ~ for i in range(0, len(pb["phaseData"])-1): +for i in range(0, 5): + # ~ for i in range(0, 3): + # ~ for i in range(2, 5): + # ~ for i in range(2, 3): + # ~ for i in range(1, 2): + # ~ com = (coms[i-i] ).tolist() + com = (coms[i]).tolist() + # ~ if i > 2: + # ~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() + # get normal + n = normal(pb["phaseData"][i]) + snew = gen_state(sprev, i + 2, com, num_max_sample=200, first=False, normal=n) + all_states += [snew] + sprev = snew + # ~ com2 = coms[i+1].tolist() + # ~ snew2 = gen_state(snew, i , com2, num_max_sample = 20, normal = n, newContact = False ) + # ~ all_states += [snew2] + # ~ sprev = snew2 + +# ~ raise ValueError + +# ~ all_states = all_states[:-1] +extraDof = [0] * 6 +configs = [st.q() + extraDof for st in all_states[:]] +i = 0 + +print("SID ", [s.sId for s in all_states]) + +beginId = 0 + +# ~ configs = configs[:-1] + +# ~ configs = [q1, q2, q3, q4, q5] +# ~ from cPickle import dump + +# ~ f = open('contacts_plateformes.txt','w') +# ~ dump(contacts,f) +# ~ f.close() +ax = draw_scene() +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=True) +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=False) + +paths = [] + + +def play_int(f_r=100): + for (pid, pl) in paths: + for i in range(f_r): + frame = float(i) / float(f_r) * pl + v((ps.configAtParam(pid, frame))) + + +# ~ def ppath(pid): +# ~ length = + +# ~ raise ValueError + + +comTrajs = [] +stateTrajs = [] + + +def comTraj(c0, c1, t=1.0): + zero = [0.0, 0.0, 0] + return fullBody.generateComTraj([c0, c1], [zero], [zero], 1.0) + + +from hpp.gepetto import PathPlayer + +pp = PathPlayer(v) + +from time import sleep +from hpp.corbaserver.rbprm.rbprmstate import StateHelper + + +def dq(t=1, qs=configs): + for q in qs: + v(q) + sleep(t) + + +def projCom(state, c, qref): + s, succ = StateHelper.cloneState(state) + succCom = succ and fullBody.projectStateToCOM(s.sId, c, 0) + if succCom: + return s + # ~ else: + # ~ c[2] += 0.2 + # ~ v(qref) + for i in range(10): + s.setQ(qref) + succCom = succ and fullBody.projectStateToCOM(s.sId, c, 0) + c[2] -= 0.05 + if succCom: + return s + # ~ else: + print("fail to project com") + # ~ return state.q() + return state + + +def projectComPath(state, c0, c1, directPathId, fps=24.0): + l = ps.pathLength(directPathId) + f = l / float(fps) + frame = 0.0 + s = state + qs = [] + ac0 = array(c0) + ac1 = array(c1) + # ~ print "c0", c0 + # ~ print "c1", c1 + while frame < l: + # ~ print "frame", frame + c = (ac0 + frame * (ac1 - ac0)).tolist() + # ~ print "c", c + frame += f + s = projCom(s, c, ps.configAtParam(directPathId, frame)) + qs += [s.q()] + return qs + + +def nil(): + global paths + global stateTrajs + global comTrajs + # ~ for j in range(2,len(all_states)-2,2): + for j in range(0, len(all_states), 2): + # ~ for j in range(4,10,2): + # ~ for j in range(4,50,2): + print("state ", j) + pathId = fullBody.limbRRT(all_states[j].sId, all_states[j + 1].sId) + # ~ print "state ", j + # ~ pathId2 = fullBody.limbRRT(all_states[j+1].sId, all_states[j+2].sId) + # ~ print "state ", j + paths += [(pathId, ps.pathLength(pathId))] + # ~ print "j", j + if j + 2 < len(all_states): + nPid = ps.directPath(all_states[j + 1].q(), all_states[j + 2].q(), False)[1] + paths += [(nPid, ps.pathLength(nPid))] + + # ~ print "coms j", all_states[j].getCenterOfMass() + # ~ print "coms j +1", all_states[j+1].getCenterOfMass() + # ~ print "coms j +2", all_states[j+2].getCenterOfMass() + + p0 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j].getCenterOfMass(), + ) + p1 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j + 1].getCenterOfMass(), + ) + p2 = comTraj( + all_states[j + 1].getCenterOfMass(), + all_states[j + 2].getCenterOfMass(), + ) + # ~ qs = projectComPath(all_states[j+1],all_states[j+1].getCenterOfMass(), all_states[j+2].getCenterOfMass(),nPid) + # ~ pp(pathId) + # ~ pp(pathId2) + # ~ dq(1./24., qs) + comTrajs += [(p0, p1, p2)] + stateTrajs += [(all_states[j].sId, all_states[j + 2].sId)] + # ~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] + # ~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] + # ~ if j > 30: + # ~ play_int() + # ~ for stateTraj, comTtraj in zip(stateTrajs,comTrajs): + # ~ paths+=[fullBody.effectorRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] + # ~ pp(int(paths[-1])) + + +def comrrt(): + global paths + global stateTrajs + global comTrajs + # ~ for j in range(2,len(all_states)-2,2): + for j in range(0, len(all_states), 2): + # ~ print "state ", j + # ~ pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) + # ~ paths += [(pathId, ps.pathLength(pathId))] + # ~ print "j", j + if j + 2 < len(all_states): + # ~ nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] + # ~ paths += [(nPid, ps.pathLength(nPid))] + + p0 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j].getCenterOfMass(), + ) + p1 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j + 1].getCenterOfMass(), + ) + p2 = comTraj( + all_states[j + 1].getCenterOfMass(), + all_states[j + 2].getCenterOfMass(), + ) + comTrajs += [(p0, p1, p2)] + stateTrajs += [(all_states[j].sId, all_states[j + 2].sId)] + # ~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] + # ~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] + # ~ play_int() + for stateTraj, comTtraj in zip(stateTrajs, comTrajs): + paths += [ + fullBody.comRRTFromPosBetweenState( + stateTraj[0], + stateTraj[1], + comTtraj[0], + comTtraj[1], + comTtraj[2], + )[-1] + ] + pp(int(paths[-1])) + + +# ~ nil() + +# ~ comrrt() + +# ~ play_int() +# ~ nil() +# ~ beginId = 0 diff --git a/sl1m/planner_scenarios/lp_complex1.py b/sl1m/planner_scenarios/hrp2/path_files/lp_complex1.py similarity index 64% rename from sl1m/planner_scenarios/lp_complex1.py rename to sl1m/planner_scenarios/hrp2/path_files/lp_complex1.py index 0b31264..8878658 100644 --- a/sl1m/planner_scenarios/lp_complex1.py +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_complex1.py @@ -2,75 +2,110 @@ from hpp.gepetto import Viewer from tools.display_tools import * import time + print("Plan guide trajectory ...") from . import lp_complex_path as tp + print("Done.") import time + DEFAULT_COM_HEIGHT = 0.72 -pId = tp.ps.numberPaths() -1 -fullBody = Robot () +pId = tp.ps.numberPaths() - 1 +fullBody = Robot() # Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-10.135,2, -20, 20, 0, 2.8]) +fullBody.setJointBounds("root_joint", [-10.135, 2, -20, 20, 0, 2.8]) fullBody.client.robot.setDimensionExtraConfigSpace(6) -fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0]) +fullBody.client.robot.setExtraConfigSpaceBounds([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) -ps = tp.ProblemSolver( fullBody ) +ps = tp.ProblemSolver(fullBody) -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True) +# load the viewer +v = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True) # load a reference configuration -q_ref = fullBody.referenceConfig[::] + [0]*6 -q_init = q_ref[::] +q_ref = fullBody.referenceConfig[::] + [0] * 6 +q_init = q_ref[::] fullBody.setReferenceConfig(q_ref) -fullBody.setCurrentConfig (q_init) -fullBody.setPostureWeights(fullBody.postureWeights[::] + [0]*6) -#fullBody.usePosturalTaskContactCreation(True) +fullBody.setCurrentConfig(q_init) +fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6) +# fullBody.usePosturalTaskContactCreation(True) print("Generate limb DB ...") tStart = time.time() -# generate databases : +# generate databases : nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) +fullBody.addLimb( + fullBody.rLegId, + fullBody.rleg, + fullBody.rfoot, + fullBody.rLegOffset, + fullBody.rLegNormal, + fullBody.rLegx, + fullBody.rLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.rLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) +fullBody.addLimb( + fullBody.lLegId, + fullBody.lleg, + fullBody.lfoot, + fullBody.lLegOffset, + fullBody.rLegNormal, + fullBody.lLegx, + fullBody.lLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.lLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) -tGenerate = time.time() - tStart +tGenerate = time.time() - tStart print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") +print("Databases generated in : " + str(tGenerate) + " s") -#define initial and final configurations : -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() +# define initial and final configurations : +configSize = ( + fullBody.getConfigSize() - fullBody.client.robot.getDimensionExtraConfigSpace() +) - -#~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation +# ~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation q_init[:3] = [0.1, -0.82, 0.648702] v(q_init) -#fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm import rbprmstate -from hpp.corbaserver.rbprm import state_alg +# fullBody.resetJointsBounds() +from hpp.corbaserver.rbprm import rbprmstate +from hpp.corbaserver.rbprm import state_alg + -def getContactsFromConfig(q, limbs = [Robot.rLegId, Robot.lLegId]): - s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs) +def getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]): + s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs) rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) - return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) } - -#~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) + return { + Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]), + Robot.lLegId: (lLegPn[0][0], lLegPn[1][0]), + } + + +# ~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) from sl1m.planner_scenarios.complex import * from hpp.corbaserver.rbprm.rbprmstate import StateHelper -pb, coms, footpos, allfeetpos, res = solve() +pb, coms, footpos, allfeetpos, res = solve() def computeCenterOfSupportPolygon(s): @@ -79,98 +114,126 @@ def computeCenterOfSupportPolygon(s): for limbId in s.getLimbsInContact(): com += np.array(s.getCenterOfContactForLimb(limbId)[0]) com /= numContacts - com[2] += DEFAULT_COM_HEIGHT + com[2] += DEFAULT_COM_HEIGHT return com.tolist() + def projectCoMInSupportPolygon(s): desiredCOM = computeCenterOfSupportPolygon(s) - #print "try to project state to com position : ",desiredCOM + # print "try to project state to com position : ",desiredCOM success = False maxIt = 20 while not success and maxIt > 0: - success = s.fullBody.projectStateToCOM(s.sId ,desiredCOM, maxNumSample = 0) - maxIt -= 1 - desiredCOM[2] -= 0.005 + success = s.fullBody.projectStateToCOM(s.sId, desiredCOM, maxNumSample=0) + maxIt -= 1 + desiredCOM[2] -= 0.005 return success - -def gen_state(s, pId, com , num_max_sample = 0, first = False, normal = z, newContact = True ,projectCOM = True): - #~ pId = 6 - phase = pb["phaseData"][pId] + +def gen_state( + s, + pId, + com, + num_max_sample=0, + first=False, + normal=z, + newContact=True, + projectCOM=True, +): + # ~ pId = 6 + phase = pb["phaseData"][pId] moving = phase["moving"] movingID = fullBody.lLegId if moving == RF: movingID = fullBody.rLegId - print("# gen state for phase Id = ",pId) - #print "current config q=",s.q() - #print "move limb ",movingID - pos = allfeetpos[pId+2]; # +2 because it contains also the 2 feet pos at the init config + print("# gen state for phase Id = ", pId) + # print "current config q=",s.q() + # print "move limb ",movingID + pos = allfeetpos[pId + 2] + # +2 because it contains also the 2 feet pos at the init config pos[2] += 0.01 if newContact: - sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample) + sres, succ = StateHelper.addNewContact( + s, + movingID, + pos.tolist(), + normal.tolist(), + num_max_sample=num_max_sample, + ) else: sres, succ = StateHelper.cloneState(s) if not succ: - print("Cannot project config q = ",sres.q()) - print("To new contact position for "+movingID+" = "+str(pos.tolist())+" : n = "+str(normal.tolist())) - raise RuntimeError("Cannot project feet to new contact position") # try something else ?? - if projectCOM : - #print "config before projecting to com q1=",sres.q() + print("Cannot project config q = ", sres.q()) + print( + "To new contact position for " + + movingID + + " = " + + str(pos.tolist()) + + " : n = " + + str(normal.tolist()) + ) + raise RuntimeError( + "Cannot project feet to new contact position" + ) # try something else ?? + if projectCOM: + # print "config before projecting to com q1=",sres.q() successCOM = projectCoMInSupportPolygon(sres) if not successCOM: - # is it really an issue ? + # is it really an issue ? print("Unable to project CoM in the center of the support polygone") v(sres.q()) return sres - - - + + q = fullBody.getCurrentConfig() q[:3] = [-2.69, 0.21, 0.649702] v(q) -s = rbprmstate.State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId]) +s = rbprmstate.State(fullBody, q=q, limbsIncontact=[fullBody.lLegId, fullBody.rLegId]) idfirst = 2 coms[0] = array(s.getCenterOfMass()) -#~ coms[0] = coms[1] -#~ coms[1] = array(s.getCenterOfMass()) +# ~ coms[0] = coms[1] +# ~ coms[1] = array(s.getCenterOfMass()) + def normal(phase): s = phase["S"][0] - n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0]) + n = cross(s[:, 1] - s[:, 0], s[:, 2] - s[:, 0]) n /= norm(n) - if n[2] < 0.: + if n[2] < 0.0: for i in range(3): n[i] = -n[i] - #~ print "normal ", n + # ~ print "normal ", n return n - + + all_states = [s] sprev = s -for i in range(0, len(pb["phaseData"])): -#for i in range(0, 5): -#~ for i in range(0, 3): -#~ for i in range(2, 5): -#~ for i in range(2, 3): -#~ for i in range(1, 2): - #~ com = (coms[i-i] ).tolist() +for i in range(0, len(pb["phaseData"])): + # for i in range(0, 5): + # ~ for i in range(0, 3): + # ~ for i in range(2, 5): + # ~ for i in range(2, 3): + # ~ for i in range(1, 2): + # ~ com = (coms[i-i] ).tolist() com = (coms[i]).tolist() - #~ if i > 2: - #~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() - #get normal + # ~ if i > 2: + # ~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() + # get normal n = normal(pb["phaseData"][i]) - snew = gen_state(sprev, i , com, num_max_sample = 0, first = False, normal = n ) - all_states += [snew] + snew = gen_state(sprev, i, com, num_max_sample=0, first=False, normal=n) + all_states += [snew] sprev = snew - #~ com2 = coms[i+1].tolist() - #~ snew2 = gen_state(snew, i , com2, num_max_sample = 20, normal = n, newContact = False ) - #~ all_states += [snew2] - #~ sprev = snew2 - -#~ raise ValueError - -#~ all_states = all_states[:-1] -configs = [ st.q() for st in all_states[:]]; i = 0 + # ~ com2 = coms[i+1].tolist() + # ~ snew2 = gen_state(snew, i , com2, num_max_sample = 20, normal = n, newContact = False ) + # ~ all_states += [snew2] + # ~ sprev = snew2 + +# ~ raise ValueError + +# ~ all_states = all_states[:-1] +configs = [st.q() for st in all_states[:]] +i = 0 print("SID ", [s.sId for s in all_states]) diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path copy.py b/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path copy.py new file mode 100644 index 0000000..b8b0245 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path copy.py @@ -0,0 +1,174 @@ +from hpp.gepetto import PathPlayer, Viewer, ViewerFactory +from hpp.corbaserver import Client +from hpp.corbaserver.affordance.affordance import AffordanceTool +from hpp.corbaserver.problem_solver import ProblemSolver +from talos_rbprm.talos_abstract import Robot as TalosAbstract +import time + +TalosAbstract.urdfName += "_large" + + +def compute_path(): + talos_abstract = TalosAbstract() + talos_abstract.setJointBounds("root_joint", [-3.2, 1.8, 0.19, 0.21, 0.95, 1.7]) + # As this scenario only consider walking, we fix the DOF of the torso : + talos_abstract.setJointBounds("torso_1_joint", [0, 0]) + talos_abstract.setJointBounds("torso_2_joint", [0.0, 0.0]) + vMax = 1.0 # linear velocity bound for the root + aMax = 2.0 # linear acceleration bound for the root + extraDof = 6 + mu = 0.5 # coefficient of friction + talos_abstract.setFilter([TalosAbstract.rLegId, TalosAbstract.lLegId]) + + talos_abstract.setAffordanceFilter( + TalosAbstract.rLegId, + [ + "Support", + ], + ) + talos_abstract.setAffordanceFilter(TalosAbstract.lLegId, ["Support"]) + talos_abstract.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) + # Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root + talos_abstract.client.robot.setDimensionExtraConfigSpace(extraDof) + # We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) + extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, + ] + talos_abstract.client.robot.setExtraConfigSpaceBounds(extraDofBounds) + ( + talos_abstract.getConfigSize() + - talos_abstract.client.robot.getDimensionExtraConfigSpace() + ) + + ps = ProblemSolver(talos_abstract) + vf = ViewerFactory(ps) + + afftool = AffordanceTool() + afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) + afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/bauzil_ramp_simplified.urdf", + "planning", + vf, + reduceSizes=[0.07, 0.0, 0.0], + ) + v = vf.createViewer(displayArrows=True) + afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) + v.addLandmark(v.sceneName, 1) + + ps.setParameter("Kinodynamic/velocityBound", vMax) + ps.setParameter("Kinodynamic/accelerationBound", aMax) + # force the orientation of the trunk to match the direction of the motion + ps.setParameter("Kinodynamic/forceYawOrientation", True) + ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) + ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) + ps.setParameter("DynamicPlanner/sizeFootX", 0.2) + ps.setParameter("DynamicPlanner/sizeFootY", 0.12) + ps.setParameter("DynamicPlanner/friction", mu) + # sample only configuration with null velocity and acceleration : + ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) + ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) + # Choosing RBPRM shooter and path validation methods. + ps.selectConfigurationShooter("RbprmShooter") + ps.addPathOptimizer("RandomShortcutDynamic") + ps.selectPathValidation("RbprmPathValidation", 0.05) + # Choosing kinodynamic methods : + ps.selectSteeringMethod("RBPRMKinodynamic") + ps.selectDistance("Kinodynamic") + ps.selectPathPlanner("DynamicPlanner") + + ### BEGIN climb the stairs ##### + ps.setParameter("Kinodynamic/velocityBound", 0.3) + ps.setParameter("Kinodynamic/accelerationBound", 0.1) + q_init = talos_abstract.getCurrentConfig() + q_init[0:3] = [-0.3, 0.2, 0.98] + v(q_init) # between rubble and stairs + q_goal = q_init[::] + q_goal[0:3] = [1.7, 0.2, 1.58] + v(q_goal) # top of stairs + q_goal[-6:-3] = [0, 0, 0] + ps.setInitialConfig(q_init) + q_init_0 = q_init[::] + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_stairs = ps.numberPaths() - 1 + ### END climb the stairs ##### + talos_abstract.setJointBounds("root_joint", [-3.2, 2.5, -0.8, 0.3, 1.4, 2.0]) + ps.resetGoalConfigs() + ### BEGIN turn around on the platform ##### + ps.setParameter("Kinodynamic/velocityBound", 0.2) + ps.setParameter("Kinodynamic/accelerationBound", 0.07) + q_init = talos_abstract.getCurrentConfig() + q_init = q_goal[::] + v(q_init) # top of stairs + q_init[-6:-3] = [0.2, 0, 0] + q_goal = q_init[::] + q_goal[0:3] = [1.7, -0.6, 1.58] + v(q_goal) # before bridge + q_goal[3:7] = [0, 0, 1, 0] + q_goal[-6:-3] = [-0.2, 0, 0] + ps.setInitialConfig(q_init) + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_platform = ps.numberPaths() - 1 + ### END turn around on the platform ##### + ps.resetGoalConfigs() + ### BEGIN bridge cross ##### + ps.setParameter("Kinodynamic/velocityBound", 0.3) + ps.setParameter("Kinodynamic/accelerationBound", 0.2) + q_init = talos_abstract.getCurrentConfig() + q_init = q_goal[::] + v(q_init) # top of stairs + q_goal[0:3] = [-1.7, -0.6, 1.58] + v(q_goal) # after bridge + q_goal[3:7] = [0, 0, 1, 0] + q_goal[-6:-3] = [0.0, 0, 0] + ps.setInitialConfig(q_init) + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_bridge = ps.numberPaths() - 1 + pathId = pId_stairs + ps.concatenatePath(pathId, pId_platform) + ps.concatenatePath(pathId, pId_bridge) + + print("done optimizing.") + pp = PathPlayer(v) + pp.dt = 0.1 + pp.displayVelocityPath(pathId) + v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") + pp.dt = 0.01 + + q_far = q_goal[::] + q_far[2] = -5 + v(q_far) + q_init = q_init_0[::] + + return talos_abstract, ps, afftool, pathId, v, q_init diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path.py b/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path.py new file mode 100644 index 0000000..68a1d70 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_complex_path.py @@ -0,0 +1,52 @@ +from hpp.gepetto import PathPlayer +from hpp.corbaserver.affordance.affordance import AffordanceTool +from hpp.corbaserver.problem_solver import ProblemSolver +from hpp.corbaserver.rbprm.rbprmbuilder import Builder +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from hpp.corbaserver.robot import Robot as Parent + +from hpp.corbaserver.rbprm.hrp2_abstract import Robot + +packageName = "hpp_environments" +meshPackageName = "hpp_environments" + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-20, 20, -10, 10, 0, 2.2]) +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-0.0, 0, -1, 1, -1, 1]) + +ps = ProblemSolver(rbprmBuilder) +r = Viewer(ps) + +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-1, -0.82, 0.5] +rbprmBuilder.setCurrentConfig(q_init) +r(q_init) + +q_goal = q_init[::] +q_goal[3:7] = [0.0, 0.0, 0.14943813, 0.98877108] +q_goal[0:3] = [100.49, -0.65, 1.2] +r(q_goal) +ps.addPathOptimizer("RandomShortcut") +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +r(q_goal) + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel(packageName, "multicontact/bauzil_ramp", "planning", r) +afftool.visualiseAffordances("Support", r, [0.25, 0.5, 0.5]) + +ps.client.problem.selectConfigurationShooter("RbprmShooter") +ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) + +pp = PathPlayer(r) diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil.py b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil.py new file mode 100644 index 0000000..848b4c4 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil.py @@ -0,0 +1,243 @@ +from hpp.corbaserver.rbprm.hrp2 import Robot +from hpp.gepetto import Viewer +from tools.display_tools import * +import time + +print("Plan guide trajectory ...") +from . import lp_stair_bauzil_hrp2_path as tp + +print("Done.") +import time + +pId = tp.ps.numberPaths() - 1 +fullBody = Robot() + +# Set the bounds for the root +fullBody.setJointBounds("root_joint", [-10.135, 2, -2, 2, 0, 2.2]) +# fullBody.setConstrainedJointsBounds() +# fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) +# fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) +# add the 6 extraDof for velocity and acceleration (see *_path.py script) +fullBody.client.robot.setDimensionExtraConfigSpace(0) +# ~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) +ps = tp.ProblemSolver(fullBody) +# ~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) +# ~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) +# load the viewer +v = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True) + +# load a reference configuration +# ~ q_ref = fullBody.referenceConfig[::]+[0]*6 +q_ref = fullBody.referenceConfig[::] +q_init = q_ref[::] +fullBody.setReferenceConfig(q_ref) +fullBody.setCurrentConfig(q_init) +fullBody.setPostureWeights(fullBody.postureWeights[::]) +# ~ fullBody.setPostureWeights(fullBody.postureWeights[::]) +# fullBody.usePosturalTaskContactCreation(True) + +print("Generate limb DB ...") +tStart = time.time() +# generate databases : + +nbSamples = 10000 +fullBody.addLimb( + fullBody.rLegId, + fullBody.rleg, + fullBody.rfoot, + fullBody.rLegOffset, + fullBody.rLegNormal, + fullBody.rLegx, + fullBody.rLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.rLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) +fullBody.addLimb( + fullBody.lLegId, + fullBody.lleg, + fullBody.lfoot, + fullBody.lLegOffset, + fullBody.rLegNormal, + fullBody.lLegx, + fullBody.lLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.lLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) + + +tGenerate = time.time() - tStart +print("Done.") +print("Databases generated in : " + str(tGenerate) + " s") + +# define initial and final configurations : +configSize = ( + fullBody.getConfigSize() - fullBody.client.robot.getDimensionExtraConfigSpace() +) + + +# ~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation +q_init[:3] = [0.1, -0.82, 0.648702] +q_init[7:] = [ + 0.0, + 0.0, + 0.0, + 0.0, # CHEST HEAD 7-10 + 0.261799388, + 0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # LARM 11-17 + 0.261799388, + -0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # RARM 18-24 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # LLEG 25-30 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # RLEG 31-36 +] +v(q_init) + +# fullBody.resetJointsBounds() +from hpp.corbaserver.rbprm import rbprmstate +from hpp.corbaserver.rbprm import state_alg + + +def getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]): + s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs) + rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) + lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) + return { + Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]), + Robot.lLegId: (lLegPn[0][0], lLegPn[1][0]), + } + + +# ~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) + + +from sl1m.planner_scenarios.escaliers import * + +pb, coms, footpos, allfeetpos, res = solve() + + +def gen_state(s, pId, com, num_max_sample=1, first=False): + # ~ pId = 6 + phase = pb["phaseData"][pId] + moving = phase["moving"] + movingID = fullBody.lLegId + if moving == RF: + movingID = fullBody.rLegId + pos = allfeetpos[pId] + pos[2] += 0.01 + com[2] += 0.5 + print("com", com) + print("pos", pos.tolist()) + # ~ q = fullBody.getCurrentConfig() + # ~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) + sres, succ = state_alg.addNewContact( + s, movingID, pos.tolist(), z.tolist(), num_max_sample=num_max_sample + ) + print("succes ?", succ) + succCom = False + ite = 0 + # ~ if first: + # ~ print "FIRST " + # ~ com[2] -= 0.25 + # ~ while(not succCom and ite < 11): + while not succCom and ite < 17: + succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) + com[2] -= 0.05 + ite += 1 + print("COM?", succCom) + v(sres.q()) + return sres + + +q = fullBody.getCurrentConfig() +q[:2] = allfeetpos[1][:2].tolist() +v(q) +s = rbprmstate.State(fullBody, q=q, limbsIncontact=[]) +all_states = [s] +sprev = s +idfirst = 2 + +for i in range(1, len(pb["phaseData"])): + com = (coms[i - i]).tolist() + # ~ if i > 2: + # ~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() + snew = gen_state(sprev, i, com, num_max_sample=200, first=i == idfirst) + all_states += [snew] + sprev = snew + com2 = coms[i].tolist() + snew2 = gen_state(snew, i, com2, num_max_sample=2) + all_states += [snew2] + sprev = snew2 + +all_states = all_states[4:-1] +configs = [st.q() for st in all_states[:]] +i = 0 + +print("SID ", [s.sId for s in all_states]) + +beginId = 4 + +# ~ configs = configs[:-1] + +# ~ configs = [q1, q2, q3, q4, q5] +# ~ from cPickle import dump + +# ~ f = open('contacts_plateformes.txt','w') +# ~ dump(contacts,f) +# ~ f.close() +ax = draw_scene() +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=True) +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=False) + +paths = [] + + +def play_int(f_r=100): + for (pid, pl) in paths: + for i in range(f_r): + frame = float(i) / float(f_r) * pl + v((ps.configAtParam(pid, frame))) + + +# ~ raise ValueError + + +def nil(): + global paths + # ~ for j in range(2,len(all_states)-2,2): + for j in range(2, len(all_states) - 2, 1): + # ~ print "state ", j + pathId = fullBody.limbRRT(all_states[j].sId, all_states[j + 1].sId) + paths += [(pathId, ps.pathLength(pathId))] + play_int() + + +from hpp.gepetto import PathPlayer + +pp = PathPlayer(v) diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_10.py b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_10.py new file mode 100644 index 0000000..dfda339 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_10.py @@ -0,0 +1,343 @@ +from hpp.corbaserver.rbprm.hrp2 import Robot +from hpp.gepetto import Viewer +from tools.display_tools import * +import time + +print("Plan guide trajectory ...") +from . import lp_stair_bauzil_hrp2_path_10 as tp + +print("Done.") +import time + +pId = tp.ps.numberPaths() - 1 +fullBody = Robot() + +# Set the bounds for the root +fullBody.setJointBounds("root_joint", [-10.135, 2, -20, 20, 0, 2.8]) +# fullBody.setConstrainedJointsBounds() +# fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) +# fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) +# add the 6 extraDof for velocity and acceleration (see *_path.py script) +fullBody.client.robot.setDimensionExtraConfigSpace(0) +# ~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) +ps = tp.ProblemSolver(fullBody) +# ~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) +# ~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) +# load the viewer +v = tp.Viewer(ps, viewerClient=tp.r.client, displayCoM=True) + +# load a reference configuration +# ~ q_ref = fullBody.referenceConfig[::]+[0]*6 +q_ref = fullBody.referenceConfig[::] +q_init = q_ref[::] +fullBody.setReferenceConfig(q_ref) +fullBody.setCurrentConfig(q_init) +fullBody.setPostureWeights(fullBody.postureWeights[::]) +# ~ fullBody.setPostureWeights(fullBody.postureWeights[::]) +# fullBody.usePosturalTaskContactCreation(True) + +print("Generate limb DB ...") +tStart = time.time() +# generate databases : + +nbSamples = 10000 +fullBody.addLimb( + fullBody.rLegId, + fullBody.rleg, + fullBody.rfoot, + fullBody.rLegOffset, + fullBody.rLegNormal, + fullBody.rLegx, + fullBody.rLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.rLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) +fullBody.addLimb( + fullBody.lLegId, + fullBody.lleg, + fullBody.lfoot, + fullBody.lLegOffset, + fullBody.rLegNormal, + fullBody.lLegx, + fullBody.lLegy, + nbSamples, + "fixedStep1", + 0.01, + kinematicConstraintsPath=fullBody.lLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) + + +tGenerate = time.time() - tStart +print("Done.") +print("Databases generated in : " + str(tGenerate) + " s") + +# define initial and final configurations : +configSize = ( + fullBody.getConfigSize() - fullBody.client.robot.getDimensionExtraConfigSpace() +) + + +# ~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation +q_init[:3] = [0.1, -0.82, 0.648702] +q_init[7:] = [ + 0.0, + 0.0, + 0.0, + 0.0, # CHEST HEAD 7-10 + 0.261799388, + 0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # LARM 11-17 + 0.261799388, + -0.174532925, + 0.0, + -0.523598776, + 0.0, + 0.0, + 0.17, # RARM 18-24 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # LLEG 25-30 + 0.0, + 0.0, + -0.453785606, + 0.872664626, + -0.41887902, + 0.0, # RLEG 31-36 +] +v(q_init) + +# fullBody.resetJointsBounds() +from hpp.corbaserver.rbprm import rbprmstate +from hpp.corbaserver.rbprm import state_alg + + +def getContactsFromConfig(q, limbs=[Robot.rLegId, Robot.lLegId]): + s = rbprmstate.State(fullBody, q=q, limbsIncontact=limbs) + rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) + lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) + return { + Robot.rLegId: (rLegPn[0][0], rLegPn[1][0]), + Robot.lLegId: (lLegPn[0][0], lLegPn[1][0]), + } + + +# ~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) + + +from sl1m.planner_scenarios.stairs_10cm import * + +pb, coms, footpos, allfeetpos, res = solve() + + +def gen_state(s, pId, com, num_max_sample=1, first=False): + # ~ pId = 6 + phase = pb["phaseData"][pId] + moving = phase["moving"] + movingID = fullBody.lLegId + if moving == RF: + movingID = fullBody.rLegId + pos = allfeetpos[pId] + pos[2] += 0.01 + com[2] += 0.5 + print("com", com) + print("pos", pos.tolist()) + # ~ q = fullBody.getCurrentConfig() + # ~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) + sres, succ = state_alg.addNewContact( + s, movingID, pos.tolist(), z.tolist(), num_max_sample=0 + ) + print("succes ?", succ) + succCom = False + ite = 0 + # ~ if first: + # ~ print "FIRST " + # ~ com[2] -= 0.25 + # ~ while(not succCom and ite < 11): + while not succCom and ite < 17: + succCom = fullBody.projectStateToCOM(sres.sId, com, num_max_sample) + com[2] -= 0.05 + ite += 1 + print("COM?", succCom) + v(sres.q()) + return sres + + +q = fullBody.getCurrentConfig() +q[:2] = allfeetpos[0][:2].tolist() +v(q) +s = rbprmstate.State(fullBody, q=q, limbsIncontact=[]) +all_states = [s] +sprev = s +idfirst = 2 + +for i in range(1, len(pb["phaseData"])): + # ~ com = (coms[i-i] ).tolist() + com = (coms[i - 1]).tolist() + # ~ if i > 2: + # ~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() + snew = gen_state(sprev, i, com, num_max_sample=20, first=i == idfirst) + all_states += [snew] + sprev = snew + com2 = coms[i].tolist() + snew2 = gen_state(snew, i, com2, num_max_sample=20) + all_states += [snew2] + sprev = snew2 + +all_states = all_states[:-1] +configs = [st.q() for st in all_states[:]] +i = 0 + +print("SID ", [s.sId for s in all_states]) + +beginId = 4 + +# ~ configs = configs[:-1] + +# ~ configs = [q1, q2, q3, q4, q5] +# ~ from cPickle import dump + +# ~ f = open('contacts_plateformes.txt','w') +# ~ dump(contacts,f) +# ~ f.close() +ax = draw_scene() +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=True) +# ~ plotQPRes(pb, res, ax=ax, plot_constraints=False) + +paths = [] + + +def play_int(f_r=100): + for (pid, pl) in paths: + for i in range(f_r): + frame = float(i) / float(f_r) * pl + v((ps.configAtParam(pid, frame))) + + +# ~ def ppath(pid): +# ~ length = + +# ~ raise ValueError + + +comTrajs = [] +stateTrajs = [] + + +def comTraj(c0, c1, t=1.0): + zero = [0.0, 0.0, 0] + return fullBody.generateComTraj([c0, c1], [zero], [zero], 1.0) + + +from hpp.gepetto import PathPlayer + +pp = PathPlayer(v) + +from time import sleep + + +def dq(t=1): + for q in configs: + v(q) + sleep(t) + + +def nil(): + global paths + global stateTrajs + global comTrajs + # ~ for j in range(2,len(all_states)-2,2): + for j in range(4, len(all_states), 2): + # ~ print "state ", j + pathId = fullBody.limbRRT(all_states[j].sId, all_states[j + 1].sId) + paths += [(pathId, ps.pathLength(pathId))] + # ~ print "j", j + if j + 2 < len(all_states): + nPid = ps.directPath(all_states[j + 1].q(), all_states[j + 2].q(), False)[1] + paths += [(nPid, ps.pathLength(nPid))] + + p0 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j].getCenterOfMass(), + ) + p1 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j + 1].getCenterOfMass(), + ) + p2 = comTraj( + all_states[j + 1].getCenterOfMass(), + all_states[j + 2].getCenterOfMass(), + ) + comTrajs += [(p0, p1, p2)] + stateTrajs += [(all_states[j].sId, all_states[j + 2].sId)] + # ~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] + # ~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] + play_int() + # ~ for stateTraj, comTtraj in zip(stateTrajs,comTrajs): + # ~ paths+=[fullBody.effectorRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] + # ~ pp(int(paths[-1])) + + +def comrrt(): + global paths + global stateTrajs + global comTrajs + # ~ for j in range(2,len(all_states)-2,2): + for j in range(4, len(all_states), 2): + # ~ print "state ", j + # ~ pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) + # ~ paths += [(pathId, ps.pathLength(pathId))] + # ~ print "j", j + if j + 2 < len(all_states): + # ~ nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] + # ~ paths += [(nPid, ps.pathLength(nPid))] + + p0 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j].getCenterOfMass(), + ) + p1 = comTraj( + all_states[j].getCenterOfMass(), + all_states[j + 1].getCenterOfMass(), + ) + p2 = comTraj( + all_states[j + 1].getCenterOfMass(), + all_states[j + 2].getCenterOfMass(), + ) + comTrajs += [(p0, p1, p2)] + stateTrajs += [(all_states[j].sId, all_states[j + 2].sId)] + # ~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] + # ~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] + # ~ play_int() + for stateTraj, comTtraj in zip(stateTrajs, comTrajs): + paths += [ + fullBody.comRRTFromPosBetweenState( + stateTraj[0], + stateTraj[1], + comTtraj[0], + comTtraj[1], + comTtraj[2], + )[-1] + ] + pp(int(paths[-1])) + + +# ~ nil() + +# ~ comrrt() + +# ~ play_int() + +beginId = 4 diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path.py b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path.py new file mode 100644 index 0000000..0254a9a --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path.py @@ -0,0 +1,95 @@ +from hpp.corbaserver.rbprm.rbprmbuilder import Builder +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from hpp.corbaserver.robot import Robot as Parent + + +from hpp.corbaserver.rbprm.hrp2_abstract import Robot + +# ~ rootJointType = 'freeflyer' +packageName = "hpp-rbprm-corba" +meshPackageName = "hpp-rbprm-corba" +# ~ urdfName = 'hrp2_trunk_flexible' +# ~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] +# ~ urdfSuffix = "" +# ~ srdfSuffix = "" + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-2, 2, -1, 1, 0, 2.2]) +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-0.0, 0, -1, 1, -1, 1]) + +# ~ from hpp.corbaserver.rbprm. import ProblemSolver +from hpp.corbaserver.problem_solver import ProblemSolver + +ps = ProblemSolver(rbprmBuilder) +r = Viewer(ps) + + +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-1, -0.82, 0.5] +rbprmBuilder.setCurrentConfig(q_init) +r(q_init) + +q_goal = q_init[::] +q_goal[3:7] = [0.0, 0.0, 0.14943813, 0.98877108] +q_goal[0:3] = [1.49, -0.65, 1.2] +r(q_goal) +ps.addPathOptimizer("RandomShortcut") +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) + +from hpp.corbaserver.affordance.affordance import AffordanceTool + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel(packageName, "stair_bauzil", "planning", r) +afftool.visualiseAffordances("Support", r, [0.25, 0.5, 0.5]) + +ps.client.problem.selectConfigurationShooter("RbprmShooter") +ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) +t = ps.solve() + + +print(t) +# ~ if isinstance(t, list): +# ~ t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] +# ~ f = open('log.txt', 'a') +# ~ f.write("path computation " + str(t) + "\n") +# ~ f.close() + + +from hpp.gepetto import PathPlayer + +pp = PathPlayer(r) +# ~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") +# ~ + +for i in range(1, 10): + rbprmBuilder.client.problem.optimizePath(i) + +# ~ pp (10) +# ~ pp (0) + +# ~ pp (1) +# ~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") +# ~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt") + +# ~ cl = Client() +# ~ cl.problem.selectProblem("rbprm_path") +# ~ rbprmBuilder2 = Robot ("toto") +# ~ ps2 = ProblemSolver( rbprmBuilder2 ) +# ~ cl.problem.selectProblem("default") +# ~ cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames()) +# ~ r2 = Viewer (ps2, viewerClient=r.client) +# ~ r.client.gui.setVisibility("toto", "OFF") +# ~ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF") +# ~ r2(q_far) diff --git a/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path_10.py b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path_10.py new file mode 100644 index 0000000..16f2f10 --- /dev/null +++ b/sl1m/planner_scenarios/hrp2/path_files/lp_stair_bauzil_hrp2_path_10.py @@ -0,0 +1,67 @@ +from hpp.corbaserver.rbprm.rbprmbuilder import Builder +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from hpp.corbaserver.robot import Robot as Parent + + +from hpp.corbaserver.rbprm.hrp2_abstract import Robot + +# ~ rootJointType = 'freeflyer' +packageName = "hpp-rbprm-corba" +meshPackageName = "hpp-rbprm-corba" +# ~ urdfName = 'hrp2_trunk_flexible' +# ~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] +# ~ urdfSuffix = "" +# ~ srdfSuffix = "" + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-2, 2, -1, 1, 0, 2.2]) +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-0.0, 0, -1, 1, -1, 1]) + +# ~ from hpp.corbaserver.rbprm. import ProblemSolver +from hpp.corbaserver.problem_solver import ProblemSolver + +ps = ProblemSolver(rbprmBuilder) +r = Viewer(ps) + + +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-1, -0.82, 0.5] +rbprmBuilder.setCurrentConfig(q_init) +r(q_init) + +q_goal = q_init[::] +q_goal[3:7] = [0.0, 0.0, 0.14943813, 0.98877108] +q_goal[0:3] = [1.49, -0.65, 1.2] +r(q_goal) +ps.addPathOptimizer("RandomShortcut") +# ~ ps.setInitialConfig (q_init) +# ~ ps.addGoalConfig (q_goal) + +from hpp.corbaserver.affordance.affordance import AffordanceTool + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +# ~ afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r) +afftool.loadObstacleModel(packageName, "bauzil_stairs_10cm", "planning", r) +afftool.visualiseAffordances("Support", r, [0.25, 0.5, 0.5]) + +ps.client.problem.selectConfigurationShooter("RbprmShooter") +ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) +# ~ t = ps.solve () + + +from hpp.gepetto import PathPlayer + +pp = PathPlayer(r) +# ~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") +# ~ diff --git a/sl1m/planner_scenarios/lp_complex.py b/sl1m/planner_scenarios/lp_complex.py deleted file mode 100644 index 4843d7b..0000000 --- a/sl1m/planner_scenarios/lp_complex.py +++ /dev/null @@ -1,337 +0,0 @@ -from hpp.corbaserver.rbprm.hrp2 import Robot -from hpp.gepetto import Viewer -from tools.display_tools import * -import time -print("Plan guide trajectory ...") -from . import lp_complex_path as tp -print("Done.") -import time - -pId = tp.ps.numberPaths() -1 -fullBody = Robot () - -# Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-10.135,2, -20, 20, 0, 2.8]) -#fullBody.setConstrainedJointsBounds() -#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) -#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) -# add the 6 extraDof for velocity and acceleration (see *_path.py script) -fullBody.client.robot.setDimensionExtraConfigSpace(0) -#~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) -ps = tp.ProblemSolver( fullBody ) -#~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) -#~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True) - -# load a reference configuration -#~ q_ref = fullBody.referenceConfig[::]+[0]*6 -q_ref = fullBody.referenceConfig[::] -q_init = q_ref[::] -fullBody.setReferenceConfig(q_ref) -fullBody.setCurrentConfig (q_init) -fullBody.setPostureWeights(fullBody.postureWeights[::]) -#~ fullBody.setPostureWeights(fullBody.postureWeights[::]) -#fullBody.usePosturalTaskContactCreation(True) - -print("Generate limb DB ...") -tStart = time.time() -# generate databases : - -nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) - - -tGenerate = time.time() - tStart -print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") - -#define initial and final configurations : -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() - - - -#~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation -q_init[:3] = [0.1, -0.82, 0.648702] -q_init[7:] = [ 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 - 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17 - 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 - ]; -v(q_init) - -#fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm import rbprmstate -from hpp.corbaserver.rbprm import state_alg - -def getContactsFromConfig(q, limbs = [Robot.rLegId, Robot.lLegId]): - s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs) - rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) - lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) - return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) } - -#~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) - - -from sl1m.planner_scenarios.complex import * - -from hpp.corbaserver.rbprm.rbprmstate import StateHelper - -pb, coms, footpos, allfeetpos, res = solve() - -def gen_state(s, pId, com , num_max_sample = 1, first = False, normal = z, newContact = True ): - #~ pId = 6 - phase = pb["phaseData"][pId] - moving = phase["moving"] - movingID = fullBody.lLegId - if moving == RF: - movingID = fullBody.rLegId - pos = allfeetpos[pId]; pos[2]+=0.01 - if not first: - com[2] += 0.5 - #~ com[2] += 0.4 - #~ print "com" , com - #~ print "pos" , pos.tolist() - #~ q = fullBody.getCurrentConfig() - #~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) - if newContact: - sres, succ = state_alg.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= 200) - else: - sres, succ = StateHelper.cloneState(s) - if not succ: - print("succes ?", succ) - succCom = False - ite = 0 - #~ if first: - #~ print "FIRST " - #~ com[2] -= 0.25 - #~ while(not succCom and ite < 11): - while(not succCom and ite < 30): - succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample) - com[2] -= 0.05 - ite += 1 - if succCom: - q = sres.q() - q[3:7] = [0.,0.,0.,1.] - q[3:] = fullBody.referenceConfig[3:] - sres.setQ(q) - succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample) - if not succCom: - print("refail") - v(sres.q()) - return sres - - - -q = fullBody.getCurrentConfig() -q[:3] = [-2.69, 0.24, 0.649702] -v(q) -s = rbprmstate.State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId]) - -idfirst = 2 -coms[0] = array(s.getCenterOfMass()) -#~ coms[0] = coms[1] -#~ coms[1] = array(s.getCenterOfMass()) - -def normal(phase): - s = phase["S"][0] - n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0]) - n /= norm(n) - if n[2] < 0.: - for i in range(3): - n[i] = -n[i] - #~ print "normal ", n - return n - -all_states = [s] -sprev = s -#~ for i in range(0, len(pb["phaseData"])-1): -for i in range(0, 5): -#~ for i in range(0, 3): -#~ for i in range(2, 5): -#~ for i in range(2, 3): -#~ for i in range(1, 2): - #~ com = (coms[i-i] ).tolist() - com = (coms[i]).tolist() - #~ if i > 2: - #~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() - #get normal - n = normal(pb["phaseData"][i]) - snew = gen_state(sprev, i+2 , com, num_max_sample = 200, first = False, normal = n ) - all_states += [snew] - sprev = snew - #~ com2 = coms[i+1].tolist() - #~ snew2 = gen_state(snew, i , com2, num_max_sample = 20, normal = n, newContact = False ) - #~ all_states += [snew2] - #~ sprev = snew2 - -#~ raise ValueError - -#~ all_states = all_states[:-1] -extraDof = [0 for _ in range(6)] -configs = [ st.q() + extraDof for st in all_states[:]]; i = 0 - -print("SID ", [s.sId for s in all_states]) - -beginId = 0 - -#~ configs = configs[:-1] - -#~ configs = [q1, q2, q3, q4, q5] -#~ from cPickle import dump - -#~ f = open('contacts_plateformes.txt','w') -#~ dump(contacts,f) -#~ f.close() -ax = draw_scene() - #~ plotQPRes(pb, res, ax=ax, plot_constraints=True) -#~ plotQPRes(pb, res, ax=ax, plot_constraints=False) - -paths = [] - -def play_int(f_r = 100): - for (pid, pl) in paths: - for i in range(f_r): - frame = float(i) / float(f_r) * pl - v((ps.configAtParam(pid,frame))) - - -#~ def ppath(pid): - #~ length = - -#~ raise ValueError - - -comTrajs = [] -stateTrajs = [] - -def comTraj(c0, c1, t = 1.): - zero = [0.,0.,0] - return fullBody.generateComTraj([c0,c1],[zero],[zero],1.) - - -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) - -from time import sleep -from hpp.corbaserver.rbprm.rbprmstate import StateHelper - -def dq(t = 1, qs = configs): - for q in qs: - v(q); sleep(t) - -def projCom(state, c, qref): - s, succ = StateHelper.cloneState(state) - succCom = succ and fullBody.projectStateToCOM(s.sId ,c, 0) - if succCom: - return s - #~ else: - #~ c[2] += 0.2 - #~ v(qref) - for i in range(10): - s.setQ(qref) - succCom = succ and fullBody.projectStateToCOM(s.sId ,c, 0) - c[2] -= .05 - if succCom: - return s - #~ else: - print("fail to project com") - #~ return state.q() - return state - -def projectComPath(state,c0,c1,directPathId, fps = 24.): - l = ps.pathLength(directPathId) - f = l/float(fps) - frame = 0. - s = state - qs = [] - ac0 = array(c0) - ac1 = array(c1) - #~ print "c0", c0 - #~ print "c1", c1 - while frame < l: - #~ print "frame", frame - c = (ac0 + frame * (ac1 - ac0)).tolist() - #~ print "c", c - frame += f - s = projCom(s, c, ps.configAtParam(directPathId,frame)) - qs += [s.q()] - return qs - -def nil(): - global paths - global stateTrajs - global comTrajs - #~ for j in range(2,len(all_states)-2,2): - for j in range(0,len(all_states),2): - #~ for j in range(4,10,2): - #~ for j in range(4,50,2): - print("state ", j) - pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) - #~ print "state ", j - #~ pathId2 = fullBody.limbRRT(all_states[j+1].sId, all_states[j+2].sId) - #~ print "state ", j - paths += [(pathId, ps.pathLength(pathId))] - #~ print "j", j - if j+2 < len(all_states): - nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] - paths += [(nPid, ps.pathLength(nPid))] - - #~ print "coms j", all_states[j].getCenterOfMass() - #~ print "coms j +1", all_states[j+1].getCenterOfMass() - #~ print "coms j +2", all_states[j+2].getCenterOfMass() - - p0 = comTraj(all_states[j].getCenterOfMass(),all_states[j].getCenterOfMass()) - p1 = comTraj(all_states[j].getCenterOfMass(),all_states[j+1].getCenterOfMass()) - p2 = comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass()) - #~ qs = projectComPath(all_states[j+1],all_states[j+1].getCenterOfMass(), all_states[j+2].getCenterOfMass(),nPid) - #~ pp(pathId) - #~ pp(pathId2) - #~ dq(1./24., qs) - comTrajs += [(p0,p1,p2)] - stateTrajs += [(all_states[j].sId, all_states[j+2].sId)] - #~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] - #~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] - #~ if j > 30: - #~ play_int() - #~ for stateTraj, comTtraj in zip(stateTrajs,comTrajs): - #~ paths+=[fullBody.effectorRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] - #~ pp(int(paths[-1])) - -def comrrt(): - global paths - global stateTrajs - global comTrajs - #~ for j in range(2,len(all_states)-2,2): - for j in range(0,len(all_states),2): - #~ print "state ", j - #~ pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) - #~ paths += [(pathId, ps.pathLength(pathId))] - #~ print "j", j - if j+2 < len(all_states): - #~ nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] - #~ paths += [(nPid, ps.pathLength(nPid))] - - p0 = comTraj(all_states[j].getCenterOfMass(),all_states[j].getCenterOfMass()) - p1 = comTraj(all_states[j].getCenterOfMass(),all_states[j+1].getCenterOfMass()) - p2 = comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass()) - comTrajs += [(p0,p1,p2)] - stateTrajs += [(all_states[j].sId, all_states[j+2].sId)] - #~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] - #~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] - #~ play_int() - for stateTraj, comTtraj in zip(stateTrajs,comTrajs): - paths+=[fullBody.comRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] - pp(int(paths[-1])) - -#~ nil() - -#~ comrrt() - -#~ play_int() -#~ nil() -#~ beginId = 0 diff --git a/sl1m/planner_scenarios/lp_complex_path.py b/sl1m/planner_scenarios/lp_complex_path.py deleted file mode 100644 index 9cae289..0000000 --- a/sl1m/planner_scenarios/lp_complex_path.py +++ /dev/null @@ -1,88 +0,0 @@ -from hpp.corbaserver.rbprm.rbprmbuilder import Builder -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.robot import Robot as Parent - - -from hpp.corbaserver.rbprm.hrp2_abstract import Robot - -#~ rootJointType = 'freeflyer' -#~ packageName = 'hpp-rbprm-corba' -#~ meshPackageName = 'hpp-rbprm-corba' -packageName = 'hpp_environments' -meshPackageName = 'hpp_environments' -#~ urdfName = 'hrp2_trunk_flexible' -#~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] -#~ urdfSuffix = "" -#~ srdfSuffix = "" - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-20,20, -10, 10, 0, 2.2]) -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1]) - -#~ from hpp.corbaserver.rbprm. import ProblemSolver -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -r = Viewer (ps) - - -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-1, -0.82, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) - -q_goal = q_init [::] -q_goal [3:7] = [ 0., 0. , 0.14943813, 0.98877108 ] -q_goal [0:3] = [100.49, -0.65, 1.2]; r (q_goal) -ps.addPathOptimizer("RandomShortcut") -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -r(q_goal) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "multicontact/bauzil_ramp", "planning", r) -afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) - -ps.client.problem.selectConfigurationShooter("RbprmShooter") -ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) -#~ t = ps.solve () - - - -#~ print t; -#~ if isinstance(t, list): - #~ t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] -#~ f = open('log.txt', 'a') -#~ f.write("path computation " + str(t) + "\n") -#~ f.close() - - -from hpp.gepetto import PathPlayer -pp = PathPlayer (r) -#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") -#~ - -#~ for i in range(1,10): - #~ rbprmBuilder.client.problem.optimizePath(i) - -#~ pp (10) -#~ pp (0) - -#~ pp (1) -#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") -#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt") - -#~ cl = Client() -#~ cl.problem.selectProblem("rbprm_path") -#~ rbprmBuilder2 = Robot ("toto") -#~ ps2 = ProblemSolver( rbprmBuilder2 ) -#~ cl.problem.selectProblem("default") -#~ cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames()) -#~ r2 = Viewer (ps2, viewerClient=r.client) -#~ r.client.gui.setVisibility("toto", "OFF") -#~ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF") -#~ r2(q_far) diff --git a/sl1m/planner_scenarios/lp_stair_bauzil.py b/sl1m/planner_scenarios/lp_stair_bauzil.py deleted file mode 100644 index ad923e5..0000000 --- a/sl1m/planner_scenarios/lp_stair_bauzil.py +++ /dev/null @@ -1,174 +0,0 @@ -from hpp.corbaserver.rbprm.hrp2 import Robot -from hpp.gepetto import Viewer -from tools.display_tools import * -import time -print("Plan guide trajectory ...") -from . import lp_stair_bauzil_hrp2_path as tp -print("Done.") -import time - -pId = tp.ps.numberPaths() -1 -fullBody = Robot () - -# Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-10.135,2, -2, 2, 0, 2.2]) -#fullBody.setConstrainedJointsBounds() -#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) -#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) -# add the 6 extraDof for velocity and acceleration (see *_path.py script) -fullBody.client.robot.setDimensionExtraConfigSpace(0) -#~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) -ps = tp.ProblemSolver( fullBody ) -#~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) -#~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True) - -# load a reference configuration -#~ q_ref = fullBody.referenceConfig[::]+[0]*6 -q_ref = fullBody.referenceConfig[::] -q_init = q_ref[::] -fullBody.setReferenceConfig(q_ref) -fullBody.setCurrentConfig (q_init) -fullBody.setPostureWeights(fullBody.postureWeights[::]) -#~ fullBody.setPostureWeights(fullBody.postureWeights[::]) -#fullBody.usePosturalTaskContactCreation(True) - -print("Generate limb DB ...") -tStart = time.time() -# generate databases : - -nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) - - -tGenerate = time.time() - tStart -print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") - -#define initial and final configurations : -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() - - - -#~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation -q_init[:3] = [0.1, -0.82, 0.648702] -q_init[7:] = [ 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 - 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17 - 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 - ]; -v(q_init) - -#fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm import rbprmstate -from hpp.corbaserver.rbprm import state_alg - -def getContactsFromConfig(q, limbs = [Robot.rLegId, Robot.lLegId]): - s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs) - rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) - lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) - return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) } - -#~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) - - -from sl1m.planner_scenarios.escaliers import * - -pb, coms, footpos, allfeetpos, res = solve() - -def gen_state(s, pId, com , num_max_sample = 1, first = False ): - #~ pId = 6 - phase = pb["phaseData"][pId] - moving = phase["moving"] - movingID = fullBody.lLegId - if moving == RF: - movingID = fullBody.rLegId - pos = allfeetpos[pId]; pos[2]+=0.01 - com[2] += 0.5 - print("com" , com) - print("pos" , pos.tolist()) - #~ q = fullBody.getCurrentConfig() - #~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) - sres, succ = state_alg.addNewContact(s, movingID, pos.tolist(), z.tolist(), num_max_sample= num_max_sample) - print("succes ?", succ) - succCom = False - ite = 0 - #~ if first: - #~ print "FIRST " - #~ com[2] -= 0.25 - #~ while(not succCom and ite < 11): - while(not succCom and ite < 17): - succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample) - com[2] -= 0.05 - ite += 1 - print("COM?", succCom) - v(sres.q()) - return sres - - -q = fullBody.getCurrentConfig() -q[:2] = allfeetpos[1][:2].tolist() -v(q) -s = rbprmstate.State(fullBody, q = q, limbsIncontact = []) -all_states = [s] -sprev = s -idfirst = 2 - -for i in range(1, len(pb["phaseData"])): - com = (coms[i-i] ).tolist() - #~ if i > 2: - #~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() - snew = gen_state(sprev, i , com, num_max_sample = 200, first = i == idfirst ) - all_states += [snew] - sprev = snew - com2 = coms[i].tolist() - snew2 = gen_state(snew, i , com2, num_max_sample = 2 ) - all_states += [snew2] - sprev = snew2 - -all_states = all_states[4:-1] -configs = [ st.q() for st in all_states[:]]; i = 0 - -print("SID ", [s.sId for s in all_states]) - -beginId = 4 - -#~ configs = configs[:-1] - -#~ configs = [q1, q2, q3, q4, q5] -#~ from cPickle import dump - -#~ f = open('contacts_plateformes.txt','w') -#~ dump(contacts,f) -#~ f.close() -ax = draw_scene() - #~ plotQPRes(pb, res, ax=ax, plot_constraints=True) -#~ plotQPRes(pb, res, ax=ax, plot_constraints=False) - -paths = [] - -def play_int(f_r = 100): - for (pid, pl) in paths: - for i in range(f_r): - frame = float(i) / float(f_r) * pl - v((ps.configAtParam(pid,frame))) - - -#~ raise ValueError - -def nil(): - global paths - #~ for j in range(2,len(all_states)-2,2): - for j in range(2,len(all_states)-2,1): - #~ print "state ", j - pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) - paths += [(pathId, ps.pathLength(pathId))] - play_int() - -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) diff --git a/sl1m/planner_scenarios/lp_stair_bauzil_10.py b/sl1m/planner_scenarios/lp_stair_bauzil_10.py deleted file mode 100644 index 72aca52..0000000 --- a/sl1m/planner_scenarios/lp_stair_bauzil_10.py +++ /dev/null @@ -1,244 +0,0 @@ -from hpp.corbaserver.rbprm.hrp2 import Robot -from hpp.gepetto import Viewer -from tools.display_tools import * -import time -print("Plan guide trajectory ...") -from . import lp_stair_bauzil_hrp2_path_10 as tp -print("Done.") -import time - -pId = tp.ps.numberPaths() -1 -fullBody = Robot () - -# Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-10.135,2, -20, 20, 0, 2.8]) -#fullBody.setConstrainedJointsBounds() -#fullBody.setJointBounds('leg_left_1_joint',[-0.1,0.2]) -#fullBody.setJointBounds('leg_right_1_joint',[-0.2,0.1]) -# add the 6 extraDof for velocity and acceleration (see *_path.py script) -fullBody.client.robot.setDimensionExtraConfigSpace(0) -#~ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0]) -ps = tp.ProblemSolver( fullBody ) -#~ ps.setParameter("Kinodynamic/velocityBound",tp.vMax) -#~ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax) -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.r.client, displayCoM = True) - -# load a reference configuration -#~ q_ref = fullBody.referenceConfig[::]+[0]*6 -q_ref = fullBody.referenceConfig[::] -q_init = q_ref[::] -fullBody.setReferenceConfig(q_ref) -fullBody.setCurrentConfig (q_init) -fullBody.setPostureWeights(fullBody.postureWeights[::]) -#~ fullBody.setPostureWeights(fullBody.postureWeights[::]) -#fullBody.usePosturalTaskContactCreation(True) - -print("Generate limb DB ...") -tStart = time.time() -# generate databases : - -nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) - - -tGenerate = time.time() - tStart -print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") - -#define initial and final configurations : -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() - - - -#~ q_init[0:7] = tp.ps.configAtParam(pId,0.)[0:7] # use this to get the correct orientation -q_init[:3] = [0.1, -0.82, 0.648702] -q_init[7:] = [ 0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10 - 0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17 - 0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30 - 0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36 - ]; -v(q_init) - -#fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm import rbprmstate -from hpp.corbaserver.rbprm import state_alg - -def getContactsFromConfig(q, limbs = [Robot.rLegId, Robot.lLegId]): - s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs) - rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId) - lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId) - return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) } - -#~ s = getContactsFromConfig ( q = fullBody.getCurrentConfig()) - - -from sl1m.planner_scenarios.stairs_10cm import * - -pb, coms, footpos, allfeetpos, res = solve() - -def gen_state(s, pId, com , num_max_sample = 1, first = False ): - #~ pId = 6 - phase = pb["phaseData"][pId] - moving = phase["moving"] - movingID = fullBody.lLegId - if moving == RF: - movingID = fullBody.rLegId - pos = allfeetpos[pId]; pos[2]+=0.01 - com[2] += 0.5 - print("com" , com) - print("pos" , pos.tolist()) - #~ q = fullBody.getCurrentConfig() - #~ s, succ = state_alg.addNewContact(s, fullBody.rLegId, rfPos.tolist(), z.tolist()) - sres, succ = state_alg.addNewContact(s, movingID, pos.tolist(), z.tolist(), num_max_sample= 0) - print("succes ?", succ) - succCom = False - ite = 0 - #~ if first: - #~ print "FIRST " - #~ com[2] -= 0.25 - #~ while(not succCom and ite < 11): - while(not succCom and ite < 17): - succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample) - com[2] -= 0.05 - ite += 1 - print("COM?", succCom) - v(sres.q()) - return sres - - - -q = fullBody.getCurrentConfig() -q[:2] = allfeetpos[0][:2].tolist() -v(q) -s = rbprmstate.State(fullBody, q = q, limbsIncontact = []) -all_states = [s] -sprev = s -idfirst = 2 - -for i in range(1, len(pb["phaseData"])): - #~ com = (coms[i-i] ).tolist() - com = (coms[i-1] ).tolist() - #~ if i > 2: - #~ com = (coms[i-1] + (coms[i] - coms[i-1]) *0.8).tolist() - snew = gen_state(sprev, i , com, num_max_sample = 20, first = i == idfirst ) - all_states += [snew] - sprev = snew - com2 = coms[i].tolist() - snew2 = gen_state(snew, i , com2, num_max_sample = 20 ) - all_states += [snew2] - sprev = snew2 - -all_states = all_states[:-1] -configs = [ st.q() for st in all_states[:]]; i = 0 - -print("SID ", [s.sId for s in all_states]) - -beginId = 4 - -#~ configs = configs[:-1] - -#~ configs = [q1, q2, q3, q4, q5] -#~ from cPickle import dump - -#~ f = open('contacts_plateformes.txt','w') -#~ dump(contacts,f) -#~ f.close() -ax = draw_scene() - #~ plotQPRes(pb, res, ax=ax, plot_constraints=True) -#~ plotQPRes(pb, res, ax=ax, plot_constraints=False) - -paths = [] - -def play_int(f_r = 100): - for (pid, pl) in paths: - for i in range(f_r): - frame = float(i) / float(f_r) * pl - v((ps.configAtParam(pid,frame))) - - -#~ def ppath(pid): - #~ length = - -#~ raise ValueError - - -comTrajs = [] -stateTrajs = [] - -def comTraj(c0, c1, t = 1.): - zero = [0.,0.,0] - return fullBody.generateComTraj([c0,c1],[zero],[zero],1.) - - -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) - -from time import sleep - -def dq(t = 1): - for q in configs: v(q); sleep(t) - -def nil(): - global paths - global stateTrajs - global comTrajs - #~ for j in range(2,len(all_states)-2,2): - for j in range(4,len(all_states),2): - #~ print "state ", j - pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) - paths += [(pathId, ps.pathLength(pathId))] - #~ print "j", j - if j+2 < len(all_states): - nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] - paths += [(nPid, ps.pathLength(nPid))] - - p0 = comTraj(all_states[j].getCenterOfMass(),all_states[j].getCenterOfMass()) - p1 = comTraj(all_states[j].getCenterOfMass(),all_states[j+1].getCenterOfMass()) - p2 = comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass()) - comTrajs += [(p0,p1,p2)] - stateTrajs += [(all_states[j].sId, all_states[j+2].sId)] - #~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] - #~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] - play_int() - #~ for stateTraj, comTtraj in zip(stateTrajs,comTrajs): - #~ paths+=[fullBody.effectorRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] - #~ pp(int(paths[-1])) - -def comrrt(): - global paths - global stateTrajs - global comTrajs - #~ for j in range(2,len(all_states)-2,2): - for j in range(4,len(all_states),2): - #~ print "state ", j - #~ pathId = fullBody.limbRRT(all_states[j].sId, all_states[j+1].sId) - #~ paths += [(pathId, ps.pathLength(pathId))] - #~ print "j", j - if j+2 < len(all_states): - #~ nPid = ps.directPath(all_states[j+1].q(),all_states[j+2].q(),False)[1] - #~ paths += [(nPid, ps.pathLength(nPid))] - - p0 = comTraj(all_states[j].getCenterOfMass(),all_states[j].getCenterOfMass()) - p1 = comTraj(all_states[j].getCenterOfMass(),all_states[j+1].getCenterOfMass()) - p2 = comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass()) - comTrajs += [(p0,p1,p2)] - stateTrajs += [(all_states[j].sId, all_states[j+2].sId)] - #~ comTrajs += [comTraj(all_states[j+1].getCenterOfMass(),all_states[j+2].getCenterOfMass())] - #~ stateTrajs += [(all_states[j+1].sId, all_states[j+2].sId)] - #~ play_int() - for stateTraj, comTtraj in zip(stateTrajs,comTrajs): - paths+=[fullBody.comRRTFromPosBetweenState(stateTraj[0],stateTraj[1],comTtraj[0],comTtraj[1],comTtraj[2])[-1]] - pp(int(paths[-1])) - -#~ nil() - -#~ comrrt() - -#~ play_int() - -beginId = 4 diff --git a/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path.py b/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path.py deleted file mode 100644 index 34bfeb0..0000000 --- a/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path.py +++ /dev/null @@ -1,85 +0,0 @@ -from hpp.corbaserver.rbprm.rbprmbuilder import Builder -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.robot import Robot as Parent - - -from hpp.corbaserver.rbprm.hrp2_abstract import Robot - -#~ rootJointType = 'freeflyer' -packageName = 'hpp-rbprm-corba' -meshPackageName = 'hpp-rbprm-corba' -#~ urdfName = 'hrp2_trunk_flexible' -#~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] -#~ urdfSuffix = "" -#~ srdfSuffix = "" - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-2,2, -1, 1, 0, 2.2]) -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1]) - -#~ from hpp.corbaserver.rbprm. import ProblemSolver -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -r = Viewer (ps) - - -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-1, -0.82, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) - -q_goal = q_init [::] -q_goal [3:7] = [ 0., 0. , 0.14943813, 0.98877108 ] -q_goal [0:3] = [1.49, -0.65, 1.2]; r (q_goal) -ps.addPathOptimizer("RandomShortcut") -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r) -afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) - -ps.client.problem.selectConfigurationShooter("RbprmShooter") -ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) -t = ps.solve () - - - -print(t); -#~ if isinstance(t, list): - #~ t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] -#~ f = open('log.txt', 'a') -#~ f.write("path computation " + str(t) + "\n") -#~ f.close() - - -from hpp.gepetto import PathPlayer -pp = PathPlayer (r) -#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") -#~ - -for i in range(1,10): - rbprmBuilder.client.problem.optimizePath(i) - -#~ pp (10) -#~ pp (0) - -#~ pp (1) -#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") -#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt") - -#~ cl = Client() -#~ cl.problem.selectProblem("rbprm_path") -#~ rbprmBuilder2 = Robot ("toto") -#~ ps2 = ProblemSolver( rbprmBuilder2 ) -#~ cl.problem.selectProblem("default") -#~ cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames()) -#~ r2 = Viewer (ps2, viewerClient=r.client) -#~ r.client.gui.setVisibility("toto", "OFF") -#~ r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF") -#~ r2(q_far) diff --git a/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path_10.py b/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path_10.py deleted file mode 100644 index e57e747..0000000 --- a/sl1m/planner_scenarios/lp_stair_bauzil_hrp2_path_10.py +++ /dev/null @@ -1,58 +0,0 @@ -from hpp.corbaserver.rbprm.rbprmbuilder import Builder -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.robot import Robot as Parent - - -from hpp.corbaserver.rbprm.hrp2_abstract import Robot - -#~ rootJointType = 'freeflyer' -packageName = 'hpp-rbprm-corba' -meshPackageName = 'hpp-rbprm-corba' -#~ urdfName = 'hrp2_trunk_flexible' -#~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] -#~ urdfSuffix = "" -#~ srdfSuffix = "" - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-2,2, -1, 1, 0, 2.2]) -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1]) - -#~ from hpp.corbaserver.rbprm. import ProblemSolver -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -r = Viewer (ps) - - -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-1, -0.82, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) - -q_goal = q_init [::] -q_goal [3:7] = [ 0., 0. , 0.14943813, 0.98877108 ] -q_goal [0:3] = [1.49, -0.65, 1.2]; r (q_goal) -ps.addPathOptimizer("RandomShortcut") -#~ ps.setInitialConfig (q_init) -#~ ps.addGoalConfig (q_goal) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -#~ afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r) -afftool.loadObstacleModel (packageName, "bauzil_stairs_10cm", "planning", r) -afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) - -ps.client.problem.selectConfigurationShooter("RbprmShooter") -ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) -#~ t = ps.solve () - - - - -from hpp.gepetto import PathPlayer -pp = PathPlayer (r) -#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path") -#~ diff --git a/sl1m/planner_scenarios/run.sh b/sl1m/planner_scenarios/run.sh deleted file mode 100644 index 6c19fae..0000000 --- a/sl1m/planner_scenarios/run.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash - -gepetto-gui & -hpp-rbprm-server & -ipython -i --no-confirm-exit ./$1 - -pkill -f 'gepetto-gui' -pkill -f 'hpp-rbprm-server' diff --git a/sl1m/planner_scenarios/rund.sh b/sl1m/planner_scenarios/rund.sh deleted file mode 100644 index bd7311a..0000000 --- a/sl1m/planner_scenarios/rund.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash - -gepetto-gui & -ipython -i --no-confirm-exit ./$1 - -pkill -f 'gepetto-gui' diff --git a/sl1m/planner_scenarios/stairs_10cm.py b/sl1m/planner_scenarios/stairs_10cm.py deleted file mode 100644 index 3e64508..0000000 --- a/sl1m/planner_scenarios/stairs_10cm.py +++ /dev/null @@ -1,145 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * - - -from plot_plytopes import * - -floor = [[-0.07, 0.3, 0.], [0.23, 0.3, 0. ], [0.23, 1.3, 0. ], [-0.07, 1.3, 0. ], ] -step1 = [[0.23, 1.3, 0.1], [0.53, 1.3, 0.1], [0.53, 0.3, 0.1 ], [0.23, 0.3, 0.1 ], ] -step2 = [[0.53, 1.3, 0.2], [0.83, 1.3, 0.2], [0.83, 0.3, 0.2 ], [0.53, 0.3, 0.2 ], ] -step3 = [[0.83, 1.3, 0.3], [1.13, 1.3, 0.3], [1.13, 0.3, 0.3 ], [0.83, 0.3, 0.3 ], ] -step4 = [[1.13, 1.3, 0.4], [1.43, 1.3, 0.4], [1.43, 0.3, 0.4 ], [1.13, 0.3, 0.4 ], ] -step5 = [[1.43, 1.3, 0.5], [1.73, 1.3, 0.5], [1.73, 0.3, 0.5 ], [1.43, 0.3, 0.5 ], ] -step6 = [[1.73, 1.3, 0.6], [2.03, 1.3, 0.6], [2.03, 0.3, 0.6 ], [1.73, 0.3, 0.6 ], ] -step7 = [[1.73, 1.7, 0.6], [2.73, 1.7, 0.6], [2.73,-0.3, 0.6 ], [1.73,-0.3, 0.6 ], ] - -all_surfaces = [floor, step1, step2, step3, step4,step5,step6] - -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -#~ astep7 = array(step7).T - -def gen_stair_pb(): - #~ for i in range(nphases) - #~ kinematicConstraints = genKinematicConstraints(min_height = 0.6) - kinematicConstraints = genKinematicConstraints(min_height = None) - relativeConstraints = genFootRelativeConstraints() - - #~ plot_polytope_H_rep(kinematicConstraints[0][0], kinematicConstraints[0][1].reshape((-1,1))) - - #~ surfaces = [[afloor], [afloor], [astep1,astep2], [astep2,astep1,astep3,astep4,astep5], [astep3,astep1,astep2,astep4,astep5], [astep4,astep1,astep2,astep3,astep5],[astep5,astep1,astep2,astep3,astep4],[astep6,astep5,astep1,astep2,astep3,astep4],[astep6,astep5,astep1,astep2,astep3,astep4],[astep6,astep5,astep1,astep2,astep3,astep4],[astep6,astep5,astep1,astep2,astep3,astep4],[astep6],[astep6]] - surfaces = [[afloor], [afloor], [astep1,astep2], [astep2,astep1,astep3,astep4,astep5], [astep3,astep1,astep2,astep4,astep5], [astep4,astep1,astep2,astep3,astep5],[astep5,astep1,astep2,astep3,astep4],[astep6],[astep6]] - #~ for Ss in surfaces: - #~ for el in Ss: - #~ el[1,:] -=0.5 - #~ surfaces = [[el[0]] for el in surfaces] - nphases = len(surfaces) - p0 = None - - res = { "p0" : None, "nphases": nphases} - - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -def solve(): - - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - - #~ C, c = least_square_cost_function (pb, A.shape[1], comTarget = array([30.,0.,0.5]), reg_term = 0.0000001) - C = identity(A.shape[1]) - c = zeros(A.shape[1]) - t2 = clock() - res = qp.solve_least_square(C,c,A,b,E,e) - t3 = clock() - - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - - return pb, coms, footpos, allfeetpos, res - -if __name__ == '__main__': - - from sl1m.planner_l1 import * - #~ draw_rectangle(l,ax) - #~ plt.show() - - pb = gen_stair_pb() - - t1 = clock() - A, b, E, e = convertProblemToLp(pb) - A,b = normalize([A,b]) - - #~ C, c = least_square_cost_function (pb, A.shape[1], comTarget = array([30.,0.,0.5]), reg_term = 0.0000001) - C = identity(A.shape[1]) * 0.00001 - #~ c = slackSelectionMatrix(pb) - c = zeros(A.shape[1]) - t2 = clock() - #~ res = qp.quadprog_solve_qp(C, c,A,b,E,e) - #~ t2 = clock() - #~ res = qp.solve_least_square(C,c,A,b,E,e) - res = qp.solve_lp(c,A,b,E,e)["x"] - t3 = clock() - - - print("time to set up problem" , timMs(t1,t2)) - print("time to solve problem" , timMs(t2,t3)) - print("total time" , timMs(t1,t3)) - - coms, footpos, allfeetpos = retrieve_points_from_res(pb, res) - - - ax = draw_scene() - #~ plotQPRes(pb, res, ax=ax, plot_constraints=True) - plotQPRes(pb, res, ax=ax, plot_constraints=False) - - diff --git a/sl1m/planner_scenarios/talos/broken/lp_slalom_debris.py b/sl1m/planner_scenarios/talos/broken/lp_slalom_debris.py new file mode 100644 index 0000000..ee6167e --- /dev/null +++ b/sl1m/planner_scenarios/talos/broken/lp_slalom_debris.py @@ -0,0 +1,323 @@ +from hpp.corbaserver.rbprm.talos import Robot +from hpp.gepetto import Viewer +from tools.display_tools import * +import time +from pinocchio import Quaternion +import numpy as np +from numpy import array, asmatrix, matrix, zeros, ones +from numpy import ( + array, + dot, + stack, + vstack, + hstack, + asmatrix, + identity, + cross, + concatenate, + isclose, +) +from numpy.linalg import norm +import random +from math import isnan +import sl1m.planner_scenarios.talos.slalom_debris as lp + +tp = lp.tp +pb, coms, footpos, allfeetpos, res = lp.solve() + + +print("Done.") +import time + +Robot.urdfSuffix += "_safeFeet" + +DEFAULT_COM_HEIGHT = 0.86 +Z_AXIS = lp.Z_AXIS +pId = tp.ps.numberPaths() - 1 +fullBody = Robot() + +# Set the bounds for the root +fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, 0, 2.8]) +fullBody.client.robot.setDimensionExtraConfigSpace(6) +fullBody.client.robot.setExtraConfigSpaceBounds(tp.extraDofBounds) +# add the 6 extraDof for velocity and acceleration (see *_path.py script) +ps = tp.ProblemSolver(fullBody) + +# load the viewer +v = tp.Viewer(ps, viewerClient=tp.v.client, displayCoM=True) + +# load a reference configuration +q_ref = fullBody.referenceConfig_elbowsUp[::] + [0] * 6 +fullBody.setCurrentConfig(q_ref) +fullBody.setReferenceConfig(q_ref) +fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6) +# fullBody.usePosturalTaskContactCreation(True) + +print("Generate limb DB ...") +tStart = time.time() +# generate databases : + +nbSamples = 10000 +fullBody.addLimb( + fullBody.rLegId, + fullBody.rleg, + fullBody.rfoot, + fullBody.rLegOffset, + fullBody.rLegNormal, + fullBody.rLegx, + fullBody.rLegy, + nbSamples, + "static", + 0.01, + kinematicConstraintsPath=fullBody.rLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) +fullBody.addLimb( + fullBody.lLegId, + fullBody.lleg, + fullBody.lfoot, + fullBody.lLegOffset, + fullBody.rLegNormal, + fullBody.lLegx, + fullBody.lLegy, + nbSamples, + "static", + 0.01, + kinematicConstraintsPath=fullBody.lLegKinematicConstraints, + kinematicConstraintsMin=0.3, +) +fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) + + +tGenerate = time.time() - tStart +print("Done.") +print("Databases generated in : " + str(tGenerate) + " s") + +configSize = ( + fullBody.getConfigSize() - fullBody.client.robot.getDimensionExtraConfigSpace() +) +q_init = q_ref[::] +q_init[0:7] = tp.q_init[0:7] +q_init[2] += q_ref[2] - 0.98 # 0.98 is the q_ref[2] for the rom device +v(q_init) + +fullBody.resetJointsBounds() +from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper + + +def quatConfigFromMatrix(m): + quat = Quaternion(m) + return quatToConfig(quat) + + +def quatToConfig(quat): + return [quat.x, quat.y, quat.z, quat.w] + + +def computeCenterOfSupportPolygon(s): + com = np.zeros(3) + numContacts = float(len(s.getLimbsInContact())) + for limbId in s.getLimbsInContact(): + com += np.array(s.getCenterOfContactForLimb(limbId)[0]) + com /= numContacts + com[2] += DEFAULT_COM_HEIGHT + return com.tolist() + + +def projectCoMInSupportPolygon(s): + desiredCOM = computeCenterOfSupportPolygon(s) + # print "try to project state to com position : ",desiredCOM + success = False + maxIt = 20 + print("project state to com : ", desiredCOM) + q_save = s.q()[::] + while not success and maxIt > 0: + success = s.fullBody.projectStateToCOM(s.sId, desiredCOM, maxNumSample=0) + maxIt -= 1 + desiredCOM[2] -= 0.005 + print("success = ", success) + print("result = ", s.q()) + if success and isnan(s.q()[0]): # FIXME why does it happen ? + success = False + s.setQ(q_save) + return success + + +def tryCreateContactAround( + s, + eff_id, + pos, + normal, + num_max_sample=0, + rotation=Quaternion.Identity(), + num_try=1000, +): + sres, succ = StateHelper.addNewContact( + s, + eff_id, + pos, + normal, + num_max_sample=num_max_sample, + rotation=quatToConfig(rotation), + ) + i_try = 0 + x_bounds = [-0.1, 0.1] + y_bounds = [-0.05, 0.05] + print("try create contact around, first try success : ", succ) + print("result = ", sres.q()) + while not succ and i_try < num_try: + print("try create contact around, try : ", i_try) + x = random.uniform(x_bounds[0], x_bounds[1]) + y = random.uniform(y_bounds[0], y_bounds[1]) + offset = np.matrix([x, y, 0]).T + offset = rotation.matrix() * offset + new_pos = pos[::] + for i in range(3): + new_pos[i] += offset[i, 0] + sres, succ = StateHelper.addNewContact( + s, + eff_id, + new_pos, + normal, + num_max_sample=num_max_sample, + rotation=quatToConfig(rotation), + ) + i_try += 1 + + return sres, succ + + +def gen_state( + s, + pId, + num_max_sample=0, + first=False, + normal=lp.Z_AXIS, + newContact=True, + projectCOM=True, +): + # ~ pId = 6 + phase = pb["phaseData"][pId] + moving = phase["moving"] + movingID = fullBody.lLegId + if moving == lp.RF: + movingID = fullBody.rLegId + print("# gen state for phase Id = ", pId) + if False and pId < len(pb["phaseData"]) - 1: + quat0 = Quaternion(pb["phaseData"][pId]["root_orientation"]) + quat1 = Quaternion(pb["phaseData"][pId + 1]["root_orientation"]) + qrot = (quat0.slerp(0.5, quat1)).matrix() + else: + qrot = Quaternion( + phase["root_orientation"] + ) # rotation of the root, from the guide + + # q_n = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T) + # rot = quatToConfig(qrot * q_n) + if not isclose(normal, Z_AXIS).all(): + qrot = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T, np.matrix(normal).T) + # ignore guide orientation when normal is not z ... + # rot = quatToConfig(qrot) + pos = allfeetpos[pId] + pos[2] += 0.002 + pose = pos.tolist() + quatToConfig(qrot) + print("Try to add contact for " + movingID + " pos = " + str(pose)) + disp.moveSphere("c", v, pose) + if newContact: + sres, succ = tryCreateContactAround( + s, + movingID, + pos.tolist(), + normal.tolist(), + num_max_sample=num_max_sample, + rotation=qrot, + ) + # sres, succ = StateHelper.removeContact(s,movingID) + # assert succ + # succ = sres.projectToRoot(s.q()[0:3]+rot) + # sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample) + else: + sres, succ = StateHelper.cloneState(s) + if not succ: + print("Cannot project config q = ", sres.q()) + print( + "To new contact position for " + + movingID + + " = " + + str(pose) + + " ; n = " + + str(normal.tolist()) + ) + raise RuntimeError( + "Cannot project feet to new contact position" + ) # try something else ?? + if projectCOM: + # sfeet, _ = StateHelper.cloneState(sres) + # print "config before projecting to com q1=",sres.q() + successCOM = projectCoMInSupportPolygon(sres) + if not successCOM: + # is it really an issue ? + print("Unable to project CoM in the center of the support polygone") + + v(sres.q()) + return sres + + +import tools.display_tools as disp + +disp.createSphere("c", v, color=v.color.green) +v.addLandmark("c", 0.1) + + +q = fullBody.getCurrentConfig() +v(q) +s = State(fullBody, q=q, limbsIncontact=[fullBody.lLegId, fullBody.rLegId]) + +idfirst = 2 +# coms[0] = array(s.getCenterOfMass()) +coms[0][2] -= 0.02 + + +def normal(phase): + s = phase["S"][0] + i = 0 + n = np.zeros(3) + while i < s.shape[1] - 3 and norm(n) < 1e-4: + n = cross(s[:, i + 1] - s[:, i + 0], s[:, i + 2] - s[:, i + 0]) + i += 1 + if norm(n) < 1e-4: + return Z_AXIS + n /= norm(n) + if n[2] < 0.0: + for i in range(3): + n[i] = -n[i] + # ~ print "normal ", n + return n + + +all_states = [s] +sprev = s +for i in range(2, len(pb["phaseData"])): + n = normal(pb["phaseData"][i]) + snew = gen_state(sprev, i, num_max_sample=100, first=False, normal=n) + all_states += [snew] + sprev = snew + +configs = [st.q() for st in all_states[:]] +i = 0 +# displayContactSequence(v,configs) + + +print("SID ", [s.sId for s in all_states]) + +beginId = 0 + + +""" +import tools.display_tools as disp +disp.createSphere("c",v,color = v.color.green) +v.addLandmark("c",0.1) + +disp.moveSphere('c',v,p) +""" diff --git a/sl1m/planner_scenarios/talos/broken/lp_slalom_debris_path.py b/sl1m/planner_scenarios/talos/broken/lp_slalom_debris_path.py new file mode 100644 index 0000000..e904356 --- /dev/null +++ b/sl1m/planner_scenarios/talos/broken/lp_slalom_debris_path.py @@ -0,0 +1,179 @@ +from hpp.gepetto import PathPlayer +from hpp.corbaserver.affordance.affordance import AffordanceTool +from hpp.gepetto import ViewerFactory +from hpp.corbaserver.problem_solver import ProblemSolver +import time +from talos_rbprm.talos_abstract import Robot + +packageName = "hpp_environments" +meshPackageName = "hpp_environments" + +time.sleep(1) + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds( + "root_joint", + [-5, 5, -5, 5, rbprmBuilder.ref_height, rbprmBuilder.ref_height], +) +# As this scenario only consider walking, we fix the DOF of the torso : +rbprmBuilder.setJointBounds("torso_1_joint", [0, 0]) +rbprmBuilder.setJointBounds("torso_2_joint", [0.0, 0.0]) +vMax = 1.0 # linear velocity bound for the root +aMax = 2.0 # linear acceleration bound for the root +extraDof = 6 +mu = 10.0 # coefficient of friction +rbprmBuilder.setFilter([]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) +# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root +rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) +# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) +extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, +] +rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) +indexECS = ( + rbprmBuilder.getConfigSize() + - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() +) + +ps = ProblemSolver(rbprmBuilder) +vf = ViewerFactory(ps) + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/slalom_debris.urdf", + "planning", + vf, + reduceSizes=[0.03, 0.03, 0.0], +) +v = vf.createViewer(displayArrows=True) +afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) +v.addLandmark(v.sceneName, 1) + + +# force the orientation of the trunk to match the direction of the motion +ps.setParameter("Kinodynamic/forceYawOrientation", True) +ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) +ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) +ps.setParameter("DynamicPlanner/sizeFootX", 0.2) +ps.setParameter("DynamicPlanner/sizeFootY", 0.12) +ps.setParameter("DynamicPlanner/friction", mu) +# sample only configuration with null velocity and acceleration : +ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) +ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) + +# Choosing RBPRM shooter and path validation methods. +ps.selectConfigurationShooter("RbprmShooter") +# ps.addPathOptimizer ("RandomShortcutDynamic") +ps.selectPathValidation("RbprmPathValidation", 0.05) +# Choosing kinodynamic methods : +ps.selectSteeringMethod("RBPRMKinodynamic") +ps.selectDistance("Kinodynamic") +ps.selectPathPlanner("DynamicPlanner") + +# # # BEGIN up to the rubbles # # # # # +ps.setParameter("Kinodynamic/velocityBound", 0.15) +ps.setParameter("Kinodynamic/accelerationBound", 0.05) +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-1.8, 0.0, rbprmBuilder.ref_height] +v(q_init) +q_init[-6:-3] = [0.1, 0, 0] +q_goal = q_init[::] +q_goal[0:3] = [-0.8, 0.9, rbprmBuilder.ref_height] +v(q_goal) +q_goal[-6:-3] = [0.1, 0, 0] +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +v(q_goal) + +q_init_0 = q_init[::] +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) +# for i in range(5): +# ps.optimizePath(ps.numberPaths() -1) + +pId_begin = ps.numberPaths() - 1 +# # # END BEGIN up to the rubbles # # # # # +ps.resetGoalConfigs() +# # # BEGIN rubbles # # # # # +ps.setParameter("Kinodynamic/velocityBound", 0.4) +ps.setParameter("Kinodynamic/accelerationBound", 0.1) +q_init = rbprmBuilder.getCurrentConfig() +q_init = q_goal[::] +v(q_init) +# q_init[-6:-3] = [0.,0,0] +q_goal[0:3] = [1.05, 0.9, rbprmBuilder.ref_height] +v(q_goal) +q_goal[-6:-3] = [0.1, 0, 0] +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +v(q_goal) + +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) +# for i in range(5): +# ps.optimizePath(ps.numberPaths() -1) + +pId_rubbles = ps.numberPaths() - 1 +# # # END rubbles # # # # # +ps.resetGoalConfigs() +# # # BEGIN after rubbles # # # # # +ps.setParameter("Kinodynamic/velocityBound", 0.15) +ps.setParameter("Kinodynamic/accelerationBound", 0.05) +q_init = rbprmBuilder.getCurrentConfig() +q_init = q_goal[::] +v(q_init) +q_goal[0:3] = [2.2, 0, rbprmBuilder.ref_height] +v(q_goal) +q_goal[-6:-3] = [0.05, 0, 0] +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +v(q_goal) + +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) +# for i in range(5): +# ps.optimizePath(ps.numberPaths() -1) + +pId_end = ps.numberPaths() - 1 +# # # END after rubbles # # # # # +pathId = pId_begin +ps.concatenatePath(pathId, pId_rubbles) +ps.concatenatePath(pathId, pId_end) + + +print("done optimizing.") +pp = PathPlayer(v) +pp.dt = 0.1 +pp.displayVelocityPath(pathId) +v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") +pp.dt = 0.01 + + +q_far = q_goal[::] +q_far[2] = -5 +v(q_far) +q_init = q_init_0[::] diff --git a/sl1m/planner_scenarios/talos/broken/slalom_debris.py b/sl1m/planner_scenarios/talos/broken/slalom_debris.py new file mode 100644 index 0000000..dfa5740 --- /dev/null +++ b/sl1m/planner_scenarios/talos/broken/slalom_debris.py @@ -0,0 +1,79 @@ +import numpy as np +import os +import sl1m.tools.plot_tools as plot +import matplotlib.pyplot as plt + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.problem_definition import Problem +from sl1m.generic_solver import * + +from time import perf_counter as clock + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_MIP = False +USE_COM = False + +paths = [ + os.environ["INSTALL_HPP_DIR"] + + "/share/talos-rbprm/com_inequalities/feet_quasi_flat/talos_", + os.environ["INSTALL_HPP_DIR"] + + "/share/talos-rbprm/relative_effector_positions/talos_", +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + + from sl1m.planner_scenarios.talos import lp_slalom_debris_path as tp + + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + tp.rbprmBuilder, tp.ps, tp.afftool, tp.pathId, tp.v, 0.7, False + ) + t_2 = clock() + + p0 = [ + np.array(tp.q_init[:3]) + [0, 0.085, -0.98], + np.array(tp.q_init[:3]) + [0, -0.085, -0.98], + ] + t_3 = clock() + + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces_gait, GAIT, p0, tp.q_init[:3]) + t_4 = clock() + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the path takes ", 1000.0 * (t_1 - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_2 - t_1)) + print("Computing the initial contacts takes ", 1000.0 * (t_3 - t_2)) + print("Generating the problem dictionary takes ", 1000.0 * (t_4 - t_3)) + print("Solving the problem takes ", 1000.0 * (t_end - t_4)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/planner_scenarios/talos/complex.py b/sl1m/planner_scenarios/talos/complex.py new file mode 100644 index 0000000..6d06091 --- /dev/null +++ b/sl1m/planner_scenarios/talos/complex.py @@ -0,0 +1,104 @@ +import numpy as np +import matplotlib.pyplot as plt +import os +from pathlib import Path +import talos_rbprm + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.planner_scenarios.talos.lp_complex_path import compute_path + +from sl1m.stand_alone_scenarios.problem_definition_talos import ( + Problem as TalosProblem, +) +from sl1m.problem_definition import Problem + +from sl1m.generic_solver import * + +import sl1m.tools.plot_tools as plot + +from time import perf_counter as clock + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_BIPED_PLANNER = True +USE_MIP = True +USE_COM = True + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + talos_abstract, ps, afftool, pathId, v, q_init = compute_path() + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + talos_abstract, ps, afftool, pathId, v, 0.5, False + ) + t_2 = clock() + + p0 = [ + np.array(q_init[:3]) + [0, 0.085, -0.98], + np.array(q_init[:3]) + [0, -0.085, -0.98], + ] + t_3 = clock() + + if USE_BIPED_PLANNER: + pb = TalosProblem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, [0, 1], p0) + t_4 = clock() + if USE_MIP: + result = solve_MIP_biped(pb) + else: + result = solve_L1_combinatorial_biped(pb, surfaces) + else: + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces_gait, GAIT, p0, q_init[:3]) + t_4 = clock() + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/planner_scenarios/talos/complex1.py b/sl1m/planner_scenarios/talos/complex1.py deleted file mode 100644 index 8f11275..0000000 --- a/sl1m/planner_scenarios/talos/complex1.py +++ /dev/null @@ -1,212 +0,0 @@ -import numpy as np -print("Plan guide trajectory ...") -from . import lp_complex1_path as tp -#import lp_ramp_path as tp -print("Guide planned.") -from tools.surfaces_from_path import getSurfacesFromGuideContinuous - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - - -from sl1m.planner import * - - -from sl1m.tools.plot_plytopes import * -from sl1m.planner_scenarios.talos.constraints import * - -Z_AXIS = np.array([0,0,1]).T - - -""" -### HARDCODED SURFACES, REPLACE IT WITH PATH PLANNING #### -floor = [[-0.30, 0.54 , 0. ], [-0.1 , 0.54, 0. ], [-0.1 , -0.46, 0. ], [-0.30, -0.46, 0. ], ] -step1 = [[ 0.01, 0.54 , 0.1 ], [0.31 , 0.54, 0.1], [0.31 , -0.46, 0.1 ], [ 0.01, -0.46, 0.1 ], ] -step2 = [[ 0.31, 0.54 , 0.2 ], [0.61 , 0.54, 0.2], [0.61 , -0.46, 0.2 ], [ 0.31, -0.46, 0.2 ], ] -step3 = [[ 0.61, 0.54 , 0.3 ], [0.91 , 0.54, 0.3], [0.91 , -0.46, 0.3 ], [ 0.61, -0.46, 0.3 ], ] -step4 = [[ 0.91, 0.54 , 0.4 ], [1.21 , 0.54, 0.4], [1.21 , -0.46, 0.4 ], [ 0.91, -0.46, 0.4 ], ] -step5 = [[ 1.24, 0.54 , 0.5 ], [1.51 , 0.54, 0.5], [1.51 , -0.46, 0.5 ], [ 1.24, -0.46, 0.5 ], ] -step6 = [[ 1.55, 0.54 , 0.6 ], [1.81 , 0.54, 0.6], [1.81 , -0.46, 0.6 ], [ 1.55, -0.46, 0.6 ], ] -#~ step7 = [[ 1.51, 0.94 , 0.6 ], [2.51 , 0.94, 0.6], [2.51 , -1.06, 0.6 ], [ 1.51, -1.06, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -#~ platfo = [[-1.49, -0.06 , 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.06, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -#~ step8 = [[-1.49, -0.06 , 0.45], [-1.49, 0.24, 0.45],[-2.49, 0.24, 0.45], [-2.49, -0.06, 0.45], ] -#~ step9 = [[-1.49, 0.24 , 0.30], [-1.49, 0.54, 0.30],[-2.49, 0.54, 0.30], [-2.49, 0.24, 0.30], ] -#~ step10= [[-1.49, 0.54 , 0.15], [-1.49, 0.84, 0.15],[-2.49, 0.84, 0.15], [-2.49, 0.54, 0.15], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.19 , 0.05 ], [-2.45 , 0.19, 0.05 ], [ -2.45, 0.53, 0.05 ], [-2.11, 0.53, 0.05 ], ] -rub3 = [[ -1.91, -0.15 , 0.1 ], [-2.25 , -0.15, 0.1 ], [ -2.25, 0.15, 0.1 ], [-1.91, 0.15, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.53, 0.15 ], [-1.69, 0.53, 0.15 ], ] -rub5 = [[ -1.49, -0.15 , 0.2 ], [-1.83 , -0.15, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.53, 0.2 ], [-1.29, 0.53, 0.2 ], ] -rub7 = [[ -1.09, -0.15 , 0.15 ], [-1.43 , -0.15, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.53, 0.1], [-0.89, 0.53, 0.1 ], ] -rub8 = [[ -0.89, -0.15 , 0.025 ], [-1.02 , -0.15, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.15 , 0.025 ], [-0.86 , -0.15, 0.025], [-0.86, 0.52, 0.025 ], [ -0.35, 0.52, 0.025], ] -rub8 = [[ -0.89, -0.15 , 0.05 ], [-1.02 , -0.15, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.45, -0.15 , 0.05 ], [-0.86 , -0.15, 0.05], [-0.86, 0.52, 0.05 ], [ -0.45, 0.52, 0.05], ] - -all_surfaces = [floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -#~ arub1 = array(rub1).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -#~ astep8 = array(step8).T -#~ astep9 = array(step9).T -#~ astep10 = array(step10).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] - -#~ surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1], [astep2,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6],[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] - -#surfaces1 = [ [abridge], [abridge],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[aplatfo],[aplatfo],[aplatfo],[aplatfo]] -surfaces = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1],[astep2],[astep3], [astep4],[astep5],[astep6],[astep6]] -""" - -### END HARDCODED SURFACES #### - - -def gen_pb(root_init,R, surfaces): - - nphases = len(surfaces) - #nphases = 20 - lf_0 = array(root_init[0:3]) + array([0, 0.085,-0.98]) # values for talos ! - rf_0 = array(root_init[0:3]) + array([0,-0.085,-0.98]) # values for talos ! - #p0 = [array([-3.0805096486250154, 0.335, 0.]), array([-3.0805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - #p0 = [array([-0.1805096486250154, 0.335, 0.]), array([-0.1805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - p0 = [lf_0,rf_0]; - print("p0 used : ",p0) - - res = { "p0" : p0, "c0" : None, "nphases": nphases} - #res = { "p0" : None, "c0" : None, "nphases": nphases} - - print("surfaces = ",surfaces) - #TODO in non planar cases, K must be rotated - #phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [genKinematicConstraints(left_foot_constraints,right_foot_constraints,index = i, rotation = R, min_height = 0.3) for _ in range(len(surfaces[i]))], "relativeK" : [genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints,index = i, rotation = R)[(i) % 2] for _ in range(len(surfaces[i]))], "rootOrientation" : R[i], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,0.2,True) - - pb = gen_pb(tp.q_init,R,surfaces) - - return solveL1(pb, surfaces, draw_scene) - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1 - - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,0.2,True) - - pb = gen_pb(tp.q_init,R,surfaces) - - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - - -""" -import matplotlib.pyplot as plt -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt, c = 'rand'): - xs = [point[0] for point in points] - ys = [point[1] for point in points] - zs = [point[2] for point in points] - xs = np.append(xs, xs[0]).tolist() - ys = np.append(ys, ys[0]).tolist() - zs = np.append(zs, zs[0]).tolist() - if c == 'rand': ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,c) - plt.draw() - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surface in my_surfaces: plotSurface(surface[0], ax, plt) - return ax - -global my_surfaces -my_surfaces = tp.surfaces -ax = draw_scene() -plt.show() -""" - diff --git a/sl1m/planner_scenarios/talos/constraints.py b/sl1m/planner_scenarios/talos/constraints.py deleted file mode 100644 index 900a222..0000000 --- a/sl1m/planner_scenarios/talos/constraints.py +++ /dev/null @@ -1,61 +0,0 @@ -from sl1m.constants_and_tools import * -import sl1m.stand_alone_scenarios - -import os - -insdir = os.environ["INSTALL_HPP_DIR"]+"/share/talos-rbprm/" - -# load constraints -__ineq_right_foot = None -__ineq_left_foot = None -__ineq_right_foot_reduced = None -__ineq_left_foot_reduced = None - -# add foot offset -def right_foot_constraints(transform): - global __ineq_right_foot - if __ineq_right_foot is None: - filekin = insdir +"com_inequalities/feet_quasi_flat/talos_COM_constraints_in_RF_effector_frame_REDUCED.obj" - obj = load_obj(filekin) - __ineq_right_foot = as_inequalities(obj) - transform2 = transform.copy() - transform2[2,3] += 0.105 - ine = rotate_inequalities(__ineq_right_foot, transform2) - return (ine.A, ine.b) - -def left_foot_constraints(transform): - global __ineq_left_foot - if __ineq_left_foot is None: - filekin = insdir +"com_inequalities/feet_quasi_flat/talos_COM_constraints_in_LF_effector_frame_REDUCED.obj" - obj = load_obj(filekin) - __ineq_left_foot = as_inequalities(obj) - transform2 = transform.copy() - transform2[2,3] += 0.105 - ine = rotate_inequalities(__ineq_left_foot, transform2) - return (ine.A, ine.b) - - -__ineq_rf_in_rl = None -__ineq_lf_in_rf = None - -# add foot offset -def right_foot_in_lf_frame_constraints(transform): - global __ineq_rf_in_rl - if __ineq_rf_in_rl is None: - filekin = insdir +"relative_effector_positions/talos_RF_constraints_in_LF_quasi_flat_REDUCED.obj" - obj = load_obj(filekin) - __ineq_rf_in_rl = as_inequalities(obj) - transform2 = transform.copy() - ine = rotate_inequalities(__ineq_rf_in_rl, transform2) - return (ine.A, ine.b) - -def left_foot_in_rf_frame_constraints(transform): - global __ineq_lf_in_rf - if __ineq_lf_in_rf is None: - filekin = insdir +"relative_effector_positions/talos_LF_constraints_in_RF_quasi_flat_REDUCED.obj" - obj = load_obj(filekin) - __ineq_lf_in_rf = as_inequalities(obj) - transform2 = transform.copy() - ine = rotate_inequalities(__ineq_lf_in_rf, transform2) - return (ine.A, ine.b) - diff --git a/sl1m/planner_scenarios/talos/lp_complex1.py b/sl1m/planner_scenarios/talos/lp_complex1.py deleted file mode 100644 index debbe82..0000000 --- a/sl1m/planner_scenarios/talos/lp_complex1.py +++ /dev/null @@ -1,189 +0,0 @@ -from hpp.corbaserver.rbprm.talos import Robot -from hpp.gepetto import Viewer -from tools.display_tools import * -import time -from pinocchio import Quaternion -import numpy as np -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -import sl1m.planner_scenarios.talos.complex1 as lp -tp = lp.tp -pb, coms, footpos, allfeetpos, res = lp.solve() - - -print("Done.") -import time -#Robot.urdfSuffix+="_safeFeet" - -DEFAULT_COM_HEIGHT = 0.86 -USE_ORIENTATION = True -pId = tp.ps.numberPaths() -1 -fullBody = Robot () - -# Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-20,20 ,-20, 20, 0, 2.8]) -fullBody.client.robot.setDimensionExtraConfigSpace(6) -fullBody.client.robot.setExtraConfigSpaceBounds(tp.extraDofBounds) -# add the 6 extraDof for velocity and acceleration (see *_path.py script) -ps = tp.ProblemSolver( fullBody ) - -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) - -# load a reference configuration -q_ref = fullBody.referenceConfig_elbowsUp[::] +[0]*6 -fullBody.setCurrentConfig (q_ref) -fullBody.setReferenceConfig(q_ref) -fullBody.setPostureWeights(fullBody.postureWeightsRootRotationConstrained[::] + [0]*6) -#fullBody.usePosturalTaskContactCreation(True) - -print("Generate limb DB ...") -tStart = time.time() -# generate databases : - -nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) - - -tGenerate = time.time() - tStart -print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") - -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() -q_init = q_ref[::] -q_init[0:7] = tp.q_init[0:7] -q_init[2] += q_ref[2] - 0.98 # 0.98 is the q_ref[2] for the rom device -v(q_init) - -fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper - -def quatConfigFromMatrix(m): - quat = Quaternion(m) - return [quat.x,quat.y,quat.z,quat.w] - -def computeCenterOfSupportPolygon(s): - com = np.zeros(3) - numContacts = float(len(s.getLimbsInContact())) - for limbId in s.getLimbsInContact(): - com += np.array(s.getCenterOfContactForLimb(limbId)[0]) - com /= numContacts - com[2] += DEFAULT_COM_HEIGHT - return com.tolist() - -def projectCoMInSupportPolygon(s): - desiredCOM = computeCenterOfSupportPolygon(s) - #print "try to project state to com position : ",desiredCOM - success = False - maxIt = 20 - while not success and maxIt > 0: - success = s.fullBody.projectStateToCOM(s.sId ,desiredCOM, maxNumSample = 0) - maxIt -= 1 - desiredCOM[2] -= 0.005 - return success - - -def gen_state(s, pId, num_max_sample = 0, first = False, normal = lp.Z_AXIS, newContact = True ,projectCOM = True): - #~ pId = 6 - phase = pb["phaseData"][pId] - moving = phase["moving"] - movingID = fullBody.lLegId - if moving == lp.RF: - movingID = fullBody.rLegId - print("# gen state for phase Id = ",pId) - if USE_ORIENTATION: - if pId < len(pb["phaseData"])-1: - if phase["moving"] == lp.RF: - rot = quatConfigFromMatrix(pb["phaseData"][pId+1]["rootOrientation"]) # rotation of the root, from the guide - else : - rot = quatConfigFromMatrix(pb["phaseData"][pId]["rootOrientation"]) # rotation of the root, from the guide - #quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) - #quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"]) - #rot = quatConfigFromMatrix((quat0.slerp(0.5,quat1)).matrix()) - else: - rot = quatConfigFromMatrix(phase["rootOrientation"]) # rotation of the root, from the guide - else : - rot = [0,0,0,1] - #rot = quatConfigFromMatrix(phase["rootOrientation"]) # rotation of the root, from the guide - #print "current config q=",s.q() - #print "move limb ",movingID - pos = allfeetpos[pId]; - print("Try to add contact for "+movingID+" pos = "+str(pos.tolist()+rot)) - disp.moveSphere('c',v,pos.tolist()+rot) - if newContact: - sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample,rotation = rot) - #sres, succ = StateHelper.removeContact(s,movingID) - #assert succ - #succ = sres.projectToRoot(s.q()[0:3]+rot) - #sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample) - else: - sres, succ = StateHelper.cloneState(s) - if not succ: - print("Cannot project config q = ",sres.q()) - print("To new contact position for "+movingID+" = "+str(pos.tolist()+rot)+" ; n = "+str(normal.tolist())) - raise RuntimeError("Cannot project feet to new contact position") # try something else ?? - if projectCOM : - #print "config before projecting to com q1=",sres.q() - successCOM = projectCoMInSupportPolygon(sres) - if not successCOM: - # is it really an issue ? - print("Unable to project CoM in the center of the support polygone") - - v(sres.q()) - return sres - -import tools.display_tools as disp -disp.createSphere("c",v,color = v.color.green) -v.addLandmark("c",0.1) - - - -q = fullBody.getCurrentConfig() -v(q) -s = State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId]) - -idfirst = 2 -#coms[0] = array(s.getCenterOfMass()) -coms[0][2] -= 0.02 - - -def normal(phase): - s = phase["S"][0] - n = cross(s[:,1] - s[:,0], s[:,2] - s[:,0]) - n /= norm(n) - if n[2] < 0.: - for i in range(3): - n[i] = -n[i] - #~ print "normal ", n - return n - -all_states = [s] -sprev = s -for i in range(2, len(pb["phaseData"])): - n = normal(pb["phaseData"][i]) - snew = gen_state(sprev, i , num_max_sample = 10000, first = False, normal = n ) - all_states += [snew] - sprev = snew - -configs = [ st.q() for st in all_states[:]]; i = 0 -#displayContactSequence(v,configs) - - -print("SID ", [s.sId for s in all_states]) - -beginId = 0 - - -""" -import tools.display_tools as disp -disp.createSphere("c",v,color = v.color.green) -v.addLandmark("c",0.1) - -disp.moveSphere('c',v,pos) -""" - diff --git a/sl1m/planner_scenarios/talos/lp_complex1_path.py b/sl1m/planner_scenarios/talos/lp_complex1_path.py deleted file mode 100644 index 625f51a..0000000 --- a/sl1m/planner_scenarios/talos/lp_complex1_path.py +++ /dev/null @@ -1,192 +0,0 @@ -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.rbprm.talos_abstract import Robot -Robot.urdfName += "_large" - -packageName = 'hpp_environments' -meshPackageName = 'hpp_environments' - -import time -time.sleep(1) - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-3.2,1.8,0.19,0.21, 0.95,1.7]) -# As this scenario only consider walking, we fix the DOF of the torso : -rbprmBuilder.setJointBounds ('torso_1_joint', [0,0]) -rbprmBuilder.setJointBounds ('torso_2_joint', [0.,0.]) -vMax = 1.# linear velocity bound for the root -aMax = 2. # linear acceleration bound for the root -extraDof = 6 -mu=0.5# coefficient of friction -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-4.,4.,-0.1,0.1,-0.1,0.1]) -# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root -rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) -# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) -extraDofBounds = [-vMax,vMax,-vMax,vMax,-10.,10.,-aMax,aMax,-aMax,aMax,-10.,10.] -rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) -indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() - -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -from hpp.gepetto import ViewerFactory -vf = ViewerFactory (ps) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "multicontact/bauzil_ramp_simplified", "planning", vf,reduceSizes=[0.07,0.,0.]) -v = vf.createViewer(displayArrows = True) -afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5]) -v.addLandmark(v.sceneName,1) - - - -ps.setParameter("Kinodynamic/velocityBound",vMax) -ps.setParameter("Kinodynamic/accelerationBound",aMax) -# force the orientation of the trunk to match the direction of the motion -ps.setParameter("Kinodynamic/forceYawOrientation",True) -ps.setParameter("Kinodynamic/synchronizeVerticalAxis",True) -ps.setParameter("Kinodynamic/verticalAccelerationBound",10.) -ps.setParameter("DynamicPlanner/sizeFootX",0.2) -ps.setParameter("DynamicPlanner/sizeFootY",0.12) -ps.setParameter("DynamicPlanner/friction",mu) -# sample only configuration with null velocity and acceleration : -ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) -ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) -# Choosing RBPRM shooter and path validation methods. -ps.selectConfigurationShooter("RbprmShooter") -ps.addPathOptimizer ("RandomShortcutDynamic") -ps.selectPathValidation("RbprmPathValidation",0.05) -# Choosing kinodynamic methods : -ps.selectSteeringMethod("RBPRMKinodynamic") -ps.selectDistance("Kinodynamic") -ps.selectPathPlanner("DynamicPlanner") - -""" -### BEGIN rubbles ##### -ps.setParameter("Kinodynamic/velocityBound",0.3) -ps.setParameter("Kinodynamic/accelerationBound",1.) -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubbles -q_goal = q_init [::] -q_goal [0:3] = [-0.3, 0.2,0.98] ; v(q_goal) # between rubble and stairs -q_goal[-6:-3] = [0.1,0,0] -ps.setInitialConfig (q_init) -q_init_0 = q_init[::] -ps.addGoalConfig (q_goal) -v(q_goal) -q_init_0 = q_init[::] -t = ps.solve () -print "done planning, optimize path ..." -#v.solveAndDisplay('rm',2,0.005) - -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_rubbles = ps.numberPaths() -1 -### END rubbles ##### -ps.resetGoalConfigs() -pathId = pId_rubbles -""" - -### BEGIN climb the stairs ##### -ps.setParameter("Kinodynamic/velocityBound",0.3) -ps.setParameter("Kinodynamic/accelerationBound",0.1) -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-0.3, 0.2,0.98] ; v(q_init) # between rubble and stairs -#q_init = q_goal[::] ; v(q_init) # between rubble and stairs -#q_init[-6:-3] = [0.15,0,0] -q_goal = q_init [::] -q_goal [0:3] = [1.7, 0.2, 1.58]; v (q_goal) #top of stairs -q_goal[-6:-3] = [0,0,0] -ps.setInitialConfig (q_init) -q_init_0 = q_init[::] -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) - -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_stairs = ps.numberPaths() -1 -### END climb the stairs ##### -rbprmBuilder.setJointBounds ("root_joint", [-3.2,2.5,-0.8,0.3, 1.4,2.]) -ps.resetGoalConfigs() -### BEGIN turn around on the platform ##### -ps.setParameter("Kinodynamic/velocityBound",0.2) -ps.setParameter("Kinodynamic/accelerationBound",0.07) -q_init = rbprmBuilder.getCurrentConfig (); -#q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubblem -#q_init [0:3] = [-0.2, 0.2,0.98] ; v(q_init) # between rubble and stairs -q_init = q_goal[::] ; v (q_init) #top of stairs -#q_init [0:3] = [1.7, -0.6, 1.58]; v (q_init) #top of stairs -#q_init[3:7] = [0,0,1,0] -q_init[-6:-3] = [0.2,0,0] -q_goal = q_init [::] -# q_goal [0:3] = [1.7, 0.2, 1.58]; v (q_goal) #top of stairs -#q_goal [0:3] = [-1.7, -0.6, 1.58]; v (q_goal) # after bridge -q_goal [0:3] = [1.7, -0.6, 1.58]; v (q_goal) # before bridge -q_goal[3:7] = [0,0,1,0] -q_goal[-6:-3] = [-0.2,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_platform = ps.numberPaths() -1 -### END turn around on the platform ##### -ps.resetGoalConfigs() -### BEGIN bridge cross ##### -ps.setParameter("Kinodynamic/velocityBound",0.3) -ps.setParameter("Kinodynamic/accelerationBound",0.2) -q_init = rbprmBuilder.getCurrentConfig (); -q_init = q_goal[::]; v (q_init) #top of stairs -q_goal [0:3] = [-1.7, -0.6, 1.58]; v (q_goal) # after bridge -q_goal[3:7] = [0,0,1,0] -q_goal[-6:-3] = [0.,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_bridge = ps.numberPaths() -1 -### END bridge cross ##### -# merge all paths -pathId = pId_stairs -#ps.concatenatePath(pathId,pId_stairs) -ps.concatenatePath(pathId,pId_platform) -ps.concatenatePath(pathId,pId_bridge) - - -print("done optimizing.") -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) -pp.dt=0.1 -pp.displayVelocityPath(pathId) -v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP") -pp.dt = 0.01 - - -q_far = q_goal[::] -q_far[2] = -5 -v(q_far) -q_init = q_init_0[::] - - diff --git a/sl1m/planner_scenarios/talos/lp_ramp_path.py b/sl1m/planner_scenarios/talos/lp_ramp_path.py deleted file mode 100644 index 211c11b..0000000 --- a/sl1m/planner_scenarios/talos/lp_ramp_path.py +++ /dev/null @@ -1,134 +0,0 @@ -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.rbprm.talos_abstract import Robot -Robot.urdfName += "_large" - -packageName = 'hpp_environments' -meshPackageName = 'hpp_environments' - -import time -time.sleep(1) - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-3.2,2.5,-0.8,0.3, 1.4,2.]) -# As this scenario only consider walking, we fix the DOF of the torso : -rbprmBuilder.setJointBounds ('torso_1_joint', [0,0]) -rbprmBuilder.setJointBounds ('torso_2_joint', [0.,0.]) -vMax = 0.3# linear velocity bound for the root -aMax = 0.1 # linear acceleration bound for the root -extraDof = 6 -mu=0.5# coefficient of friction -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-4.,4.,-0.1,0.1,-0.1,0.1]) -# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root -rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) -# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) -extraDofBounds = [-vMax,vMax,-vMax,vMax,-10.,10.,-aMax,aMax,-aMax,aMax,-10.,10.] -rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) -indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() - -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -from hpp.gepetto import ViewerFactory -vf = ViewerFactory (ps) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "multicontact/bauzil_ramp_up", "planning", vf,reduceSizes=[0,0.,0.]) -v = vf.createViewer(displayArrows = True) -afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5]) -v.addLandmark(v.sceneName,1) - - - -ps.setParameter("Kinodynamic/velocityBound",vMax) -ps.setParameter("Kinodynamic/accelerationBound",aMax) -# force the orientation of the trunk to match the direction of the motion -ps.setParameter("Kinodynamic/forceYawOrientation",True) -ps.setParameter("Kinodynamic/synchronizeVerticalAxis",True) -ps.setParameter("Kinodynamic/verticalAccelerationBound",10.) -ps.setParameter("DynamicPlanner/sizeFootX",0.2) -ps.setParameter("DynamicPlanner/sizeFootY",0.12) -ps.setParameter("DynamicPlanner/friction",mu) -# sample only configuration with null velocity and acceleration : -ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) -ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) - -# Choosing RBPRM shooter and path validation methods. -ps.selectConfigurationShooter("RbprmShooter") -ps.addPathOptimizer ("RandomShortcutDynamic") -ps.selectPathValidation("RbprmDynamicPathValidation",0.05) -# Choosing kinodynamic methods : -ps.selectSteeringMethod("RBPRMKinodynamic") -ps.selectDistance("Kinodynamic") -ps.selectPathPlanner("DynamicPlanner") - -### BEGIN turn around on the platform ##### -q_init = rbprmBuilder.getCurrentConfig (); -#q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubblem -#q_init [0:3] = [-0.2, 0.2,0.98] ; v(q_init) # between rubble and stairs -q_init [0:3] = [1.7, 0.2, 1.58]; v (q_init) #top of stairs -q_init_0 = q_init[::] -#q_init [0:3] = [1.7, -0.6, 1.58]; v (q_init) #top of stairs -#q_init[3:7] = [0,0,1,0] -q_init[-6:-3] = [0.2,0,0] -q_goal = q_init [::] -# q_goal [0:3] = [1.7, 0.2, 1.58]; v (q_goal) #top of stairs -#q_goal [0:3] = [-1.7, -0.6, 1.58]; v (q_goal) # after bridge -q_goal [0:3] = [1.7, -0.6, 1.58]; v (q_goal) # before bridge -q_goal[3:7] = [0,0,1,0] -q_goal[-6:-3] = [-0.2,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_platform = ps.numberPaths() -1 -### END turn around on the platform ##### -ps.resetGoalConfigs() -### BEGIN bridge cross ##### -q_init = rbprmBuilder.getCurrentConfig (); -q_init = q_goal[::]; v (q_init) #top of stairs -q_goal [0:3] = [-1.7, -0.6, 1.58]; v (q_goal) # after bridge -q_goal[3:7] = [0,0,1,0] -q_goal[-6:-3] = [0.,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -for i in range(5): - ps.optimizePath(ps.numberPaths() -1) - -pId_bridge = ps.numberPaths() -1 -### END bridge cross ##### - -ps.concatenatePath(pId_platform,pId_bridge) -pathId = pId_platform - -print("done optimizing.") -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) -pp.dt=0.1 -pp.displayVelocityPath(pathId) -v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP") -pp.dt = 0.01 - - -q_far = q_goal[::] -q_far[2] = -5 -v(q_far) -q_init = q_init_0[::] - - diff --git a/sl1m/planner_scenarios/talos/lp_slalom_debris.py b/sl1m/planner_scenarios/talos/lp_slalom_debris.py deleted file mode 100644 index e2dd2c9..0000000 --- a/sl1m/planner_scenarios/talos/lp_slalom_debris.py +++ /dev/null @@ -1,226 +0,0 @@ -from hpp.corbaserver.rbprm.talos import Robot -from hpp.gepetto import Viewer -from tools.display_tools import * -import time -from pinocchio import Quaternion -import numpy as np -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate,isclose -from numpy.linalg import norm -import random -from math import isnan -import sl1m.planner_scenarios.talos.slalom_debris as lp -tp = lp.tp -pb, coms, footpos, allfeetpos, res = lp.solve() - - -print("Done.") -import time -Robot.urdfSuffix+="_safeFeet" - -DEFAULT_COM_HEIGHT = 0.86 -Z_AXIS = lp.Z_AXIS -pId = tp.ps.numberPaths() -1 -fullBody = Robot () - -# Set the bounds for the root -fullBody.setJointBounds ("root_joint", [-20,20 ,-20, 20, 0, 2.8]) -fullBody.client.robot.setDimensionExtraConfigSpace(6) -fullBody.client.robot.setExtraConfigSpaceBounds(tp.extraDofBounds) -# add the 6 extraDof for velocity and acceleration (see *_path.py script) -ps = tp.ProblemSolver( fullBody ) - -#load the viewer -v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) - -# load a reference configuration -q_ref = fullBody.referenceConfig_elbowsUp[::] +[0]*6 -fullBody.setCurrentConfig (q_ref) -fullBody.setReferenceConfig(q_ref) -fullBody.setPostureWeights(fullBody.postureWeights[::] + [0]*6) -#fullBody.usePosturalTaskContactCreation(True) - -print("Generate limb DB ...") -tStart = time.time() -# generate databases : - -nbSamples = 10000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "static", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.3) -fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) - - -tGenerate = time.time() - tStart -print("Done.") -print("Databases generated in : "+str(tGenerate)+" s") - -configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() -q_init = q_ref[::] -q_init[0:7] = tp.q_init[0:7] -q_init[2] += q_ref[2] - 0.98 # 0.98 is the q_ref[2] for the rom device -v(q_init) - -fullBody.resetJointsBounds() -from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper - -def quatConfigFromMatrix(m): - quat = Quaternion(m) - return quatToConfig(quat) - -def quatToConfig(quat): - return [quat.x,quat.y,quat.z,quat.w] - -def computeCenterOfSupportPolygon(s): - com = np.zeros(3) - numContacts = float(len(s.getLimbsInContact())) - for limbId in s.getLimbsInContact(): - com += np.array(s.getCenterOfContactForLimb(limbId)[0]) - com /= numContacts - com[2] += DEFAULT_COM_HEIGHT - return com.tolist() - -def projectCoMInSupportPolygon(s): - desiredCOM = computeCenterOfSupportPolygon(s) - #print "try to project state to com position : ",desiredCOM - success = False - maxIt = 20 - print("project state to com : ",desiredCOM) - q_save = s.q()[::] - while not success and maxIt > 0: - success = s.fullBody.projectStateToCOM(s.sId ,desiredCOM, maxNumSample = 0) - maxIt -= 1 - desiredCOM[2] -= 0.005 - print("success = ",success) - print("result = ",s.q()) - if success and isnan(s.q()[0]): # FIXME why does it happen ? - success = False - s.setQ(q_save) - return success - -def tryCreateContactAround(s, eff_id, pos, normal, num_max_sample= 0,rotation = Quaternion.Identity(),num_try = 1000): - sres, succ = StateHelper.addNewContact(s, eff_id, pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation)) - i_try = 0 - x_bounds = [-0.1,0.1] - y_bounds = [-0.05,0.05] - print("try create contact around, first try success : ",succ) - print("result = ",sres.q()) - while not succ and i_try < num_try: - print("try create contact around, try : ",i_try) - x = random.uniform(x_bounds[0],x_bounds[1]) - y = random.uniform(y_bounds[0],y_bounds[1]) - offset = np.matrix([x,y,0]).T - offset = rotation.matrix() * offset - new_pos = pos[::] - for i in range(3): - new_pos[i] += offset[i,0] - sres, succ = StateHelper.addNewContact(s, eff_id, new_pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation)) - i_try += 1 - - return sres,succ - -def gen_state(s, pId, num_max_sample = 0, first = False, normal = lp.Z_AXIS, newContact = True ,projectCOM = True): - #~ pId = 6 - phase = pb["phaseData"][pId] - moving = phase["moving"] - movingID = fullBody.lLegId - if moving == lp.RF: - movingID = fullBody.rLegId - print("# gen state for phase Id = ",pId) - if False and pId < len(pb["phaseData"])-1: - quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) - quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"]) - qrot = (quat0.slerp(0.5,quat1)).matrix() - else: - qrot = Quaternion(phase["rootOrientation"]) # rotation of the root, from the guide - - #q_n = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T) - #rot = quatToConfig(qrot * q_n) - if not isclose(normal,Z_AXIS).all(): - qrot = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T) - # ignore guide orientation when normal is not z ... - #rot = quatToConfig(qrot) - pos = allfeetpos[pId]; - pos[2] += 0.002 - pose = pos.tolist()+quatToConfig(qrot) - print("Try to add contact for "+movingID+" pos = "+str(pose)) - disp.moveSphere('c',v,pose) - if newContact: - sres, succ = tryCreateContactAround(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample,rotation = qrot) - #sres, succ = StateHelper.removeContact(s,movingID) - #assert succ - #succ = sres.projectToRoot(s.q()[0:3]+rot) - #sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample) - else: - sres, succ = StateHelper.cloneState(s) - if not succ: - print("Cannot project config q = ",sres.q()) - print("To new contact position for "+movingID+" = "+str(pose)+" ; n = "+str(normal.tolist())) - raise RuntimeError("Cannot project feet to new contact position") # try something else ?? - if projectCOM : - #sfeet, _ = StateHelper.cloneState(sres) - #print "config before projecting to com q1=",sres.q() - successCOM = projectCoMInSupportPolygon(sres) - if not successCOM: - # is it really an issue ? - print("Unable to project CoM in the center of the support polygone") - - v(sres.q()) - return sres - -import tools.display_tools as disp -disp.createSphere("c",v,color = v.color.green) -v.addLandmark("c",0.1) - - - -q = fullBody.getCurrentConfig() -v(q) -s = State(fullBody, q = q, limbsIncontact = [fullBody.lLegId, fullBody.rLegId]) - -idfirst = 2 -#coms[0] = array(s.getCenterOfMass()) -coms[0][2] -= 0.02 - - -def normal(phase): - s = phase["S"][0] - i = 0 - n = np.zeros(3) - while i < s.shape[1]-3 and norm(n) < 1e-4 : - n = cross(s[:,i+1] - s[:,i+0], s[:,i+2] - s[:,i+0]) - i+=1 - if norm(n) < 1e-4: - return Z_AXIS - n /= norm(n) - if n[2] < 0.: - for i in range(3): - n[i] = -n[i] - #~ print "normal ", n - return n - -all_states = [s] -sprev = s -for i in range(2, len(pb["phaseData"])): - n = normal(pb["phaseData"][i]) - snew = gen_state(sprev, i , num_max_sample = 100, first = False, normal = n ) - all_states += [snew] - sprev = snew - -configs = [ st.q() for st in all_states[:]]; i = 0 -#displayContactSequence(v,configs) - - -print("SID ", [s.sId for s in all_states]) - -beginId = 0 - - -""" -import tools.display_tools as disp -disp.createSphere("c",v,color = v.color.green) -v.addLandmark("c",0.1) - -disp.moveSphere('c',v,p) -""" - diff --git a/sl1m/planner_scenarios/talos/lp_slalom_debris_path.py b/sl1m/planner_scenarios/talos/lp_slalom_debris_path.py deleted file mode 100644 index 1220899..0000000 --- a/sl1m/planner_scenarios/talos/lp_slalom_debris_path.py +++ /dev/null @@ -1,149 +0,0 @@ -from hpp.gepetto import Viewer -from hpp.corbaserver import Client -from hpp.corbaserver.rbprm.talos_abstract import Robot -#Robot.urdfName += "_large" - -packageName = 'hpp_environments' -meshPackageName = 'hpp_environments' - -import time -time.sleep(1) - -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-5,5,-5,5, rbprmBuilder.ref_height,rbprmBuilder.ref_height]) -# As this scenario only consider walking, we fix the DOF of the torso : -rbprmBuilder.setJointBounds ('torso_1_joint', [0,0]) -rbprmBuilder.setJointBounds ('torso_2_joint', [0.,0.]) -vMax = 1.# linear velocity bound for the root -aMax = 2. # linear acceleration bound for the root -extraDof = 6 -mu=10.# coefficient of friction -rbprmBuilder.setFilter([]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-4.,4.,-0.1,0.1,-0.1,0.1]) -# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root -rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) -# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) -extraDofBounds = [-vMax,vMax,-vMax,vMax,-10.,10.,-aMax,aMax,-aMax,aMax,-10.,10.] -rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) -indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() - -from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -from hpp.gepetto import ViewerFactory -vf = ViewerFactory (ps) - -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "multicontact/slalom_debris", "planning", vf,reduceSizes=[0.,0.,0.]) -v = vf.createViewer(displayArrows = True) -afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5]) -v.addLandmark(v.sceneName,1) - - -# force the orientation of the trunk to match the direction of the motion -ps.setParameter("Kinodynamic/forceYawOrientation",True) -ps.setParameter("Kinodynamic/synchronizeVerticalAxis",True) -ps.setParameter("Kinodynamic/verticalAccelerationBound",10.) -ps.setParameter("DynamicPlanner/sizeFootX",0.2) -ps.setParameter("DynamicPlanner/sizeFootY",0.12) -ps.setParameter("DynamicPlanner/friction",mu) -# sample only configuration with null velocity and acceleration : -ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) -ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) - -# Choosing RBPRM shooter and path validation methods. -ps.selectConfigurationShooter("RbprmShooter") -#ps.addPathOptimizer ("RandomShortcutDynamic") -ps.selectPathValidation("RbprmPathValidation",0.05) -# Choosing kinodynamic methods : -ps.selectSteeringMethod("RBPRMKinodynamic") -ps.selectDistance("Kinodynamic") -ps.selectPathPlanner("DynamicPlanner") - -### BEGIN up to the rubbles ##### -ps.setParameter("Kinodynamic/velocityBound",0.15) -ps.setParameter("Kinodynamic/accelerationBound",0.1) -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-1.8, 0., rbprmBuilder.ref_height]; v (q_init) -q_init[-6:-3] = [0.1,0,0] -q_goal = q_init [::] -q_goal [0:3] = [-0.9, 0.95, rbprmBuilder.ref_height]; v (q_goal) -q_goal[-6:-3] = [0.1,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -q_init_0 = q_init[::] -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -#for i in range(5): -# ps.optimizePath(ps.numberPaths() -1) - -pId_begin = ps.numberPaths() -1 -### END BEGIN up to the rubbles ##### -ps.resetGoalConfigs() -### BEGIN rubbles ##### -ps.setParameter("Kinodynamic/velocityBound",0.4) -ps.setParameter("Kinodynamic/accelerationBound",0.1) -q_init = rbprmBuilder.getCurrentConfig (); -q_init = q_goal[::]; v (q_init) -#q_init[-6:-3] = [0.,0,0] -q_goal [0:3] = [1.05, 0.95,rbprmBuilder.ref_height]; v (q_goal) -q_goal[-6:-3] = [0.1,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -#for i in range(5): -# ps.optimizePath(ps.numberPaths() -1) - -pId_rubbles = ps.numberPaths() -1 -### END rubbles ##### -ps.resetGoalConfigs() -### BEGIN after rubbles ##### -ps.setParameter("Kinodynamic/velocityBound",0.15) -ps.setParameter("Kinodynamic/accelerationBound",0.1) -q_init = rbprmBuilder.getCurrentConfig (); -q_init = q_goal[::]; v (q_init) -q_goal [0:3] = [2.2, 0, rbprmBuilder.ref_height]; v (q_goal) -q_goal[-6:-3] = [0.05,0,0] -ps.setInitialConfig (q_init) -ps.addGoalConfig (q_goal) -v(q_goal) - -t = ps.solve () -print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) -#for i in range(5): -# ps.optimizePath(ps.numberPaths() -1) - -pId_end = ps.numberPaths() -1 -### END after rubbles ##### -pathId = pId_begin -ps.concatenatePath(pathId,pId_rubbles) -ps.concatenatePath(pathId,pId_end) - - -print("done optimizing.") -from hpp.gepetto import PathPlayer -pp = PathPlayer (v) -pp.dt=0.1 -pp.displayVelocityPath(pathId) -v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP") -pp.dt = 0.01 - - -q_far = q_goal[::] -q_far[2] = -5 -v(q_far) -q_init = q_init_0[::] - - diff --git a/sl1m/planner_scenarios/talos/maze.py b/sl1m/planner_scenarios/talos/maze.py deleted file mode 100644 index 2850169..0000000 --- a/sl1m/planner_scenarios/talos/maze.py +++ /dev/null @@ -1,108 +0,0 @@ -import numpy as np -print("Plan guide trajectory ...") -import scenarios.sandbox.talos_maze_path as tp -v = tp.v -ps = tp.ps -root_init = tp.q_init -print("Guide planned.") -from tools.surfaces_from_path import getSurfacesFromGuideContinuous - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm -import random -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * -from sl1m.planner_scenarios.talos.constraints import * - -Z_AXIS = np.array([0,0,1]).T - - -def gen_pb(root_init,R, surfaces): - - nphases = len(surfaces) - lf_0 = array(root_init[0:3]) + array([0, 0.085,-0.98]) # values for talos ! - rf_0 = array(root_init[0:3]) + array([0,-0.085,-0.98]) # values for talos ! - p0 = [lf_0,rf_0]; - print("p0 used : ",p0) - - res = { "p0" : p0, "c0" : None, "nphases": nphases} - #res = { "p0" : None, "c0" : None, "nphases": nphases} - - #print "surfaces = ",surfaces - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [genKinematicConstraints(left_foot_constraints,right_foot_constraints,index = i, rotation = R, min_height = 0.3) for _ in range(len(surfaces[i]))], "relativeK" : [genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints,index = i, rotation = R)[(i) % 2] for _ in range(len(surfaces[i]))], "rootOrientation" : R[i], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - success = False - maxIt = 50 - it = 0 - defaultStep = 0.5 - step = defaultStep - variation = 0.4 - while not success and it < maxIt: - if it > 0 : - step = defaultStep + random.uniform(-variation,variation) - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,step,True,False) - pb = gen_pb(tp.q_init,R,surfaces) - try: - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None) - success = True - except : - print("## Planner failed at iter : "+str(it)+" with step length = "+str(step)) - it += 1 - if not success : - raise RuntimeError("planner always fail.") - return pb, coms, footpos, allfeetpos, res - -if __name__ == '__main__': - pb, coms, footpos, allfeetpos, res = solve() - diff --git a/sl1m/planner_scenarios/talos/path_files/lp_complex_path.py b/sl1m/planner_scenarios/talos/path_files/lp_complex_path.py new file mode 100644 index 0000000..b8b0245 --- /dev/null +++ b/sl1m/planner_scenarios/talos/path_files/lp_complex_path.py @@ -0,0 +1,174 @@ +from hpp.gepetto import PathPlayer, Viewer, ViewerFactory +from hpp.corbaserver import Client +from hpp.corbaserver.affordance.affordance import AffordanceTool +from hpp.corbaserver.problem_solver import ProblemSolver +from talos_rbprm.talos_abstract import Robot as TalosAbstract +import time + +TalosAbstract.urdfName += "_large" + + +def compute_path(): + talos_abstract = TalosAbstract() + talos_abstract.setJointBounds("root_joint", [-3.2, 1.8, 0.19, 0.21, 0.95, 1.7]) + # As this scenario only consider walking, we fix the DOF of the torso : + talos_abstract.setJointBounds("torso_1_joint", [0, 0]) + talos_abstract.setJointBounds("torso_2_joint", [0.0, 0.0]) + vMax = 1.0 # linear velocity bound for the root + aMax = 2.0 # linear acceleration bound for the root + extraDof = 6 + mu = 0.5 # coefficient of friction + talos_abstract.setFilter([TalosAbstract.rLegId, TalosAbstract.lLegId]) + + talos_abstract.setAffordanceFilter( + TalosAbstract.rLegId, + [ + "Support", + ], + ) + talos_abstract.setAffordanceFilter(TalosAbstract.lLegId, ["Support"]) + talos_abstract.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) + # Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root + talos_abstract.client.robot.setDimensionExtraConfigSpace(extraDof) + # We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) + extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, + ] + talos_abstract.client.robot.setExtraConfigSpaceBounds(extraDofBounds) + ( + talos_abstract.getConfigSize() + - talos_abstract.client.robot.getDimensionExtraConfigSpace() + ) + + ps = ProblemSolver(talos_abstract) + vf = ViewerFactory(ps) + + afftool = AffordanceTool() + afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) + afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/bauzil_ramp_simplified.urdf", + "planning", + vf, + reduceSizes=[0.07, 0.0, 0.0], + ) + v = vf.createViewer(displayArrows=True) + afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) + v.addLandmark(v.sceneName, 1) + + ps.setParameter("Kinodynamic/velocityBound", vMax) + ps.setParameter("Kinodynamic/accelerationBound", aMax) + # force the orientation of the trunk to match the direction of the motion + ps.setParameter("Kinodynamic/forceYawOrientation", True) + ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) + ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) + ps.setParameter("DynamicPlanner/sizeFootX", 0.2) + ps.setParameter("DynamicPlanner/sizeFootY", 0.12) + ps.setParameter("DynamicPlanner/friction", mu) + # sample only configuration with null velocity and acceleration : + ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) + ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) + # Choosing RBPRM shooter and path validation methods. + ps.selectConfigurationShooter("RbprmShooter") + ps.addPathOptimizer("RandomShortcutDynamic") + ps.selectPathValidation("RbprmPathValidation", 0.05) + # Choosing kinodynamic methods : + ps.selectSteeringMethod("RBPRMKinodynamic") + ps.selectDistance("Kinodynamic") + ps.selectPathPlanner("DynamicPlanner") + + ### BEGIN climb the stairs ##### + ps.setParameter("Kinodynamic/velocityBound", 0.3) + ps.setParameter("Kinodynamic/accelerationBound", 0.1) + q_init = talos_abstract.getCurrentConfig() + q_init[0:3] = [-0.3, 0.2, 0.98] + v(q_init) # between rubble and stairs + q_goal = q_init[::] + q_goal[0:3] = [1.7, 0.2, 1.58] + v(q_goal) # top of stairs + q_goal[-6:-3] = [0, 0, 0] + ps.setInitialConfig(q_init) + q_init_0 = q_init[::] + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_stairs = ps.numberPaths() - 1 + ### END climb the stairs ##### + talos_abstract.setJointBounds("root_joint", [-3.2, 2.5, -0.8, 0.3, 1.4, 2.0]) + ps.resetGoalConfigs() + ### BEGIN turn around on the platform ##### + ps.setParameter("Kinodynamic/velocityBound", 0.2) + ps.setParameter("Kinodynamic/accelerationBound", 0.07) + q_init = talos_abstract.getCurrentConfig() + q_init = q_goal[::] + v(q_init) # top of stairs + q_init[-6:-3] = [0.2, 0, 0] + q_goal = q_init[::] + q_goal[0:3] = [1.7, -0.6, 1.58] + v(q_goal) # before bridge + q_goal[3:7] = [0, 0, 1, 0] + q_goal[-6:-3] = [-0.2, 0, 0] + ps.setInitialConfig(q_init) + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_platform = ps.numberPaths() - 1 + ### END turn around on the platform ##### + ps.resetGoalConfigs() + ### BEGIN bridge cross ##### + ps.setParameter("Kinodynamic/velocityBound", 0.3) + ps.setParameter("Kinodynamic/accelerationBound", 0.2) + q_init = talos_abstract.getCurrentConfig() + q_init = q_goal[::] + v(q_init) # top of stairs + q_goal[0:3] = [-1.7, -0.6, 1.58] + v(q_goal) # after bridge + q_goal[3:7] = [0, 0, 1, 0] + q_goal[-6:-3] = [0.0, 0, 0] + ps.setInitialConfig(q_init) + ps.addGoalConfig(q_goal) + v(q_goal) + + ps.solve() + print("done planning, optimize path ...") + for _ in range(5): + ps.optimizePath(ps.numberPaths() - 1) + + pId_bridge = ps.numberPaths() - 1 + pathId = pId_stairs + ps.concatenatePath(pathId, pId_platform) + ps.concatenatePath(pathId, pId_bridge) + + print("done optimizing.") + pp = PathPlayer(v) + pp.dt = 0.1 + pp.displayVelocityPath(pathId) + v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") + pp.dt = 0.01 + + q_far = q_goal[::] + q_far[2] = -5 + v(q_far) + q_init = q_init_0[::] + + return talos_abstract, ps, afftool, pathId, v, q_init diff --git a/sl1m/planner_scenarios/talos/path_files/lp_ramp_path.py b/sl1m/planner_scenarios/talos/path_files/lp_ramp_path.py new file mode 100644 index 0000000..829e1a0 --- /dev/null +++ b/sl1m/planner_scenarios/talos/path_files/lp_ramp_path.py @@ -0,0 +1,167 @@ +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from talos_rbprm.talos_abstract import Robot + +Robot.urdfName += "_large" + +packageName = "hpp_environments" +meshPackageName = "hpp_environments" + +import time + +time.sleep(1) + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-3.2, 2.5, -0.8, 0.3, 1.4, 2.0]) +# As this scenario only consider walking, we fix the DOF of the torso : +rbprmBuilder.setJointBounds("torso_1_joint", [0, 0]) +rbprmBuilder.setJointBounds("torso_2_joint", [0.0, 0.0]) +vMax = 0.3 # linear velocity bound for the root +aMax = 0.1 # linear acceleration bound for the root +extraDof = 6 +mu = 0.5 # coefficient of friction +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) +# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root +rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) +# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) +extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, +] +rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) +indexECS = ( + rbprmBuilder.getConfigSize() + - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() +) + +from hpp.corbaserver.problem_solver import ProblemSolver + +ps = ProblemSolver(rbprmBuilder) +from hpp.gepetto import ViewerFactory + +vf = ViewerFactory(ps) + +from hpp.corbaserver.affordance.affordance import AffordanceTool + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/bauzil_ramp_up.urdf", + "planning", + vf, + reduceSizes=[0, 0.0, 0.0], +) +v = vf.createViewer(displayArrows=True) +afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) +v.addLandmark(v.sceneName, 1) + + +ps.setParameter("Kinodynamic/velocityBound", vMax) +ps.setParameter("Kinodynamic/accelerationBound", aMax) +# force the orientation of the trunk to match the direction of the motion +ps.setParameter("Kinodynamic/forceYawOrientation", True) +ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) +ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) +ps.setParameter("DynamicPlanner/sizeFootX", 0.2) +ps.setParameter("DynamicPlanner/sizeFootY", 0.12) +ps.setParameter("DynamicPlanner/friction", mu) +# sample only configuration with null velocity and acceleration : +ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) +ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) + +# Choosing RBPRM shooter and path validation methods. +ps.selectConfigurationShooter("RbprmShooter") +ps.addPathOptimizer("RandomShortcutDynamic") +ps.selectPathValidation("RbprmDynamicPathValidation", 0.05) +# Choosing kinodynamic methods : +ps.selectSteeringMethod("RBPRMKinodynamic") +ps.selectDistance("Kinodynamic") +ps.selectPathPlanner("DynamicPlanner") + +### BEGIN turn around on the platform ##### +q_init = rbprmBuilder.getCurrentConfig() +# q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubblem +# q_init [0:3] = [-0.2, 0.2,0.98] ; v(q_init) # between rubble and stairs +q_init[0:3] = [1.7, 0.2, 1.58] +v(q_init) # top of stairs +q_init_0 = q_init[::] +# q_init [0:3] = [1.7, -0.6, 1.58]; v (q_init) #top of stairs +# q_init[3:7] = [0,0,1,0] +q_init[-6:-3] = [0.2, 0, 0] +q_goal = q_init[::] +# q_goal [0:3] = [1.7, 0.2, 1.58]; v (q_goal) #top of stairs +# q_goal [0:3] = [-1.7, -0.6, 1.58]; v (q_goal) # after bridge +q_goal[0:3] = [1.7, -0.6, 1.58] +v(q_goal) # before bridge +q_goal[3:7] = [0, 0, 1, 0] +q_goal[-6:-3] = [-0.2, 0, 0] +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +v(q_goal) + +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) +for i in range(5): + ps.optimizePath(ps.numberPaths() - 1) + +pId_platform = ps.numberPaths() - 1 +### END turn around on the platform ##### +ps.resetGoalConfigs() +### BEGIN bridge cross ##### +q_init = rbprmBuilder.getCurrentConfig() +q_init = q_goal[::] +v(q_init) # top of stairs +q_goal[0:3] = [-1.7, -0.6, 1.58] +v(q_goal) # after bridge +q_goal[3:7] = [0, 0, 1, 0] +q_goal[-6:-3] = [0.0, 0, 0] +ps.setInitialConfig(q_init) +ps.addGoalConfig(q_goal) +v(q_goal) + +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) +for i in range(5): + ps.optimizePath(ps.numberPaths() - 1) + +pId_bridge = ps.numberPaths() - 1 +### END bridge cross ##### + +ps.concatenatePath(pId_platform, pId_bridge) +pathId = pId_platform + +print("done optimizing.") +from hpp.gepetto import PathPlayer + +pp = PathPlayer(v) +pp.dt = 0.1 +pp.displayVelocityPath(pathId) +v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") +pp.dt = 0.01 + + +q_far = q_goal[::] +q_far[2] = -5 +v(q_far) +q_init = q_init_0[::] diff --git a/sl1m/planner_scenarios/talos/lp_rubbles_path.py b/sl1m/planner_scenarios/talos/path_files/lp_rubbles_path.py similarity index 60% rename from sl1m/planner_scenarios/talos/lp_rubbles_path.py rename to sl1m/planner_scenarios/talos/path_files/lp_rubbles_path.py index 7011f03..bd427ea 100644 --- a/sl1m/planner_scenarios/talos/lp_rubbles_path.py +++ b/sl1m/planner_scenarios/talos/path_files/lp_rubbles_path.py @@ -1,66 +1,96 @@ from hpp.gepetto import Viewer from hpp.corbaserver import Client -from hpp.corbaserver.rbprm.talos_abstract import Robot +from talos_rbprm.talos_abstract import Robot + Robot.urdfName += "_large" -packageName = 'hpp_environments' -meshPackageName = 'hpp_environments' +packageName = "hpp_environments" +meshPackageName = "hpp_environments" import time + time.sleep(1) -rbprmBuilder = Robot () -rbprmBuilder.setJointBounds ("root_joint", [-3.2,1.8,0.19,0.21, 0.95,1.7]) +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-3.2, 1.8, 0.19, 0.21, 0.95, 1.7]) # As this scenario only consider walking, we fix the DOF of the torso : -rbprmBuilder.setJointBounds ('torso_1_joint', [0,0]) -rbprmBuilder.setJointBounds ('torso_2_joint', [0.,0.]) -vMax = 1.# linear velocity bound for the root -aMax = 2. # linear acceleration bound for the root +rbprmBuilder.setJointBounds("torso_1_joint", [0, 0]) +rbprmBuilder.setJointBounds("torso_2_joint", [0.0, 0.0]) +vMax = 1.0 # linear velocity bound for the root +aMax = 2.0 # linear acceleration bound for the root extraDof = 6 -mu=0.5# coefficient of friction -rbprmBuilder.setFilter([Robot.rLegId,Robot.lLegId]) - -rbprmBuilder.setAffordanceFilter(Robot.rLegId, ['Support',]) -rbprmBuilder.setAffordanceFilter(Robot.lLegId, ['Support']) -rbprmBuilder.boundSO3([-4.,4.,-0.1,0.1,-0.1,0.1]) +mu = 0.5 # coefficient of friction +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) # Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) # We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) -extraDofBounds = [-vMax,vMax,-vMax,vMax,-10.,10.,-aMax,aMax,-aMax,aMax,-10.,10.] +extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, +] rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) -indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() +indexECS = ( + rbprmBuilder.getConfigSize() + - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() +) from hpp.corbaserver.problem_solver import ProblemSolver -ps = ProblemSolver( rbprmBuilder ) -from hpp.gepetto import ViewerFactory -vf = ViewerFactory (ps) -from hpp.corbaserver.affordance.affordance import AffordanceTool -afftool = AffordanceTool () -afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) -afftool.loadObstacleModel (packageName, "multicontact/bauzil_ramp_simplified", "planning", vf)#,reduceSizes=[0.05,0.,0.]) -v = vf.createViewer(displayArrows = True) -afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5]) -v.addLandmark(v.sceneName,1) +ps = ProblemSolver(rbprmBuilder) +from hpp.gepetto import ViewerFactory +vf = ViewerFactory(ps) +from hpp.corbaserver.affordance.affordance import AffordanceTool -ps.setParameter("Kinodynamic/velocityBound",vMax) -ps.setParameter("Kinodynamic/accelerationBound",aMax) +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/bauzil_ramp_simplified.urdf", + "planning", + vf, + reduceSizes=[0.0, 0.0, 0.0], +) +v = vf.createViewer(displayArrows=True) +afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) +v.addLandmark(v.sceneName, 1) + + +ps.setParameter("Kinodynamic/velocityBound", vMax) +ps.setParameter("Kinodynamic/accelerationBound", aMax) # force the orientation of the trunk to match the direction of the motion -ps.setParameter("Kinodynamic/forceYawOrientation",True) -ps.setParameter("Kinodynamic/synchronizeVerticalAxis",True) -ps.setParameter("Kinodynamic/verticalAccelerationBound",10.) -ps.setParameter("DynamicPlanner/sizeFootX",0.2) -ps.setParameter("DynamicPlanner/sizeFootY",0.12) -ps.setParameter("DynamicPlanner/friction",mu) +ps.setParameter("Kinodynamic/forceYawOrientation", True) +ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) +ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) +ps.setParameter("DynamicPlanner/sizeFootX", 0.2) +ps.setParameter("DynamicPlanner/sizeFootY", 0.12) +ps.setParameter("DynamicPlanner/friction", mu) # sample only configuration with null velocity and acceleration : -ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) -ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) +ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) +ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) # Choosing RBPRM shooter and path validation methods. ps.selectConfigurationShooter("RbprmShooter") -ps.addPathOptimizer ("RandomShortcutDynamic") -ps.selectPathValidation("RbprmPathValidation",0.05) +ps.addPathOptimizer("RandomShortcutDynamic") +ps.selectPathValidation("RbprmPathValidation", 0.05) # Choosing kinodynamic methods : ps.selectSteeringMethod("RBPRMKinodynamic") ps.selectDistance("Kinodynamic") @@ -68,30 +98,32 @@ ### BEGIN rubbles ##### -ps.setParameter("Kinodynamic/velocityBound",0.3) -ps.setParameter("Kinodynamic/accelerationBound",1.) -q_init = rbprmBuilder.getCurrentConfig (); -q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubbles -q_goal = q_init [::] -q_goal [0:3] = [-0.3, 0.2,0.98] ; v(q_goal) # between rubble and stairs -q_goal[-6:-3] = [0.1,0,0] -ps.setInitialConfig (q_init) +ps.setParameter("Kinodynamic/velocityBound", 0.3) +ps.setParameter("Kinodynamic/accelerationBound", 1.0) +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-3.1, 0.2, 0.98] +v(q_init) # before rubbles +q_goal = q_init[::] +q_goal[0:3] = [-0.3, 0.2, 0.98] +v(q_goal) # between rubble and stairs +q_goal[-6:-3] = [0.1, 0, 0] +ps.setInitialConfig(q_init) q_init_0 = q_init[::] -ps.addGoalConfig (q_goal) +ps.addGoalConfig(q_goal) v(q_goal) q_init_0 = q_init[::] -t = ps.solve () +t = ps.solve() print("done planning, optimize path ...") -#v.solveAndDisplay('rm',2,0.005) +# v.solveAndDisplay('rm',2,0.005) for i in range(5): - ps.optimizePath(ps.numberPaths() -1) + ps.optimizePath(ps.numberPaths() - 1) -pathId = ps.numberPaths() -1 -#pId_rubbles = ps.numberPaths() -1 +pathId = ps.numberPaths() - 1 +# pId_rubbles = ps.numberPaths() -1 ### END rubbles ##### -#ps.resetGoalConfigs() -#pathId = pId_rubbles +# ps.resetGoalConfigs() +# pathId = pId_rubbles """ @@ -181,10 +213,11 @@ print("done optimizing.") from hpp.gepetto import PathPlayer -pp = PathPlayer (v) -pp.dt=0.1 + +pp = PathPlayer(v) +pp.dt = 0.1 pp.displayVelocityPath(pathId) -v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP") +v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") pp.dt = 0.01 @@ -192,5 +225,3 @@ q_far[2] = -5 v(q_far) q_init = q_init_0[::] - - diff --git a/sl1m/planner_scenarios/talos/path_files/lp_stairs_path.py b/sl1m/planner_scenarios/talos/path_files/lp_stairs_path.py new file mode 100644 index 0000000..f4e0c45 --- /dev/null +++ b/sl1m/planner_scenarios/talos/path_files/lp_stairs_path.py @@ -0,0 +1,166 @@ +from hpp.gepetto import Viewer +from hpp.corbaserver import Client +from talos_rbprm.talos_abstract import Robot + +Robot.urdfName += "_large" + +packageName = "hpp_environments" +meshPackageName = "hpp_environments" + +import time + +time.sleep(1) + +rbprmBuilder = Robot() +rbprmBuilder.setJointBounds("root_joint", [-3.2, 1.8, 0.19, 0.21, 0.95, 1.7]) +# As this scenario only consider walking, we fix the DOF of the torso : +rbprmBuilder.setJointBounds("torso_1_joint", [0, 0]) +rbprmBuilder.setJointBounds("torso_2_joint", [0.0, 0.0]) +vMax = 1.0 # linear velocity bound for the root +aMax = 2.0 # linear acceleration bound for the root +extraDof = 6 +mu = 0.5 # coefficient of friction +rbprmBuilder.setFilter([Robot.rLegId, Robot.lLegId]) + +rbprmBuilder.setAffordanceFilter( + Robot.rLegId, + [ + "Support", + ], +) +rbprmBuilder.setAffordanceFilter(Robot.lLegId, ["Support"]) +rbprmBuilder.boundSO3([-4.0, 4.0, -0.1, 0.1, -0.1, 0.1]) +# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root +rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) +# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) +extraDofBounds = [ + -vMax, + vMax, + -vMax, + vMax, + -10.0, + 10.0, + -aMax, + aMax, + -aMax, + aMax, + -10.0, + 10.0, +] +rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) +indexECS = ( + rbprmBuilder.getConfigSize() + - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() +) + +from hpp.corbaserver.problem_solver import ProblemSolver + +ps = ProblemSolver(rbprmBuilder) +from hpp.gepetto import ViewerFactory + +vf = ViewerFactory(ps) + +from hpp.corbaserver.affordance.affordance import AffordanceTool + +afftool = AffordanceTool() +afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) +afftool.loadObstacleModel( + "package://hpp_environments/urdf/multicontact/bauzil_ramp_simplified.urdf", + "planning", + vf, + reduceSizes=[0.07, 0.0, 0.0], +) +v = vf.createViewer(displayArrows=True) +afftool.visualiseAffordances("Support", v, [0.25, 0.5, 0.5]) +v.addLandmark(v.sceneName, 1) + + +ps.setParameter("Kinodynamic/velocityBound", vMax) +ps.setParameter("Kinodynamic/accelerationBound", aMax) +# force the orientation of the trunk to match the direction of the motion +ps.setParameter("Kinodynamic/forceYawOrientation", True) +ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) +ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.0) +ps.setParameter("DynamicPlanner/sizeFootX", 0.2) +ps.setParameter("DynamicPlanner/sizeFootY", 0.12) +ps.setParameter("DynamicPlanner/friction", mu) +# sample only configuration with null velocity and acceleration : +ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) +ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) +# Choosing RBPRM shooter and path validation methods. +ps.selectConfigurationShooter("RbprmShooter") +ps.addPathOptimizer("RandomShortcutDynamic") +ps.selectPathValidation("RbprmPathValidation", 0.05) +# Choosing kinodynamic methods : +ps.selectSteeringMethod("RBPRMKinodynamic") +ps.selectDistance("Kinodynamic") +ps.selectPathPlanner("DynamicPlanner") + +""" +### BEGIN rubbles ##### +ps.setParameter("Kinodynamic/velocityBound",0.3) +ps.setParameter("Kinodynamic/accelerationBound",1.) +q_init = rbprmBuilder.getCurrentConfig (); +q_init [0:3] = [-3.1, 0.2,0.98] ; v(q_init) # before rubbles +q_goal = q_init [::] +q_goal [0:3] = [-0.3, 0.2,0.98] ; v(q_goal) # between rubble and stairs +q_goal[-6:-3] = [0.1,0,0] +ps.setInitialConfig (q_init) +q_init_0 = q_init[::] +ps.addGoalConfig (q_goal) +v(q_goal) +q_init_0 = q_init[::] +t = ps.solve () +print "done planning, optimize path ..." +#v.solveAndDisplay('rm',2,0.005) + +for i in range(5): + ps.optimizePath(ps.numberPaths() -1) + +pId_rubbles = ps.numberPaths() -1 +### END rubbles ##### +ps.resetGoalConfigs() +pathId = pId_rubbles +""" + +### BEGIN climb the stairs ##### +ps.setParameter("Kinodynamic/velocityBound", 0.3) +ps.setParameter("Kinodynamic/accelerationBound", 0.1) +q_init = rbprmBuilder.getCurrentConfig() +q_init[0:3] = [-0.3, 0.2, 0.98] +v(q_init) # between rubble and stairs +# q_init = q_goal[::] ; v(q_init) # between rubble and stairs +# q_init[-6:-3] = [0.15,0,0] +q_goal = q_init[::] +q_goal[0:3] = [1.7, 0.2, 1.58] +v(q_goal) # top of stairs +q_goal[-6:-3] = [0, 0, 0] +ps.setInitialConfig(q_init) +q_init_0 = q_init[::] +ps.addGoalConfig(q_goal) +v(q_goal) + +t = ps.solve() +print("done planning, optimize path ...") +# v.solveAndDisplay('rm',2,0.005) + +for i in range(5): + ps.optimizePath(ps.numberPaths() - 1) + +pathId = ps.numberPaths() - 1 + + +print("done optimizing.") +from hpp.gepetto import PathPlayer + +pp = PathPlayer(v) +pp.dt = 0.1 +pp.displayVelocityPath(pathId) +v.client.gui.setVisibility("path_" + str(pathId) + "_root", "ALWAYS_ON_TOP") +pp.dt = 0.01 + + +q_far = q_goal[::] +q_far[2] = -5 +v(q_far) +q_init = q_init_0[::] diff --git a/sl1m/planner_scenarios/talos/ramp.py b/sl1m/planner_scenarios/talos/ramp.py new file mode 100644 index 0000000..e24aca3 --- /dev/null +++ b/sl1m/planner_scenarios/talos/ramp.py @@ -0,0 +1,101 @@ +import numpy as np +import os +import sl1m.tools.plot_tools as plot +import matplotlib.pyplot as plt +from pathlib import Path +import talos_rbprm + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.stand_alone_scenarios.problem_definition_talos import ( + Problem as TalosProblem, +) +from sl1m.problem_definition import Problem +from sl1m.generic_solver import * + +from time import perf_counter as clock + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_BIPED_PLANNER = True +USE_MIP = True +USE_COM = True + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + + +if __name__ == "__main__": + t_init = clock() + + from sl1m.planner_scenarios.talos import lp_ramp_path as tp + + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + tp.rbprmBuilder, tp.ps, tp.afftool, tp.pathId, tp.v, 0.7, False + ) + t_2 = clock() + + p0 = [ + np.array(tp.q_init[:3]) + [0, 0.085, -0.98], + np.array(tp.q_init[:3]) + [0, -0.085, -0.98], + ] + t_3 = clock() + + if USE_BIPED_PLANNER: + pb = TalosProblem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, [0, 1], p0) + t_4 = clock() + if USE_MIP: + result = solve_MIP_biped(pb) + else: + result = solve_L1_combinatorial_biped(pb, surfaces) + else: + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces_gait, GAIT, p0, tp.q_init[:3]) + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/planner_scenarios/talos/ramp_noGuide.py b/sl1m/planner_scenarios/talos/ramp_noGuide.py deleted file mode 100644 index 0f866aa..0000000 --- a/sl1m/planner_scenarios/talos/ramp_noGuide.py +++ /dev/null @@ -1,158 +0,0 @@ -import numpy as np - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - - -from sl1m.planner import * -from sl1m.planner_scenarios.talos.constraints import * - -from sl1m.tools.plot_plytopes import * - -Z_AXIS = np.array([0,0,1]).T - -begin = array([[1.75, 1.65, 1.65, 1.75], - [0.3, 0.3, 0.1, 0.1], - [0.6, 0.6, 0.6, 0.6]]) - -platform = array([[2.5, 1.5, 1.5, 2.5], - [0.9, 0.9, -1.1, -1.1], - [0.6, 0.6, 0.6, 0.6]]) - -bridge = array([[-1.5, -1.5, 1.5, 1.5], - [-0.5, -0.8, -0.8, -0.5], - [0.6, 0.6, 0.6, 0.6]]) - -end = array([[-1.5, -2.0, -2.0, -1.5], - [-0.4, -0.4, -1.1, -1.1], - [0.6, 0.6, 0.6, 0.6]]) - -### END HARDCODED SURFACES #### - -surfaces = [] -surfaces += [[begin]]+[[begin]] -for i in range(20): - surfaces += [[platform,bridge]] -surfaces += [[end]]+[[end]] - - -def gen_pb(surfaces): - kinematicConstraints = genKinematicConstraints(left_foot_constraints,right_foot_constraints,min_height = 0.3) - relativeConstraints = genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints) - - nphases = len(surfaces) - p0 = [array([ 1.7 , 0.285, 0.6 ]), array([ 1.7 , 0.115, 0.6 ])] - print("p0 used : ",p0) - - #res = { "p0" : p0, "c0" : None, "nphases": nphases} - res = { "p0" : None, "c0" : None, "nphases": nphases} - - print("surfaces = ",surfaces) - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - R,surfaces = getSurfacesFromGuide(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,1.,False) - - pb = gen_pb(tp.q_init,R,surfaces) - - return solveL1(pb, surfaces, draw_scene) - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1 - - pb = gen_pb(surfaces) - """ - import pickle - f = open("pb_platform_bridge_noGuide","w") - pickle.dump(pb,f) - f.close() - """ - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - - -""" -import matplotlib.pyplot as plt -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt, c = 'rand'): - xs = [point[0] for point in points] - ys = [point[1] for point in points] - zs = [point[2] for point in points] - xs = np.append(xs, xs[0]).tolist() - ys = np.append(ys, ys[0]).tolist() - zs = np.append(zs, zs[0]).tolist() - if c == 'rand': ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,c) - plt.draw() - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surface in my_surfaces: plotSurface(surface[0], ax, plt) - return ax - -global my_surfaces -my_surfaces = tp.surfaces -ax = draw_scene() -plt.show() -""" - diff --git a/sl1m/planner_scenarios/talos/rubble_stairs.py b/sl1m/planner_scenarios/talos/rubble_stairs.py deleted file mode 100644 index d0b5a30..0000000 --- a/sl1m/planner_scenarios/talos/rubble_stairs.py +++ /dev/null @@ -1,200 +0,0 @@ -import numpy as np - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - - -from sl1m.planner import * - - -from sl1m.tools.plot_plytopes import * - -Z_AXIS = np.array([0,0,1]).T -from sl1m.planner_scenarios.talos.constraints import * - - -### HARDCODED SURFACES, REPLACE IT WITH PATH PLANNING #### -floor = [[-0.30, 0.54 , 0. ], [-0.1 , 0.54, 0. ], [-0.1 , -0.46, 0. ], [-0.30, -0.46, 0. ], ] -step1 = [[ 0.01, 0.54 , 0.1 ], [0.31 , 0.54, 0.1], [0.31 , -0.46, 0.1 ], [ 0.01, -0.46, 0.1 ], ] -step2 = [[ 0.31, 0.54 , 0.2 ], [0.61 , 0.54, 0.2], [0.61 , -0.46, 0.2 ], [ 0.31, -0.46, 0.2 ], ] -step3 = [[ 0.61, 0.54 , 0.3 ], [0.91 , 0.54, 0.3], [0.91 , -0.46, 0.3 ], [ 0.61, -0.46, 0.3 ], ] -step4 = [[ 0.91, 0.54 , 0.4 ], [1.21 , 0.54, 0.4], [1.21 , -0.46, 0.4 ], [ 0.91, -0.46, 0.4 ], ] -step5 = [[ 1.24, 0.54 , 0.5 ], [1.51 , 0.54, 0.5], [1.51 , -0.46, 0.5 ], [ 1.24, -0.46, 0.5 ], ] -step6 = [[ 1.55, 0.54 , 0.6 ], [1.81 , 0.54, 0.6], [1.81 , -0.46, 0.6 ], [ 1.55, -0.46, 0.6 ], ] -#~ step7 = [[ 1.51, 0.94 , 0.6 ], [2.51 , 0.94, 0.6], [2.51 , -1.06, 0.6 ], [ 1.51, -1.06, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -#~ platfo = [[-1.49, -0.06 , 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.06, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -#~ step8 = [[-1.49, -0.06 , 0.45], [-1.49, 0.24, 0.45],[-2.49, 0.24, 0.45], [-2.49, -0.06, 0.45], ] -#~ step9 = [[-1.49, 0.24 , 0.30], [-1.49, 0.54, 0.30],[-2.49, 0.54, 0.30], [-2.49, 0.24, 0.30], ] -#~ step10= [[-1.49, 0.54 , 0.15], [-1.49, 0.84, 0.15],[-2.49, 0.84, 0.15], [-2.49, 0.54, 0.15], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.19 , 0.05 ], [-2.45 , 0.19, 0.05 ], [ -2.45, 0.53, 0.05 ], [-2.11, 0.53, 0.05 ], ] -rub3 = [[ -1.91, -0.15 , 0.1 ], [-2.25 , -0.15, 0.1 ], [ -2.25, 0.15, 0.1 ], [-1.91, 0.15, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.53, 0.15 ], [-1.69, 0.53, 0.15 ], ] -rub5 = [[ -1.49, -0.15 , 0.2 ], [-1.83 , -0.15, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.53, 0.2 ], [-1.29, 0.53, 0.2 ], ] -rub7 = [[ -1.09, -0.15 , 0.15 ], [-1.43 , -0.15, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.53, 0.1], [-0.89, 0.53, 0.1 ], ] -rub8 = [[ -0.89, -0.15 , 0.025 ], [-1.02 , -0.15, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.15 , 0.025 ], [-0.86 , -0.15, 0.025], [-0.86, 0.52, 0.025 ], [ -0.35, 0.52, 0.025], ] -rub8 = [[ -0.89, -0.15 , 0.05 ], [-1.02 , -0.15, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.45, -0.15 , 0.05 ], [-0.86 , -0.15, 0.05], [-0.86, 0.52, 0.05 ], [ -0.45, 0.52, 0.05], ] - -all_surfaces = [floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -#~ arub1 = array(rub1).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -#~ astep8 = array(step8).T -#~ astep9 = array(step9).T -#~ astep10 = array(step10).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] - -#~ surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1], [astep2,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6],[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] - -#surfaces1 = [ [abridge], [abridge],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[aplatfo],[aplatfo],[aplatfo],[aplatfo]] -surfaces = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1],[astep2],[astep3], [astep4],[astep5],[astep6],[astep6]] - - -### END HARDCODED SURFACES #### - - -def gen_pb(surfaces): - kinematicConstraints = genKinematicConstraints(left_foot_constraints,right_foot_constraints,min_height = None) - relativeConstraints = genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints) - - nphases = len(surfaces) - - p0 = [array([-2.7805096486250154, 0.335, 0.]), array([-2.7805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - print("p0 used : ",p0) - - #res = { "p0" : p0, "c0" : None, "nphases": nphases} - res = { "p0" : None, "c0" : None, "nphases": nphases} - - print("surfaces = ",surfaces) - #TODO in non planar cases, K must be rotated - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - - pb = gen_pb(surfaces) - - return solveL1(pb, surfaces, draw_scene) - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1 ,solveMIP - - pb = gen_pb(surfaces) - solveMIP(pb, surfaces, MIP = True, draw_scene = draw_scene, plot = True) - #pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - - -""" -import matplotlib.pyplot as plt -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt, c = 'rand'): - xs = [point[0] for point in points] - ys = [point[1] for point in points] - zs = [point[2] for point in points] - xs = np.append(xs, xs[0]).tolist() - ys = np.append(ys, ys[0]).tolist() - zs = np.append(zs, zs[0]).tolist() - if c == 'rand': ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,c) - plt.draw() - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surface in my_surfaces: plotSurface(surface[0], ax, plt) - return ax - -global my_surfaces -my_surfaces = tp.surfaces -ax = draw_scene() -plt.show() -""" - diff --git a/sl1m/planner_scenarios/talos/rubbles.py b/sl1m/planner_scenarios/talos/rubbles.py index 676b188..3f1224e 100644 --- a/sl1m/planner_scenarios/talos/rubbles.py +++ b/sl1m/planner_scenarios/talos/rubbles.py @@ -1,210 +1,102 @@ import numpy as np -print("Plan guide trajectory ...") -from . import lp_rubbles_path as tp -#import lp_ramp_path as tp -print("Guide planned.") -from tools.surfaces_from_path import getSurfacesFromGuideContinuous,getSurfacesFromGuide -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - - -from sl1m.planner import * - - -from sl1m.tools.plot_plytopes import * -Z_AXIS = np.array([0,0,1]).T -from sl1m.planner_scenarios.talos.constraints import * - - -""" -### HARDCODED SURFACES, REPLACE IT WITH PATH PLANNING #### -floor = [[-0.30, 0.54 , 0. ], [-0.1 , 0.54, 0. ], [-0.1 , -0.46, 0. ], [-0.30, -0.46, 0. ], ] -step1 = [[ 0.01, 0.54 , 0.1 ], [0.31 , 0.54, 0.1], [0.31 , -0.46, 0.1 ], [ 0.01, -0.46, 0.1 ], ] -step2 = [[ 0.31, 0.54 , 0.2 ], [0.61 , 0.54, 0.2], [0.61 , -0.46, 0.2 ], [ 0.31, -0.46, 0.2 ], ] -step3 = [[ 0.61, 0.54 , 0.3 ], [0.91 , 0.54, 0.3], [0.91 , -0.46, 0.3 ], [ 0.61, -0.46, 0.3 ], ] -step4 = [[ 0.91, 0.54 , 0.4 ], [1.21 , 0.54, 0.4], [1.21 , -0.46, 0.4 ], [ 0.91, -0.46, 0.4 ], ] -step5 = [[ 1.24, 0.54 , 0.5 ], [1.51 , 0.54, 0.5], [1.51 , -0.46, 0.5 ], [ 1.24, -0.46, 0.5 ], ] -step6 = [[ 1.55, 0.54 , 0.6 ], [1.81 , 0.54, 0.6], [1.81 , -0.46, 0.6 ], [ 1.55, -0.46, 0.6 ], ] -#~ step7 = [[ 1.51, 0.94 , 0.6 ], [2.51 , 0.94, 0.6], [2.51 , -1.06, 0.6 ], [ 1.51, -1.06, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -#~ platfo = [[-1.49, -0.06 , 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.06, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -#~ step8 = [[-1.49, -0.06 , 0.45], [-1.49, 0.24, 0.45],[-2.49, 0.24, 0.45], [-2.49, -0.06, 0.45], ] -#~ step9 = [[-1.49, 0.24 , 0.30], [-1.49, 0.54, 0.30],[-2.49, 0.54, 0.30], [-2.49, 0.24, 0.30], ] -#~ step10= [[-1.49, 0.54 , 0.15], [-1.49, 0.84, 0.15],[-2.49, 0.84, 0.15], [-2.49, 0.54, 0.15], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.19 , 0.05 ], [-2.45 , 0.19, 0.05 ], [ -2.45, 0.53, 0.05 ], [-2.11, 0.53, 0.05 ], ] -rub3 = [[ -1.91, -0.15 , 0.1 ], [-2.25 , -0.15, 0.1 ], [ -2.25, 0.15, 0.1 ], [-1.91, 0.15, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.53, 0.15 ], [-1.69, 0.53, 0.15 ], ] -rub5 = [[ -1.49, -0.15 , 0.2 ], [-1.83 , -0.15, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.53, 0.2 ], [-1.29, 0.53, 0.2 ], ] -rub7 = [[ -1.09, -0.15 , 0.15 ], [-1.43 , -0.15, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.53, 0.1], [-0.89, 0.53, 0.1 ], ] -rub8 = [[ -0.89, -0.15 , 0.025 ], [-1.02 , -0.15, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.15 , 0.025 ], [-0.86 , -0.15, 0.025], [-0.86, 0.52, 0.025 ], [ -0.35, 0.52, 0.025], ] -rub8 = [[ -0.89, -0.15 , 0.05 ], [-1.02 , -0.15, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.45, -0.15 , 0.05 ], [-0.86 , -0.15, 0.05], [-0.86, 0.52, 0.05 ], [ -0.45, 0.52, 0.05], ] - -all_surfaces = [floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -#~ arub1 = array(rub1).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -#~ astep8 = array(step8).T -#~ astep9 = array(step9).T -#~ astep10 = array(step10).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] - -#~ surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2],[arub3],allrub,allrub,allrub,allrub,[arub75,arub7,arub9] ,[arub9,afloor],[arub9,afloor],[afloor,arub9,astep1],[astep1,arub9,afloor], [astep2,astep1,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6,astep5,astep7],[astep6,astep5,astep7],[astep6],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] -#surfaces0 = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1], [astep2,astep3,astep4,astep5],[astep3,astep2,astep4,astep5],[astep4,astep1,astep2,astep3,astep5], [astep5,astep4,astep1,astep2,astep3],[astep6,astep5,astep4],[astep6],[astep6],[astep6,],[astep7],[astep7],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,astep7,aplatfo]] - -#surfaces1 = [ [abridge], [abridge],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[abridge,aplatfo],[aplatfo],[aplatfo],[aplatfo],[aplatfo]] -surfaces = [[arub2,arub3],[arub3,arub2],[arub4,arub3,arub5],[arub5,arub4,arub3,arub6],[arub6],[arub7],[arub75] ,[arub9,afloor],[arub9,afloor],[afloor,arub9],[astep1],[astep2],[astep3], [astep4],[astep5],[astep6],[astep6]] -""" - -### END HARDCODED SURFACES #### - - -def gen_pb(root_init,R, surfaces): - - nphases = len(surfaces) - #nphases = 20 - lf_0 = array(root_init[0:3]) + array([0, 0.085,-0.98]) # values for talos ! - rf_0 = array(root_init[0:3]) + array([0,-0.085,-0.98]) # values for talos ! - #p0 = [array([-3.0805096486250154, 0.335, 0.]), array([-3.0805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - #p0 = [array([-0.1805096486250154, 0.335, 0.]), array([-0.1805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - p0 = [lf_0,rf_0]; - print("p0 used : ",p0) - - res = { "p0" : p0, "c0" : None, "nphases": nphases} - #res = { "p0" : None, "c0" : None, "nphases": nphases} - - print("surfaces = ",surfaces) - #TODO in non planar cases, K must be rotated - #phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [genKinematicConstraints(left_foot_constraints,right_foot_constraints,index = i, rotation = R, min_height = 0.3) for _ in range(len(surfaces[i]))], "relativeK" : [genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints,index = i, rotation = R)[(i) % 2] for _ in range(len(surfaces[i]))], "rootOrientation" : R[i], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,1.3,False,True) - - pb = gen_pb(tp.q_init,R,surfaces) - - return solveL1(pb, surfaces, draw_scene) - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1 - - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,tp.v,1.3,False,True) - - pb = gen_pb(tp.q_init,R,surfaces) - - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - - -""" -import matplotlib.pyplot as plt -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt, c = 'rand'): - xs = [point[0] for point in points] - ys = [point[1] for point in points] - zs = [point[2] for point in points] - xs = np.append(xs, xs[0]).tolist() - ys = np.append(ys, ys[0]).tolist() - zs = np.append(zs, zs[0]).tolist() - if c == 'rand': ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,c) - plt.draw() - -def draw_scene(ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surface in my_surfaces: plotSurface(surface[0], ax, plt) - return ax - -global my_surfaces -my_surfaces = tp.surfaces -ax = draw_scene() -plt.show() -""" - +import os +import sl1m.tools.plot_tools as plot +import matplotlib.pyplot as plt +from pathlib import Path +import talos_rbprm + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.stand_alone_scenarios.problem_definition_talos import ( + Problem as TalosProblem, +) +from sl1m.problem_definition import Problem +from sl1m.generic_solver import * + +from time import perf_counter as clock + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_BIPED_PLANNER = False +USE_MIP = False +USE_COM = True + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + + from sl1m.planner_scenarios.talos import lp_rubbles_path as tp + + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + tp.rbprmBuilder, tp.ps, tp.afftool, tp.pathId, tp.v, 0.7, False + ) + t_2 = clock() + + p0 = [ + np.array(tp.q_init[:3]) + [0, 0.085, -0.98], + np.array(tp.q_init[:3]) + [0, -0.085, -0.98], + ] + t_3 = clock() + + if USE_BIPED_PLANNER: + pb = TalosProblem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, [0, 1], p0) + t_4 = clock() + if USE_MIP: + result = solve_MIP_biped(pb) + else: + result = solve_L1_combinatorial_biped(pb, surfaces) + else: + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces_gait, GAIT, p0, tp.q_init[:3]) + t_4 = clock() + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the path takes ", 1000.0 * (t_1 - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_2 - t_1)) + print("Computing the initial contacts takes ", 1000.0 * (t_3 - t_2)) + print("Generating the problem dictionary takes ", 1000.0 * (t_4 - t_3)) + print("Solving the problem takes ", 1000.0 * (t_end - t_4)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/planner_scenarios/talos/run.sh b/sl1m/planner_scenarios/talos/run.sh deleted file mode 100644 index 6c19fae..0000000 --- a/sl1m/planner_scenarios/talos/run.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash - -gepetto-gui & -hpp-rbprm-server & -ipython -i --no-confirm-exit ./$1 - -pkill -f 'gepetto-gui' -pkill -f 'hpp-rbprm-server' diff --git a/sl1m/planner_scenarios/talos/slalom_debris.py b/sl1m/planner_scenarios/talos/slalom_debris.py deleted file mode 100644 index 7f4e8f0..0000000 --- a/sl1m/planner_scenarios/talos/slalom_debris.py +++ /dev/null @@ -1,176 +0,0 @@ -import numpy as np -print("Plan guide trajectory ...") -from . import lp_slalom_debris_path as tp -print("Guide planned.") -from tools.surfaces_from_path import getSurfacesFromGuideContinuous - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm -import random -#~ from scipy.spatial import ConvexHull -#~ from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -from sl1m.planner import * -Z_AXIS = np.array([0,0,1]).T -from sl1m.planner_scenarios.talos.constraints import * -""" -from sl1m.plot_plytopes import * -rubB1 = [[-0.5079353885643417, 1.282387089736744, 0.0021999916093061456], [-0.5079353885643416, 1.2444260597154655, 0.0021999916093061456], [-0.4555532842024058, 1.244426054175804, 0.016235816474940087], [-0.4555533048779398, 1.2823870841970821, 0.016235812378438574]] -rubB2 = [[-0.49591913077286087, 1.1089893205877919, 0.004305334067727976], [-0.4959192866539029, 1.0723211830469714, 0.014130502161835578], [-0.4416887364162, 1.0723216355962044, 0.014130432868663537], [-0.4416885369888308, 1.1089893095084922, 0.004305337036416214]] -rubB3 = [[-0.48205335101433644, 0.8487340468277829, 0.016235827387680896], [-0.42967124312103594, 0.8487340681209311, 0.00219999743865322], [-0.4296712416379183, 0.8866951153800525, 0.002199995597725976], [-0.4820533288556889, 0.886695094086905, 0.016235821450252305]] -rubB4 = [[-0.4959189669199701, 0.6631073957571295, 0.01413046086543412], [-0.49591898907856746, 0.6264397238524806, 0.004305370958339671], [-0.4416886194638495, 0.6264397496095392, 0.004305374972883749], [-0.4416885973052521, 0.6631074215141881, 0.014130464879978225]] -rubB5 = [[-0.1286227022905665, 1.2865674070731667, 0.004305356276006811], [-0.18285298595645427, 1.2865674159019864, 0.004305353910326992], [-0.18285296418321512, 1.2498997897345068, 0.01413043425581112], [-0.1286226893627057, 1.2498997809056869, 0.014130436621490938]] -rubB6 = [[-0.19487018593892436, 1.0642733322579028, 0.002199924194792202], [-0.19487018593892436, 1.026311899523103, 0.002199924194792202], [-0.14248738910025185, 1.0263118953186507, 0.016235892193181743], [-0.14248740407073668, 1.06427332805345 , 0.01623588639038652]] -rubB7 = [[-0.1286224879028442, 0.8406861284205095, 0.014130488309530481], [-0.18285313142627807, 0.8406861372493323, 0.014130490675199364], [-0.18285310965307405, 0.8040180965091135, 0.004305348131801057], [-0.12862247497500423, 0.8040180876802904, 0.00430534576613209]] -rubB8 = [[-0.16898820573177617, 0.6685813451790906, 0.016235902651238112], [-0.16898819670073095, 0.6306198870269437, 0.016235902925035444], [-0.11660536431136716, 0.6306198843501182, 0.002199925819797003], [-0.11660538815137254, 0.6685813156694488, 0.002199931316793472]] - -rubE1 = [[0.47039856640115135, 1.2823872614660115, 0.016235877922373423], [0.4180158210967855, 1.282387267005657, 0.002199926065377139], [0.41801584177237405, 1.2444258935258445, 0.0021999301618671058], [0.47039856640115135, 1.244425887986198, 0.016235877922373423]] -rubE2 = [[0.4300330225380426, 1.1089893042736412, 0.004305344981069587], [0.4300328231105139, 1.0723212368072526, 0.014130497185884688], [0.4842633076431641, 1.0723216893563094, 0.014130427892686833], [0.48426346352412575, 1.1089892931943353, 0.004305347949760364]] -rubE3 = [[0.44389771410174234, 0.8866951647708724, 0.01623582554675447], [0.4438977126186285, 0.8487341064324276, 0.01623582738768091], [0.496279820511929, 0.8487341277255759, 0.002199997438653234], [0.4962798426705747, 0.8866951860640212, 0.0021999915012257676]] -rubE4 = [[0.43003313137984017, 0.6264398056157277, 0.00430537689572764], [0.4842634139015866, 0.6264398313727851, 0.00430538091027495], [0.48426343606019295, 0.6631074589602306, 0.014130458942590155], [0.4300331535384464, 0.6631074332031729, 0.014130454928042817]] -rubE5 = [[0.743098096698756, 1.28656744542173, 0.004305359084522087], [0.7430980523815441, 1.249899816830634, 0.014130440079408849], [0.7973283363876394, 1.2498998683447495, 0.01413043205031209], [0.7973283807048511, 1.2865674969358454, 0.004305351055425384]] -rubE6 = [[0.783463669083787, 1.0642734072201385, 0.016235889639458684], [0.7310808794622428, 1.0642733764552545, 0.0021999259076780076], [0.7310808986548348, 1.0263119698608123, 0.002199928163265169], [0.7834636903246144, 1.0263119469600557, 0.01623589513753257]] -rubE7 = [[0.7430979576180856, 0.8040181510934469, 0.004305346786616704], [0.7973285946775992, 0.8040181510934469, 0.004305346786616704], [0.7973285946775991, 0.8406861907949811, 0.014130489051701167], [0.7430979576180857, 0.8406861907949811, 0.014130489051701167]] -rubE8 = [[0.7569629079334563, 0.6685813931634638, 0.01623589835472744], [0.7569629169645031, 0.6306199582518592, 0.016235898628522177], [0.8093456996032325, 0.6306199589940333, 0.0021999313804602627], [0.8093456885239412, 0.6685813670728219, 0.0021999343491562184]] - - - - -mid = [[0.2525372748550794, 1.188345286893716, 0.019086446287424022], [0.0617591727940063, 1.1883452947119464, 0.019086446287423415], [0.06175921867215623, 0.7002973822902399, 0.019086484191625633], [0.25253731563565707, 0.7002973744720097, 0.01908648419162624]] -beginFloor = [[-1.7 ,0.2 ,0.],[ -0.8,1.2 ,0.],[-0.8,0.5 ,0.],[-1.7 ,-0.4 ,0.]] -endFloor = [[ 1.,1.2 ,0.],[2 ,0.2 ,0.],[2 ,-0.4 ,0.],[1.,0.5 ,0.]] -final = [[2 ,0.1 ,0.],[2.1 ,0.1 ,0.],[2.1 ,-0.1 ,0.],[2 ,-0.1 ,0.]] - -all_surfaces = [beginFloor,rubB1,rubB2,rubB3,rubB4,rubB5,rubB6,rubB7,rubB8,mid,rubE1,rubE2,rubE3,rubE4,rubE5,rubE6,rubE7,rubE8,endFloor] - -arubB1 = array(rubB1).T -arubB2 = array(rubB2).T -arubB3 = array(rubB3).T -arubB4 = array(rubB4).T -arubB5 = array(rubB5).T -arubB6 = array(rubB6).T -arubB7 = array(rubB7).T -arubB8 = array(rubB8).T -arubE1 = array(rubE1).T -arubE2 = array(rubE2).T -arubE3 = array(rubE3).T -arubE4 = array(rubE4).T -arubE5 = array(rubE5).T -arubE6 = array(rubE6).T -arubE7 = array(rubE7).T -arubE8 = array(rubE8).T -amid = array(mid).T -abeginFloor = array(beginFloor).T -aendFloor = array(endFloor).T - - -all_rub_begin = [arubB1,arubB2,arubB3,arubB4,arubB5,arubB6,arubB7,arubB8] -all_rub_and = [arubE1,arubE2,arubE3,arubE4,arubE5,arubE6,arubE7,arubE8] - - -surfaces = [[abeginFloor],[arubB3],[arubB6],[amid],[amid],[arubE3],[arubE6],[aendFloor],[aendFloor]] # work with p0x -0.80 - - -#~ surfaces = [[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[arubB2],[arubB7],[amid],[arubE3],[arubE6],[aendFloor],[aendFloor]] -surfaces = [[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[abeginFloor],[arubB2],[arubB7],[amid],[arubE3],[arubE6],[aendFloor],[aendFloor]] -""" - -final = [[2 ,0.15 ,0.],[2.1 ,0.15 ,0.],[2.1 ,-0.15 ,0.],[2 ,-0.15 ,0.]] -afinal = array(final).T - -def gen_pb(root_init,R, surfaces): - - nphases = len(surfaces) - lf_0 = array(root_init[0:3]) + array([0, 0.085,-0.98]) # values for talos ! - rf_0 = array(root_init[0:3]) + array([0,-0.085,-0.98]) # values for talos ! - #p0 = [array([-3.0805096486250154, 0.335, 0.]), array([-3.0805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - #p0 = [array([-0.1805096486250154, 0.335, 0.]), array([-0.1805096486250154, 0.145,0.])]; ## FIXME : get it from planning too - p0 = [lf_0,rf_0]; - print("p0 used : ",p0) - - res = { "p0" : p0, "c0" : None, "nphases": nphases} - #res = { "p0" : None, "c0" : None, "nphases": nphases} - - #print "surfaces = ",surfaces - #TODO in non planar cases, K must be rotated - #phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [genKinematicConstraints(left_foot_constraints,right_foot_constraints,index = i, rotation = R, min_height = 0.3) for _ in range(len(surfaces[i]))], "relativeK" : [genFootRelativeConstraints(right_foot_in_lf_frame_constraints,left_foot_in_rf_frame_constraints,index = i, rotation = R)[(i) % 2] for _ in range(len(surfaces[i]))], "rootOrientation" : R[i], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def plotSurface (points, ax, plt,color_id = -1): - xs = np.append(points[0,:] ,points[0,0] ).tolist() - ys = np.append(points[1,:] ,points[1,0] ).tolist() - zs = (np.append(points[2,:] ,points[2,0] ) - np.ones(len(xs))*0.005*color_id).tolist() - colors = ['r','g','b','m','y','c'] - if color_id == -1: ax.plot(xs,ys,zs) - else: ax.plot(xs,ys,zs,colors[color_id]) - plt.draw() - -def draw_scene(surfaces,ax = None): - colors = ['r','g','b','m','y','c'] - color_id = 0 - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - #[draw_rectangle(l,ax) for l in all_surfaces] - for surfaces_phase in surfaces: - for surface in surfaces_phase: - plotSurface(surface, ax, plt,color_id) - color_id += 1 - if color_id >= len(colors): - color_id = 0 - return ax - - - -############# main ################### -def solve(): - from sl1m.fix_sparsity import solveL1 - success = False - maxIt = 50 - it = 0 - defaultStep = 1. - step = defaultStep - variation = 0.2 - while not success and it < maxIt: - if it > 0 : - step = defaultStep + random.uniform(-variation,variation) - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,None,step,True) - pb = gen_pb(tp.q_init,R,surfaces) - try: - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, None) - success = True - except : - print("## Planner failed at iter : "+str(it)+" with step length = "+str(step)) - it += 1 - if not success : - raise RuntimeError("planner always fail.") - return pb, coms, footpos, allfeetpos, res - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1 - step = 1. - R,surfaces = getSurfacesFromGuideContinuous(tp.rbprmBuilder,tp.ps,tp.afftool,tp.pathId,None,step,True) - - pb = gen_pb(tp.q_init,R,surfaces) - - pb, coms, footpos, allfeetpos, res = solveL1(pb, surfaces, draw_scene) - diff --git a/sl1m/planner_scenarios/talos/stairs.py b/sl1m/planner_scenarios/talos/stairs.py new file mode 100644 index 0000000..f3fdc30 --- /dev/null +++ b/sl1m/planner_scenarios/talos/stairs.py @@ -0,0 +1,103 @@ +import numpy as np +import os +import sl1m.tools.plot_tools as plot +import matplotlib.pyplot as plt +from pathlib import Path +import talos_rbprm + +from sl1m.rbprm.surfaces_from_planning import getSurfacesFromGuideContinuous +from sl1m.stand_alone_scenarios.problem_definition_talos import ( + Problem as TalosProblem, +) +from sl1m.problem_definition import Problem +from sl1m.generic_solver import * + +from time import perf_counter as clock + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +USE_BIPED_PLANNER = False +USE_MIP = False +USE_COM = True + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + + from sl1m.planner_scenarios.talos import lp_stairs_path as tp + + t_1 = clock() + + R, surfaces = getSurfacesFromGuideContinuous( + tp.rbprmBuilder, tp.ps, tp.afftool, tp.pathId, tp.v, 0.7, False + ) + t_2 = clock() + + p0 = [ + np.array(tp.q_init[:3]) + [0, 0.085, -0.98], + np.array(tp.q_init[:3]) + [0, -0.085, -0.98], + ] + t_3 = clock() + + if USE_BIPED_PLANNER: + pb = TalosProblem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, [0, 1], p0) + t_4 = clock() + if USE_MIP: + result = solve_MIP_biped(pb) + else: + result = solve_L1_combinatorial_biped(pb, surfaces) + else: + surfaces_gait = [[surface] for surface in surfaces] + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces_gait, GAIT, p0, tp.q_init[:3]) + t_4 = clock() + + if USE_MIP: + result = solve_MIP(pb, com=USE_COM) + else: + result = solve_L1_combinatorial(pb, com=USE_COM) + + t_end = clock() + + print(result) + + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the path takes ", 1000.0 * (t_1 - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_2 - t_1)) + print("Computing the initial contacts takes ", 1000.0 * (t_3 - t_2)) + print("Generating the problem dictionary takes ", 1000.0 * (t_4 - t_3)) + print("Solving the problem takes ", 1000.0 * (t_end - t_4)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(surfaces) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/problem_data.py b/sl1m/problem_data.py new file mode 100644 index 0000000..e047611 --- /dev/null +++ b/sl1m/problem_data.py @@ -0,0 +1,29 @@ +class ProblemData: + """ + Class to store the result of the planning problem + """ + + def __init__( + self, + success, + time, + coms=None, + moving_feet_pos=None, + all_feet_pos=None, + surface_indices=None, + ): + self.success = success + self.time = time + self.coms = coms + self.moving_feet_pos = moving_feet_pos + self.all_feet_pos = all_feet_pos + self.surface_indices = surface_indices + + def __str__(self): + string = "ProblemData: " + string += "\n \t Success: " + str(self.success) + string += "\n \t Time: " + str(self.time) + return string + + def __repr__(self): + return self.__str__() diff --git a/sl1m/problem_definition.py b/sl1m/problem_definition.py index 7e6a884..51eb219 100644 --- a/sl1m/problem_definition.py +++ b/sl1m/problem_definition.py @@ -1,83 +1,190 @@ -from sl1m.constants_and_tools import * - -from . import qp - -np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) - -#LP contact planner using inequality formulation - - -############### Problem definition ############# - - -LF = 0 -RF = 1 - -def normalize(Ab): - A = Ab[0] - b = Ab[1] - Ares = zeros(A.shape) - bres = zeros(b.shape) - for i in range(A.shape[0]): - n = norm(A[i,:]) - if n <= 0.000001: - n = 1. - Ares[i,:] = A[i,:] / n; bres[i] = b[i] / n - return Ares, bres - - -# added: align foot orientation with the root orientation -def genKinematicConstraints(lf_constraints_fun, rf_constraints_fun, index = 0, rotation = [Id,Id], normals = [z, z], min_height = None): #assume that root transform is given in 3x3 rotation matrix - res = [None, None] - if index == 0 : - trLF = default_transform_from_pos_normal_(rotation[index], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index], zero3, normals[RF]) - elif index % 2 == LF : # left foot is moving - trLF = default_transform_from_pos_normal_(rotation[index], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index-1], zero3, normals[RF]) - elif index % 2 == RF : # right foot is moving - #print index - trLF = default_transform_from_pos_normal_(rotation[index-1], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index], zero3, normals[RF]) - - #~ KLF = left_foot_talos_constraints (trLF) - #~ KRF = right_foot_talos_constraints (trRF) - #~ KLF = left_foot_hrp2_constraints (trLF) - #~ KRF = right_foot_hrp2_constraints (trRF) - KLF = lf_constraints_fun (trLF) - KRF = rf_constraints_fun (trRF) - if min_height is None: - res [LF] = KLF - res [RF] = KRF - else: - res [LF] = addHeightConstraint(KLF[0], KLF[1], min_height) - res [RF] = addHeightConstraint(KRF[0], KRF[1], min_height) - return res - -# added: align foot orientation with the root orientation -def genFootRelativeConstraints(rf_in_lf_frame_constraints_fun, lf_in_rf_frame_constraints_fun, index = 0, rotation = [Id,Id], normals = [z, z]): #assume that root transform is given in 3x3 rotation matrix - res = [None, None] - if index == 0 : - trLF = default_transform_from_pos_normal_(rotation[index], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index], zero3, normals[RF]) - elif index % 2 == LF : # left foot is moving - trLF = default_transform_from_pos_normal_(rotation[index], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index-1], zero3, normals[RF]) - elif index % 2 == RF : # right foot is moving - trLF = default_transform_from_pos_normal_(rotation[index-1], zero3, normals[LF]) - trRF = default_transform_from_pos_normal_(rotation[index], zero3, normals[RF]) - #~ KRF = right_foot_in_lf_frame_talos_constraints (trLF) - #~ KLF = left_foot_in_rf_frame_talos_constraints (trRF) - - #~ KRF = right_foot_in_lf_frame_hrp2_constraints (trLF) - #~ KLF = left_foot_in_rf_frame_hrp2_constraints (trRF) - - KRF = rf_in_lf_frame_constraints_fun (trLF) - KLF = lf_in_rf_frame_constraints_fun (trRF) - res [LF] = KLF #constraints of right foot in lf frame. Same idea as COM in lf frame - res [RF] = KRF - return res - - -def copyKin(kC): - return [(Kk[0].copy(), Kk[1].copy()) for Kk in kC] +from sl1m.constants_and_tools import ( + default_transform_from_pos_normal, + convert_surface_to_inequality, +) +from sl1m.tools.obj_to_constraints import ( + load_obj, + as_inequalities, + rotate_inequalities, +) +import numpy as np + + +class PhaseData: + """ + phaseData.moving = moving feet in phase i + phaseData.root_orientation = root orientation for phase i + phaseData.S = surfaces of phase i + phaseData.n_surfaces = number of surfaces + phaseData.K = Com constraints for the phase, for each foot and each surface + phaseData.allRelativeK = Relative constraints for the phase for each foot and each surface + """ + + def __init__( + self, i, R, surfaces, gait, normal, n_effectors, com_obj, foot_obj, com + ): + self.id = i + previous_swing_feet = np.nonzero(gait[(i - 1) % len(gait)] == 0)[0] + self.stance = np.nonzero(gait[i % len(gait)] == 1)[0] + self.moving = self.stance[ + np.in1d(self.stance, previous_swing_feet, assume_unique=True) + ] + self.root_orientation = R + self.S = [ + [convert_surface_to_inequality(s, True) for s in foot_surfaces] + for foot_surfaces in surfaces + ] + self.n_surfaces = [len(s) for s in self.S] + if len(self.moving) != len(self.n_surfaces): + raise ArithmeticError( + "Error on the list of surfaces: for each moving foot, provide a list of potential surfaces." + ) + self.transform = default_transform_from_pos_normal(np.zeros(3), normal, R) + if com: + self.generate_K(n_effectors, com_obj) + self.generate_relative_K(n_effectors, foot_obj) + + def generate_K(self, n_effectors, obj): + """ + Generate the constraints on the CoM position for all the effectors as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: com constraint objects + """ + self.K = [] + for foot in range(n_effectors): + ine = rotate_inequalities(obj[foot], self.transform.copy()) + self.K.append((ine.A, ine.b)) + + def generate_relative_K(self, n_effectors, obj): + """ + Generate all the relative position constraints for all limbs as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: foot relative constraints + """ + self.allRelativeK = [] + for foot in range(n_effectors): + foot_res = [] + for other in range(n_effectors): + if other != foot: + ine = rotate_inequalities(obj[foot][other], self.transform.copy()) + foot_res.append((other, (ine.A, ine.b))) + self.allRelativeK += [foot_res] + + +class Problem: + """ + General sl1m problem definition + + pb.n_effectors = number of effectors + pb.p0 = initial feet positions + pb.c0 = initial com positions + pb.nphases = number of phases + pb.phaseData list of Phase data objects + """ + + def __init__( + self, + rbprm_robot=None, + suffix_com="_effector_frame_quasi_static_reduced.obj", + suffix_feet="_reduced.obj", + limb_names=None, + other_names=None, + constraint_paths=None, + ): + effectors = None + kinematic_constraints_path = None + relative_feet_constraints_path = None + + if rbprm_robot is not None: + effectors = rbprm_robot.limbs_names + kinematic_constraints_path = rbprm_robot.kinematic_constraints_path + relative_feet_constraints_path = rbprm_robot.relative_feet_constraints_path + + if limb_names is not None: + effectors = limb_names[:] + + if constraint_paths is not None: + kinematic_constraints_path = constraint_paths[0] + relative_feet_constraints_path = constraint_paths[1] + + self.n_effectors = len(effectors) + self.com_objects = [] + self.foot_objects = [] + for foot, foot_name in enumerate(effectors): + filekin = ( + kinematic_constraints_path + + "COM_constraints_in_" + + foot_name + + suffix_com + ) + self.com_objects.append(as_inequalities(load_obj(filekin))) + + foot_object = [] + for other, other_name in enumerate(effectors): + if other != foot: + if other_names is not None: + o_name = other_names[other] + elif limb_names is not None: + o_name = other_name + else: + o_name = rbprm_robot.dict_limb_joint[ + rbprm_robot.limbs_names[other] + ] + filekin = ( + relative_feet_constraints_path + + o_name + + "_constraints_in_" + + foot_name + + suffix_feet + ) + foot_object.append(as_inequalities(load_obj(filekin))) + else: + foot_object.append(None) + + self.foot_objects.append(foot_object) + + def generate_problem(self, R, surfaces, gait, p0, c0=None, com=True): + """ + Build a SL1M problem for the Mixed Integer formulation, + with all the kinematics and foot relative position constraints required + :param Robot: an rbprm robot + :param R: a list of rotation matrix for the base of the robot (must be the same size as surfaces) + :param surfaces: A list of surfaces candidates, with one set of surface candidates for each phase + :param gait: The gait of the robot (list of id of the moving foot) + :param p0: The initial positions of the limbs + :param c0: The initial position of the com + :return: a "res" dictionary with the format required by SL1M + """ + normal = np.array([0, 0, 1]) + self.p0 = p0 + self.c0 = c0 + self.n_phases = len(surfaces) + self.phaseData = [] + for i in range(self.n_phases): + self.phaseData.append( + PhaseData( + i, + R[i], + surfaces[i], + gait, + normal, + self.n_effectors, + self.com_objects, + self.foot_objects, + com, + ) + ) + + def __str__(self): + string = "Problem: " + string += "\n \t n_effectors: " + str(self.n_effectors) + string += "\n \t n_phases: " + str(self.n_phases) + string += "\n \t p0: " + str(self.p0) + string += "\n \t c0: " + str(self.c0) + for i in range(self.n_phases): + string += "\n \t \t Phase: " + str(i) + string += "\n \t \t moving: " + str(self.phaseData[i].moving) + string += "\n \t \t n_surfaces: " + str(self.phaseData[i].n_surfaces) + return string diff --git a/sl1m/qp.py b/sl1m/qp.py deleted file mode 100644 index 9e563fb..0000000 --- a/sl1m/qp.py +++ /dev/null @@ -1,67 +0,0 @@ -import quadprog -from numpy import array, dot, vstack, hstack, asmatrix, identity - -from scipy.optimize import linprog - - -#min (1/2)x' P x + q' x -#subject to G x <= h -#subject to C x = d -def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose = False): - #~ qp_G = .5 * (P + P.T) # make sure P is symmetric - qp_G = .5 * (P + P.T) # make sure P is symmetric - qp_a = -q - if C is not None: - if G is not None: - qp_C = -vstack([C, G]).T - qp_b = -hstack([d, h]) - else: - qp_C = -C.transpose() - qp_b = -d - meq = C.shape[0] - else: # no equality constraint - qp_C = -G.T - qp_b = -h - meq = 0 - res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) - if verbose: - return res - #~ print 'qp status ', res - return res[0] - - -#min ||Ax-b||**2 -#subject to G x <= h -#subject to C x = d -def solve_least_square(A,b,G=None, h=None, C=None, d=None): - P = dot(A.T, A) - #~ q = 2*dot(b, A).reshape(b.shape[0]) - q = -dot(A.T, b).reshape(b.shape[0]) - #~ q = 2*dot(b, A) - return quadprog_solve_qp(P, q, G, h, C, d) - - -#min q' x -#subject to G x <= h -#subject to C x = d -def solve_lp(q, G=None, h=None, C=None, d=None): - res = linprog(q, A_ub=G, b_ub=h, A_eq=C, b_eq=d, bounds=[(-100000.,10000.) for _ in range(q.shape[0])], method='interior-point', callback=None, options={'presolve': True}) - return res - - - -if __name__ == '__main__': - - from numpy.linalg import norm - - A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]]) - b = array([3., 2., 3.]) - P = dot(A.T, A) - q = 2*dot(b, A).reshape((3,)) - G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]]) - h = array([3., 2., -2.]).reshape((3,)) - - res2 = solve_least_square(A, b, G, h) - res1 = quadprog_solve_qp(P, q, G, h) - print(res1) - print(res2) diff --git a/sl1m/rbprm/constants_and_tools.py b/sl1m/rbprm/constants_and_tools.py index 016cf30..485de72 100644 --- a/sl1m/rbprm/constants_and_tools.py +++ b/sl1m/rbprm/constants_and_tools.py @@ -1,102 +1,129 @@ -from sl1m.constants_and_tools import * +from sl1m.constants_and_tools import * import os + if "INSTALL_HPP_DIR" in os.environ: - insdir = os.environ['INSTALL_HPP_DIR'] + '/share/' + insdir = os.environ["INSTALL_HPP_DIR"] + "/share/" elif "DEVEL_HPP_DIR" in os.environ: - insdir = os.environ['DEVEL_HPP_DIR'] + '/install/share/' + insdir = os.environ["DEVEL_HPP_DIR"] + "/install/share/" # load hrp2 constraints __ineq_right_foot_hrp2 = None -__ineq_left_foot_hrp2 = None +__ineq_left_foot_hrp2 = None __ineq_right_foot_hrp2_reduced = None -__ineq_left_foot_hrp2_reduced = None +__ineq_left_foot_hrp2_reduced = None # add foot offset def right_foot_hrp2_constraints(transform): global __ineq_right_foot_hrp2 if __ineq_right_foot_hrp2 is None: - filekin = insdir +"hrp2-rbprm/com_inequalities/feet_quasi_flat/hrp2_COM_constraints_in_RF_effector_frame_REDUCED.obj" + filekin = ( + insdir + + "hrp2-rbprm/com_inequalities/feet_quasi_flat/hrp2_COM_constraints_in_RF_effector_frame_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_right_foot_hrp2 = as_inequalities(obj) transform2 = transform.copy() - transform2[2,3] += 0.105 + transform2[2, 3] += 0.105 ine = rotate_inequalities(__ineq_right_foot_hrp2, transform2) return (ine.A, ine.b) - + + def left_foot_hrp2_constraints(transform): global __ineq_left_foot_hrp2 if __ineq_left_foot_hrp2 is None: - filekin = insdir +"hrp2-rbprm/com_inequalities/feet_quasi_flat/hrp2_COM_constraints_in_LF_effector_frame_REDUCED.obj" + filekin = ( + insdir + + "hrp2-rbprm/com_inequalities/feet_quasi_flat/hrp2_COM_constraints_in_LF_effector_frame_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_left_foot_hrp2 = as_inequalities(obj) transform2 = transform.copy() - transform2[2,3] += 0.105 + transform2[2, 3] += 0.105 ine = rotate_inequalities(__ineq_left_foot_hrp2, transform2) return (ine.A, ine.b) - + + def right_foot_talos_constraints(transform): global __ineq_right_foot_hrp2 if __ineq_right_foot_hrp2 is None: - filekin = insdir +"talos-rbprm/com_inequalities/feet_quasi_flat/talos_COM_constraints_in_RF_effector_frame_REDUCED.obj" + filekin = ( + insdir + + "talos-rbprm/com_inequalities/feet_quasi_flat/talos_COM_constraints_in_RF_effector_frame_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_right_foot_hrp2 = as_inequalities(obj) ine = rotate_inequalities(__ineq_right_foot_hrp2, transform.copy()) return (ine.A, ine.b) - + + def left_foot_talos_constraints(transform): global __ineq_left_foot_hrp2 if __ineq_left_foot_hrp2 is None: - filekin = insdir +"talos-rbprm/com_inequalities/feet_quasi_flat/talos_COM_constraints_in_LF_effector_frame_REDUCED.obj" + filekin = ( + insdir + + "talos-rbprm/com_inequalities/feet_quasi_flat/talos_COM_constraints_in_LF_effector_frame_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_left_foot_hrp2 = as_inequalities(obj) ine = rotate_inequalities(__ineq_left_foot_hrp2, transform.copy()) return (ine.A, ine.b) - + __ineq_rf_in_rl_hrp2 = None -__ineq_lf_in_rf_hrp2 = None - +__ineq_lf_in_rf_hrp2 = None + # add foot offset def right_foot_in_lf_frame_hrp2_constraints(transform): global __ineq_rf_in_rl_hrp2 if __ineq_rf_in_rl_hrp2 is None: - filekin = insdir +"hrp2-rbprm/relative_effector_positions/hrp2_RF_constraints_in_LF_quasi_flat_REDUCED.obj" + filekin = ( + insdir + + "hrp2-rbprm/relative_effector_positions/hrp2_RF_constraints_in_LF_quasi_flat_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_rf_in_rl_hrp2 = as_inequalities(obj) transform2 = transform.copy() ine = rotate_inequalities(__ineq_rf_in_rl_hrp2, transform2) return (ine.A, ine.b) - + + def left_foot_in_rf_frame_hrp2_constraints(transform): global __ineq_lf_in_rf_hrp2 if __ineq_lf_in_rf_hrp2 is None: - filekin = insdir +"hrp2-rbprm/relative_effector_positions/hrp2_LF_constraints_in_RF_quasi_flat_REDUCED.obj" + filekin = ( + insdir + + "hrp2-rbprm/relative_effector_positions/hrp2_LF_constraints_in_RF_quasi_flat_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_lf_in_rf_hrp2 = as_inequalities(obj) transform2 = transform.copy() ine = rotate_inequalities(__ineq_lf_in_rf_hrp2, transform2) return (ine.A, ine.b) - + # add foot offset def right_foot_in_lf_frame_talos_constraints(transform): global __ineq_rf_in_rl_hrp2 if __ineq_rf_in_rl_hrp2 is None: - filekin = insdir +"talos-rbprm/relative_effector_positions/talos_RF_constraints_in_LF_quasi_flat_REDUCED.obj" + filekin = ( + insdir + + "talos-rbprm/relative_effector_positions/talos_RF_constraints_in_LF_quasi_flat_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_rf_in_rl_hrp2 = as_inequalities(obj) ine = rotate_inequalities(__ineq_rf_in_rl_hrp2, transform.copy()) return (ine.A, ine.b) - + + def left_foot_in_rf_frame_talos_constraints(transform): global __ineq_lf_in_rf_hrp2 if __ineq_lf_in_rf_hrp2 is None: - filekin = insdir +"talos-rbprm/relative_effector_positions/talos_LF_constraints_in_RF_quasi_flat_REDUCED.obj" + filekin = ( + insdir + + "talos-rbprm/relative_effector_positions/talos_LF_constraints_in_RF_quasi_flat_REDUCED.obj" + ) obj = load_obj(filekin) __ineq_lf_in_rf_hrp2 = as_inequalities(obj) ine = rotate_inequalities(__ineq_lf_in_rf_hrp2, transform.copy()) return (ine.A, ine.b) - - - diff --git a/sl1m/rbprm/narrow_convex_hull.py b/sl1m/rbprm/narrow_convex_hull.py new file mode 100644 index 0000000..111447a --- /dev/null +++ b/sl1m/rbprm/narrow_convex_hull.py @@ -0,0 +1,110 @@ +import numpy as np +from numpy import arange, array, append, cross, dot, zeros +from numpy.linalg import norm + +from scipy.spatial import ConvexHull + + +def normal(points): + p1 = array(points[0]) + p2 = array(points[1]) + p3 = array(points[2]) + n = cross((p2 - p1), (p3 - p1)) + n /= norm(n) + if n[2] <= 0.0: + for i in range(3): + n[i] = -n[i] + return n.tolist() + + +def cutList2D(l): + return [el[:2] for el in l] + + +def roundPoints(points, precision): + return [[round(x, precision) for x in p] for p in points] + + +def removeDuplicates(points): + pList = [] + for p in points: + if p not in pList: + pList.append(p) + return pList + + +def removeNone(l): + return [i for i in l if len(i) != 0] + + +def computeAxisAngleRotation(u, c): + ux = u[0] + uy = u[1] + uz = u[2] + s = np.sqrt(1 - c * c) + return [ + [ + c + ux * ux * (1 - c), + ux * uy * (1 - c) - uz * s, + ux * uz * (1 - c) + uy * s, + ], + [ + uy * ux * (1 - c) + uz * s, + c + uy * uy * (1 - c), + uy * uz * (1 - c) - ux * s, + ], + [ + uz * ux * (1 - c) - uy * s, + uz * uy * (1 - c) + ux * s, + c + uz * uz * (1 - c), + ], + ] + + +def getSurfaceRotation(surface): + n = surface[1] + cosx = np.dot(surface[1], [0, 0, 1]) + axisx = np.cross(surface[1], [0, 0, 1]) + n_axisx = norm(axisx) + if n_axisx > 0: + axisx /= n_axisx + return computeAxisAngleRotation(axisx, cosx) + + +def getPtsRotation(points): + return getSurfaceRotation((points, normal(points))) + + +def getSurfaceTranslation(surface): + return [sum(x) / len(x) for x in zip(*surface[0])] + + +def getPtsTranslation(points): + return getSurfaceTranslation((points, normal(points))) + + +def allignSurface(surface): + R = getSurfaceRotation(surface) + t = getSurfaceTranslation(surface) + translatedPts = [(array(p) - array(t)).tolist() for p in surface[0]] + rotatedPts = [np.dot(R, p).tolist() for p in translatedPts] + return [(array(p) + array(t)).tolist() for p in rotatedPts] + + +def allignPoints(points): + return allignSurface((points, normal(points))) + + +def pointsTransform(points, R, t): + translatedPts = [(array(pt) - array(t)).tolist() for pt in points] + rotatedPts = [np.dot(R, pt).tolist() for pt in translatedPts] + return [(array(pt) + array(t)).tolist() for pt in rotatedPts] + + +def getSurfaceExtremumPoints(el): + pts = removeDuplicates( + el[0] + el[1] + ) # FIX ME: Only works in rectangular shape with triangular mesh*2 + apts = allignPoints(pts) + hull = ConvexHull(cutList2D(apts)) + return [pts[idx] for idx in hull.vertices] diff --git a/sl1m/rbprm/surfaces_from_planning.py b/sl1m/rbprm/surfaces_from_planning.py index 6a61ffb..b6310cb 100644 --- a/sl1m/rbprm/surfaces_from_planning.py +++ b/sl1m/rbprm/surfaces_from_planning.py @@ -1,22 +1,20 @@ - -from numpy import arange -from sl1m.test import contactSurfaces,getCollidingAffIndex,removeDuplicates -from pinocchio import XYZQUATToSe3 +from numpy import arange, array import numpy as np -from numpy import array -from sl1m.problem_definition import LF - -MAX_SURFACE = 0.3 # if a contact surface is greater than this value, the intersection is used instead of the whole surface - -def rotationMatrixFromConfigs(configs): - R = [] - for config in configs: - q_rot = config[3:7] - R.append(XYZQUATToSe3([0,0,0]+q_rot).rotation) - return R - -def listToArray (seqs): - nseq = []; nseqs= [] +from hpp.corbaserver.rbprm.tools.narrow_convex_hull import ( + getSurfaceExtremumPoints, + removeDuplicates, + normal, +) +from pinocchio import XYZQUATToSE3 + +MAX_SURFACE = 1.0 # if a contact surface is greater than this value, the intersection is used instead of the whole surface +LF = 0 +RF = 1 + + +def listToArray(seqs): + nseq = [] + nseqs = [] for seq in seqs: nseq = [] for surface in seq: @@ -26,173 +24,278 @@ def listToArray (seqs): def area(s): - #print "in area, s = ",s area = 0 - for i in range(1,len(s)-1): - #area += x[i]*( y[i+1] - y[i-1] ); - area += abs(s[i][0]*(s[i+1][1] - s[i-1][1])) - i = len(s) -1 - area += abs(s[i][0]*(s[0][1] - s[i-1][1])) - #print "area = ",area*0.5 + for i in range(1, len(s) - 1): + p0, p1, p2 = array(s[0]), array(s[i]), array(s[i + 1]) + area += np.linalg.norm(np.cross(p1 - p0, p2 - p0)) return area * 0.5 -def getContactsNames(rbprmBuilder,i,q): - if i % 2 == LF : # left leg moving - step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q,'talos_lleg_rom') - else : - step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig(q,'talos_rleg_rom') - return step_contacts - -def getContactsIntersections(rbprmBuilder,i,q): - if i % 2 == LF : # left leg - intersections = rbprmBuilder.getContactSurfacesAtConfig(q,'talos_lleg_rom') - else : # right leg - intersections = rbprmBuilder.getContactSurfacesAtConfig(q,'talos_rleg_rom') + +def getRotationsFromConfigs(configs): + R = [] + for config in configs: + q_rot = config[3:7] + R.append(XYZQUATToSE3([0, 0, 0] + q_rot).rotation) + return R + + +def getContactsNames(rbprmBuilder, i, q): + if i % 2 == LF: # left leg + step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig( + q, "talos_lleg_rom" + ) + elif i % 2 == RF: # right leg + step_contacts = rbprmBuilder.clientRbprm.rbprm.getCollidingObstacleAtConfig( + q, "talos_rleg_rom" + ) + return step_contacts + + +def getContactsIntersections(rbprmBuilder, i, q): + if i % 2 == LF: # left leg + intersections = rbprmBuilder.getContactSurfacesAtConfig(q, "talos_lleg_rom") + elif i % 2 == RF: # right leg + intersections = rbprmBuilder.getContactSurfacesAtConfig(q, "talos_rleg_rom") return intersections -def getSurfacesFromGuideContinuous(rbprmBuilder,ps,afftool,pId,viewer = None,step = 1.,useIntersection= False): - pathLength = ps.pathLength(pId) #length of the path - discretizationStep = 0.5 # step at which we check the colliding surfaces - #print "path length = ",pathLength - # get surface information - all_surfaces = contactSurfaces(afftool) - all_names = afftool.getAffRefObstacles("Support") # id in names and surfaces match - surfaces_dict = dict(zip(all_names, all_surfaces)) # map surface names to surface points - seqs = [] # list of list of surfaces : for each phase contain a list of surfaces. One phase is defined by moving of 'step' along the path - t = 0. - current_phase_end = step - end = False - i = 0 - while not end: # for all the path - #print "Looking for surfaces for phase "+str(len(seqs))+" for t in ["+str(t)+" ; "+str(current_phase_end)+" ] " - phase_contacts_names = [] - while t < current_phase_end: # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end - q = ps.configAtParam(pId, t) - step_contacts = getContactsNames(rbprmBuilder,i,q) - for contact_name in step_contacts : - if not contact_name in phase_contacts_names: - phase_contacts_names.append(contact_name) - t += discretizationStep - # end current phase - # get all the surfaces from the names and add it to seqs: - if useIntersection : - intersections = getContactsIntersections(rbprmBuilder,i,q) - phase_surfaces = [] - for name in phase_contacts_names: - surface = surfaces_dict[name][0] - if useIntersection and area(surface) > MAX_SURFACE : - if name in step_contacts : - intersection = intersections[step_contacts.index(name)] - phase_surfaces.append(intersection) - else : - phase_surfaces.append(surface) # [0] because the last vector contain the normal of the surface - #print "There was "+str(len(phase_contacts_names))+" surfaces in contact during this phase." - phase_surfaces = sorted(phase_surfaces) # why is this step required ? without out the lp can fail - phase_surfaces_array = [] # convert from list to array, we cannot do this before because sorted() require list - for surface in phase_surfaces: - phase_surfaces_array.append(array(surface).T) - #print "phase_surfaces_array = ",phase_surfaces_array - seqs.append(phase_surfaces_array) - - # increase values for next phase - t = current_phase_end - i += 1 - if current_phase_end == pathLength: - end = True - current_phase_end += step - if current_phase_end >= pathLength: - current_phase_end = pathLength - # end for all the guide path - - #get rotation matrix of the root at each discretization step - configs = [] - for t in arange (0, pathLength, step) : - configs.append(ps.configAtParam(pId, t)) - R = rotationMatrixFromConfigs(configs) - return R,seqs - - -def getSurfacesFromGuide(rbprmBuilder,ps,afftool,pId,viewer = None,discretisationStep = 0.6,useIntersection = False, mergeCandidates = False): - if viewer : - from tools.display_tools import displaySurfaceFromPoints - pathLength = ps.pathLength(pId) #length of the path - configs = [] - # get configuration along the path - for s in arange (0, pathLength, discretisationStep) : - configs.append(ps.configAtParam(pId, s)) - - # get surface information - surfaces = contactSurfaces(afftool) - surfaces = sorted(surfaces) #sort the planes in order - - # get surface candidate at each discretization step - # suppose that we start with the left leg - seqs = []; #indices = [] - for i, config in enumerate(configs): - seq = [] ; #index_list = [] - if i % 2 == 0 : # left leg - contacts = rbprmBuilder.getContactSurfacesAtConfig(config,'talos_lleg_rom') - else : # right leg - contacts = rbprmBuilder.getContactSurfacesAtConfig(config,'talos_rleg_rom') - for contact in contacts: - if contact != []: - if useIntersection: - seq.append(contact) # changed it to list to make it easier to remove duplicates - #seq.append(array(contact).T) - else: - index = getCollidingAffIndex(contact, surfaces) - #index_list.append(index) - #if index != -1 : seq.append(array(surfaces[index][0]).T) - if index != -1 : seq.append(surfaces[index][0]) # changed it to list to make it easier to remove duplicates - if viewer: - displaySurfaceFromPoints(viewer,contact,[0,0,1,1]) - seqs.append(seq) - #indices.append(index_list) - - # merge candidates with the previous and the next phase - if mergeCandidates: - nseqs = [] - for i, seq in enumerate(seqs): - nseq = [] - if i == 0: nseq=seqs[i]+seqs[i+1] - elif i == len(seqs)-1: nseq=seqs[i-1]+seqs[i] - else: nseq=seqs[i-1]+seqs[i]+seqs[i+1] - nseqs.append(nseq) - seqs = nseqs - - # remove duplicates - for i, seq in enumerate(seqs): seqs[i] = removeDuplicates(seq) - - seqs = listToArray(seqs) # change the format from list to array - R = rotationMatrixFromConfigs(configs) - return R,seqs - - -""" -# get surface candidate at each discretization step -# suppose that se start with the left leg -seqs = []; indices = [] -for i, config in enumerate(configs): - seq = [] ; index_list = [] - if i % 2 == 0 : # left leg - contacts = rbprmBuilder.getContactSurfacesAtConfig(config,'talos_lleg_rom') - for contact in contacts: - if contact != []: - index = getCollidingAffIndex(contact, surfaces) - index_list.append(index) - if index != -1 : seq.append(array(surfaces[index][0]).T) - if viewer: - displaySurfaceFromPoints(viewer,contact,[0,0,1,1]) - else : # right leg - contacts = rbprmBuilder.getContactSurfacesAtConfig(config,'talos_rleg_rom') - for contact in contacts: - if contact != []: - index = getCollidingAffIndex(contact, surfaces) - index_list.append(index) - if index != -1 :seq.append(array(surfaces[index][0]).T) - if viewer: - displaySurfaceFromPoints(viewer,contact,[0,0,1,1]) - seqs.append(seq) - indices.append(index_list) -""" +# get all the contact surfaces (pts and normal) + + +def getAllSurfaces(afftool): + l = afftool.getAffordancePoints("Support") + return [(getSurfaceExtremumPoints(el), normal(el[0])) for el in l] + + +def getAllSurfacesDict(afftool): + all_surfaces = getAllSurfaces(afftool) + all_names = afftool.getAffRefObstacles("Support") # id in names and surfaces match + surfaces_dict = dict( + zip(all_names, all_surfaces) + ) # map surface names to surface points + return surfaces_dict + + +def getSurfacesFromGuideContinuous( + rbprmBuilder, + ps, + afftool, + pathId, + viewer=None, + step=1.0, + useIntersection=False, +): + if viewer: + from hpp.corbaserver.rbprm.tools.display_tools import ( + displaySurfaceFromPoints, + ) + + if pathId is None: + pathId = ps.numberPaths() - 1 + pathLength = ps.pathLength(pathId) # length of the path + # get surface information + surfaces_dict = getAllSurfacesDict(afftool) # map surface names to surface points + current_phase_end = step + window_size = 0.5 # smaller step at which we check the colliding surfaces + # Get surfaces for each phase along the path + seqs = ( + [] + ) # list of list of surfaces : for each phase contain a list of surfaces. One phase is defined by moving of 'step' along the path + i = 0 + t = 0.0 + end = False + while not end: # for all the path + phase_contacts = [] + phase_contacts_names = [] + while ( + t < current_phase_end + ): # get the names of all the surfaces that the rom collide while moving from current_phase_end-step to current_phase_end + q = ps.configAtParam(pathId, t) + step_contacts = getContactsIntersections(rbprmBuilder, i, q) + step_contacts_names = getContactsNames( + rbprmBuilder, i, q + ) # (FIX ME - RPBRM) step_contacts and step_contacts_names not in the same order + # (FIX - PYTHON) Re-associate every intersections to their corresponding surfaces + step_contacts_aux = [] + step_contacts_names_aux = [] + for index_intersection, intersection in enumerate(step_contacts): + if len(intersection) > 0: # Filter empty intersections + name = getNameSurfaceIntersected( + intersection, step_contacts_names, surfaces_dict + ) + step_contacts_aux.append(intersection) + step_contacts_names_aux.append(name) + # print(" Name of surface ",step_contacts_names[index_intersection]," replaced by ",name) + # Do not consider the duplicates yet + phase_contacts += step_contacts_aux + phase_contacts_names += step_contacts_names_aux + t += window_size + assert len(phase_contacts) == len(phase_contacts_names) + # end current phase + # Add surfaces or intersection to this phase. + seq, seq_name = [], [] + if useIntersection: + for index_phase, contact in enumerate(phase_contacts): + # Add all intersections with ROMs => There can be two intersections on the same surface. + # But in Sl1m paper : All surfaces must not intersect with one another (ERROR?) + """ + if contact != [] and area(contact) > MAX_SURFACE: + seq.append(contact) # Add intersection + seq_name.append(phase_contacts_names[index_phase]) # Do something with it for solutions a,b or c + """ + # Solutions : (a) Combine duplicates in one convex surface; + # (b) Keep the intersection with the biggest area ? + # (c) keep only the last intersection with this surface ? + # Solution C implemented + if area(contact) > MAX_SURFACE: + surface_name = phase_contacts_names[index_phase] + if surface_name in seq_name: + index_surface = seq_name.index(surface_name) + seq[index_surface] = contact # Replace intersection + else: + seq.append(contact) # Add intersection + seq_name.append( + phase_contacts_names[index_phase] + ) # Do something with it for solutions a,b or c + else: + for index_phase, name_contact in enumerate(phase_contacts_names): + # Add surfaces intersecting with ROMs, only once per phase => No duplicate + if not name_contact in seq_name: + seq.append(surfaces_dict[name_contact][0]) # Add entire surface + seq_name.append(name_contact) + seq.sort() + seqs.append(seq) + # increase value for the next phase + t = current_phase_end + current_phase_end += step + i += 1 # phase number + if t == pathLength: + current_phase_end = pathLength + 0.01 + if t > pathLength: + end = True + # end of all guide path + # get rotation matrix of the root at each step + seqs = listToArray(seqs) + configs = [] + for t in arange(0, pathLength, step): + configs.append(ps.configAtParam(pathId, t)) + R = getRotationsFromConfigs(configs) + return R, seqs + + +def getSurfacesFromGuide( + rbprmBuilder, + ps, + afftool, + pathId, + viewer=None, + step=0.6, + useIntersection=False, +): + if viewer: + from hpp.corbaserver.rbprm.tools.display_tools import ( + displaySurfaceFromPoints, + ) + if pathId is None: + pathId = ps.numberPaths() - 1 + pathLength = ps.pathLength(pathId) # length of the path + configs = [] + # get configuration along the path + for s in arange(0, pathLength, step): + configs.append(ps.configAtParam(pathId, s)) + + # get surface information + surface_dict = getAllSurfacesDict(afftool) + + # get surface candidate at each discretization step + # suppose that we start with the left leg + seqs = [] + for i, q in enumerate(configs): + seq = [] + contacts = getContactsIntersections(rbprmBuilder, i, q) + contact_names = getContactsNames( + rbprmBuilder, i, q + ) # Contacts and contact_names not in the same order + # (FIX - PYTHON) Re-associate every intersections to their corresponding surfaces + contacts_aux = [] + contact_names_aux = [] + for index_intersection, intersection in enumerate(contacts): + if len(intersection) > 0: # Filter empty intersections + name = getNameSurfaceIntersected( + intersection, contact_names, surface_dict + ) + contacts_aux.append(intersection) + contact_names_aux.append(name) + # print(" Name of surface ",contact_names[index_intersection]," replaced by ",name) + contacts = contacts_aux + contact_names = contact_names_aux + # Add intersection or surface + for j, contact in enumerate(contacts): + if useIntersection: + if area(contact) > MAX_SURFACE: + seq.append(contact) + else: + seq.append(surface_dict[contact_names[j]][0]) + if viewer: + displaySurfaceFromPoints(viewer, contact, [0, 0, 1, 1]) + seq.sort() + seqs.append(seq) + + # remove duplicates + for i, seq in enumerate(seqs): + seqs[i] = removeDuplicates(seq) + + seqs = listToArray(seqs) # change the format from list to array + R = getRotationsFromConfigs(configs) + return R, seqs + + +# p : point tested +# p0, p1, p2 : points defining plane +def pointOnPlane(p, p0, p1, p2): + n = np.cross(np.array(p1) - np.array(p0), np.array(p2) - np.array(p0)) + # n = n / np.linalg.norm(n) + return np.dot(n, np.array(p) - np.array(p0)) < 1e-6 + + +def sumAngles(p, surface): + sum_angles = 0.0 + for i in range(len(surface) - 1): + v0 = np.array(surface[i]) - np.array(p) + v1 = np.array(surface[i + 1]) - np.array(p) + v0, v1 = v0 / np.linalg.norm(v0), v1 / np.linalg.norm(v1) + # print(" dot : ",np.dot(v0,v1)) + sum_angles += np.arccos(np.dot(v0, v1)) + # Last + v0 = np.array(surface[-1]) - np.array(p) + v1 = np.array(surface[0]) - np.array(p) + v0, v1 = v0 / np.linalg.norm(v0), v1 / np.linalg.norm(v1) + # print(" dot : ",np.dot(v0,v1)) + sum_angles += np.arccos(np.dot(v0, v1)) + return abs(sum_angles) + + +# Hypothesis : +# - intersection lies in on of the surface in nameSurfaces. +def getNameSurfaceIntersected(intersection, namesSurfaces, surfaces_dict): + if len(intersection) > 0: + for name in namesSurfaces: + surface = surfaces_dict[name][0] + # Check if a point of intersection is equal to one of this surface + for p_intersection in intersection: + for p_surface in surface: + if ( + np.linalg.norm(np.array(p_intersection) - np.array(p_surface)) + < 1e-6 + ): + # Equal + return name + # Check if the first point of intersection lies on plane of the surface + p = intersection[0] + if pointOnPlane(p, surface[0], surface[1], surface[2]): + # Check if point inside or outside the surface + # Sum of angles between p and all the vertex pairs. + sum_angles = sumAngles(p, surface) + if abs(sum_angles - np.pi * 2) < 1e-6: + return name + return None diff --git a/sl1m/solver.py b/sl1m/solver.py new file mode 100644 index 0000000..f1eefab --- /dev/null +++ b/sl1m/solver.py @@ -0,0 +1,661 @@ +import numpy as np +import quadprog +from enum import Enum +from scipy.optimize import linprog +try: + from time import perf_counter as clock +except: + from time import clock + +try: + import glpk +except ImportError: + print("Import error: No module GLPK") + pass + +try: + import gurobipy as grb + + grb.setParam("LogFile", "") + grb.setParam("OutputFlag", 0) +except ImportError: + print("Import error: No module Gurobipy") + pass + +try: + import cvxpy +except ImportError: + print("Import error: No module cvxpy") + pass + + +# -------------------------- RESULT DATA STRUCTURE ------------------------------------------------- + + +class ResultData: + def __init__(self, success, time, x=None): + self.success = success + self.time = time + self.x = x + + def __str__(self): + string = "ResultData: " + string += "\n Success: " + str(self.success) + string += "\n Time: " + str(self.time) + return string + + def __repr__(self): + return self.__str__() + + +# -------------------------- CONSTANTS AND HELPERS ------------------------------------------------- + +EPSILON = 0.00001 + + +class Solvers(Enum): + GUROBI = 0 + GLPK = 1 + LINPROG = 2 + QUADPROG = 3 + CVXPY = 4 + + +def call_LP_solver(q, G=None, h=None, C=None, d=None, solver=Solvers.GUROBI): + """ + Solve the LP problem with a specific solver + :param: q, G, h, C, d problem data + :param: SOLVER Solver choice + :return: None if wrong SOLVER, else ResultData + """ + if solver == Solvers.GUROBI: + result = solve_lp_gurobi(q, G, h, C, d) + elif solver == Solvers.GLPK: + result = solve_lp_glpk(q, G, h, C, d) + elif solver == Solvers.LINPROG: + result = solve_lp_linprog(q, G, h, C, d) + elif solver == Solvers.CVXPY: + result = solve_qp_lp_cvxpy(None, None, G, h, C, d) + else: + print("Unknown LP solver asked : ", solver) + return + return result + + +def call_QP_solver(P, q, G=None, h=None, C=None, d=None, solver=Solvers.GUROBI): + """ + Solve the QP problem with a specific solver + :param: P, q, G, h, C, d problem data + :param: SOLVER Solver choice + :return: None if wrong SOLVER, else ResultData + """ + if solver == Solvers.GUROBI: + result = solve_qp_gurobi(P, q, G, h, C, d) + + elif solver == Solvers.QUADPROG: + result = solve_qp_quaprog(P, q, G, h, C, d) + elif solver == Solvers.CVXPY: + result = solve_qp_lp_cvxpy(P, q, G, h, C, d) + else: + print("Unknown QP solver asked : ", solver) + return + return result + + +def call_MIP_solver( + slack_selection_vector, + P=None, + q=None, + G=None, + h=None, + C=None, + d=None, + solver=Solvers.GUROBI, +): + """ + Solve the MIP problem with a specific solver + :param: q, G, h, C, d problem data + :param: SOLVER Solver choice + :param: slack_selection_vector Slack variables selection matrice of the problem + :return: None if wrong SOLVER, else ResultData + """ + hasCost = not (P is None or q is None) + result = None + if hasCost: + if solver == Solvers.GUROBI: + result = solve_MIP_gurobi_cost(slack_selection_vector, P, q, G, h, C, d) + elif solver == Solvers.CVXPY: + result = solve_MIP_cvxpy(slack_selection_vector, G, h, C, d) + else: + print("Unknown MIP solver asked : ", solver) + return + else: + if solver == Solvers.GUROBI: + result = solve_MIP_gurobi(slack_selection_vector, G, h, C, d) + + elif solver == Solvers.CVXPY: + result = solve_MIP_cvxpy(slack_selection_vector, G, h, C, d) + else: + print("Unknown MIP solver asked : ", solver) + return + return result + + +def ms(t): + """ + Convert a time in microseconds to milliseconds + """ + return t * 1000.0 + + +def get_nonzero_rows(M): + """ + Return the Matrix M with only the rows not null + """ + M_result = {} + rows, cols = M.nonzero() + for ij in zip(rows, cols): + i, j = ij + if i not in M_result: + M_result[i] = [] + M_result[i].append(j) + return M_result + + +# -------------------------- SOLVE METHODS --------------------------------------------------------- + + +def solve_least_square(A, b, G=None, h=None, C=None, d=None, solver=Solvers.GUROBI): + """ + min ||Ax-b||**2 + subject to G x <= h + subject to C x = d + """ + P = np.dot(A.T, A) + q = -np.dot(A.T, b).reshape(A.shape[1]) + return call_QP_solver(P, q, G, h, C, d, solver) + + +# -------------------------- LINEAR PROGRAM -------------------------------------------------------- + + +def solve_lp_linprog(q, G=None, h=None, C=None, d=None): + """ + Solve linear programm using scipy.linprog + min q' x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + bounds = [(-100000.0, 10000.0) for _ in range(q.shape[0])] + res = linprog( + q, + A_ub=G, + b_ub=h, + A_eq=C, + b_eq=d, + bounds=bounds, + method="interior-point", + callback=None, + options={"presolve": True}, + ) + t_end = clock() + return ResultData(res["success"], ms(t_end - t_init), res["x"]) + + +def solve_lp_glpk(q, G=None, h=None, C=None, d=None): + """ + Solve linear programm using the simplex method with glpk + min q' x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + lp = glpk.LPX() + lp.obj.maximize = False + + n_equality_constraints = 0 + n_inequality_constraints = 0 + if C is not None: + n_equality_constraints = C.shape[0] + n_variables = C.shape[1] + if G is not None: + n_inequality_constraints = G.shape[0] + n_variables = G.shape[1] + n_constraints = n_equality_constraints + n_inequality_constraints + lp.cols.add(n_variables) + + for c in lp.cols: + c.bounds = None, None + if n_constraints > 0: + lp.rows.add(n_constraints) + i_start = 0 + mat = [] + for i in range(n_inequality_constraints): + lp.rows[i_start].bounds = None, h[i] + for j in range(n_variables): + if abs(G[i, j]) > EPSILON: + mat.append((i_start, j, G[i, j])) + i_start += 1 + for i in range(n_equality_constraints): + lp.rows[i_start].bounds = d[i] + for j in range(n_variables): + if abs(C[i, j]) > EPSILON: + mat.append((i_start, j, C[i, j])) + i_start += 1 + lp.matrix = mat + + lp.obj[:] = q.tolist() + lp.simplex() + t_end = clock() + return ResultData( + lp.status == "opt", + ms(t_end - t_init), + np.array([c.primal for c in lp.cols]), + ) + + +def solve_lp_gurobi(q, G=None, h=None, C=None, d=None): + """ + Solve linear programm using the simplex method with glpk + min q' x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + model = grb.Model("lp") + + # add continuous variables + cVars = [] + for el in q: + cVars.append( + model.addVar( + vtype=grb.GRB.CONTINUOUS, + lb=-grb.GRB.INFINITY, + ub=grb.GRB.INFINITY, + ) + ) + + # Update model to integrate new variables + model.update() + x = np.array(model.getVars(), copy=False) + + # Equality constraints + if C.shape[0] > 0: + for i in range(C.shape[0]): + idx = [j for j, el in enumerate(C[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = C[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.EQUAL, d[i]) + + # Inequality constraints + if G.shape[0] > 0: + for i in range(G.shape[0]): + idx = [j for j, el in enumerate(G[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = G[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.LESS_EQUAL, h[i]) + + slack_indices = [i for i, el in enumerate(q) if el > 0] + + # set objective + variables = [] + previousL = 0 + obj = 0 + for i, el in enumerate(slack_indices): + if i != 0 and el - previousL > 2.0: + assert len(variables) > 0 + expr = grb.LinExpr(np.ones(len(variables)), variables) + obj += expr + variables = [x[el]] + elif el != 0: + variables += [x[el]] + previousL = el + if len(variables) > 1: + expr = grb.LinExpr(np.ones(len(variables)), variables) + obj += expr + + model.setObjective(obj, grb.GRB.MINIMIZE) + model.optimize() + t_end = clock() + try: + res = [el.x for el in cVars] + return ResultData(model.Status == grb.GRB.OPTIMAL, ms(t_end - t_init), res) + except: + return ResultData(False, ms(t_end - t_init)) + + +# -------------------------- QUADRATIC PROGRAM ----------------------------------------------------- + + +def solve_qp_quaprog(P, q, G=None, h=None, C=None, d=None): + """ + Solve the QP problem using Quadprog + min (1/2)x' P x + q' x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + qp_G = 0.5 * (P + P.T) + qp_a = -q + if C is not None and d is not None: + if G is not None and h is not None: + qp_C = -np.vstack([C, G]).T + qp_b = -np.hstack([d, h]) + else: + qp_C = -C.transpose() + qp_b = -d + meq = C.shape[0] + else: + qp_C = -G.T + qp_b = -h + meq = 0 + try: + res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) + t_end = clock() + return ResultData(True, ms(t_end - t_init), res[0]) + except: + t_end = clock() + return ResultData(False, ms(t_end - t_init)) + + +def solve_qp_lp_cvxpy(P, q, G=None, h=None, C=None, d=None): + """ + Solve the Mixed-Integer problem using CVXPY + find x + min x'P'x + q' x + subject to G x <= h + subject to C x = d + """ + + t_init = clock() + if G is not None: + n_variables = G.shape[1] + elif C is not none: + n_variables = C.shape[1] + + variables = cvxpy.Variable(n_variables) + obj = 0.0 + if P is not None: + n_variables = P.shape[1] + obj = cvxpy.Minimize(cvxpy.quad_form(variables, P) + q.T * variables) + + constraints = [] + if G.shape[0] > 0: + ineq_constraints = G * variables <= h + constraints.append(ineq_constraints) + + if C.shape[0] > 0: + eq_constraints = C * variables == d + constraints.append(eq_constraints) + + prob = cvxpy.Problem(obj, constraints) + prob.solve(verbose=False) + t_end = clock() + if prob.status not in ["infeasible", "unbounded"]: + res = np.array([v.value for v in variables]) + return ResultData(True, ms(t_end - t_init), res) + else: + return ResultData(False, ms(t_end - t_init)) + + +def solve_qp_gurobi(P, q, G=None, h=None, C=None, d=None, verbose=False): + """ + Solve the QP problem using Quadprog + min (1/2)x' P x + q' x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + grb.setParam("OutputFlag", 1 if verbose else 0) + n = P.shape[1] + model = grb.Model("qp") + + cVars = [] + for i in range(n): + cVars.append( + model.addVar( + vtype=grb.GRB.CONTINUOUS, + lb=-grb.GRB.INFINITY, + ub=grb.GRB.INFINITY, + ) + ) + + # Update model to integrate new variables + model.update() + x = np.array(model.getVars(), copy=False) + obj = grb.QuadExpr() + rows, cols = P.nonzero() + for i, j in zip(rows, cols): + obj += 0.5 * x[i] * P[i, j] * x[j] + for i in range(n): + obj += q[i] * x[i] + model.setObjective(obj, grb.GRB.MINIMIZE) + + # Inquality constraints + if G is not None: + G_nonzero_rows = get_nonzero_rows(G) + for i, row in G_nonzero_rows.items(): + model.addConstr(grb.quicksum(G[i, j] * x[j] for j in row) <= h[i]) + + # Equality constraints + if C is not None: + C_nonzero_rows = get_nonzero_rows(C) + for i, row in C_nonzero_rows.items(): + model.addConstr(grb.quicksum(C[i, j] * x[j] for j in row) == d[i]) + + model.optimize() + t_end = clock() + try: + res = [el.x for el in cVars] + return ResultData(model.Status == grb.GRB.OPTIMAL, ms(t_end - t_init), res) + except: + return ResultData(False, ms(t_end - t_init)) + + +# -------------------------- MIXED_INTEGER PROGRAM ------------------------------------------------- + + +def solve_MIP_gurobi(slack_selection_vector, G=None, h=None, C=None, d=None): + """ + Solve the Mixed-Integer problem using Gurobipy + Choose one alpha per phase + subject to G x <= h + subject to C x = d + """ + t_init = clock() + model = grb.Model("mip") + + cVars = [] + for variable in slack_selection_vector: + if variable > 0: + cVars.append(model.addVar(lb=0, ub=1, vtype=grb.GRB.BINARY, name="slack")) + else: + cVars.append( + model.addVar( + lb=-grb.GRB.INFINITY, + ub=grb.GRB.INFINITY, + vtype=grb.GRB.CONTINUOUS, + name="x", + ) + ) + + # Update model to integrate new variables + model.update() + x = np.array(model.getVars(), copy=False) + + # Inequality constraints + if G.shape[0] > 0: + for i in range(G.shape[0]): + idx = [j for j, el in enumerate(G[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = G[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.LESS_EQUAL, h[i]) + model.update() + + # Equality constraints + if C.shape[0] > 0: + for i in range(C.shape[0]): + idx = [j for j, el in enumerate(C[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = C[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.EQUAL, d[i]) + model.update() + + slack_indices = [i for i, el in enumerate(slack_selection_vector) if el > 0] + + # equality slack sum + variables = [] + previousL = 0 + for i, el in enumerate(slack_indices): + if i != 0 and el - previousL > 2.0: + assert len(variables) > 0 + expr = grb.LinExpr(np.ones(len(variables)), variables) + model.addConstr(expr, grb.GRB.EQUAL, len(variables) - 1) + variables = [x[el]] + elif el != 0: + variables += [x[el]] + previousL = el + if len(variables) > 1: + expr = grb.LinExpr(np.ones(len(variables)), variables) + model.addConstr(expr, grb.GRB.EQUAL, len(variables) - 1) + model.update() + model.optimize() + t_end = clock() + try: + res = [el.x for el in cVars] + return ResultData(model.Status == grb.GRB.OPTIMAL, ms(t_end - t_init), res) + except: + return ResultData(False, ms(t_end - t_init)) + + +def solve_MIP_gurobi_cost(slack_selection_vector, P, q, G=None, h=None, C=None, d=None): + """ + Solve the Mixed-Integer problem using Gurobipy + min (1/2)x' P x + q' x + subject to G x <= h + subject to C x = d + """ + grb.setParam("LogFile", "") + grb.setParam("OutputFlag", 0) + + slack_indices = [i for i, el in enumerate(slack_selection_vector) if el > 0] + n_variables = len(slack_selection_vector) + + model = grb.Model("mip") + + # add continuous variables + cVars = [] + for variable in slack_selection_vector: + if variable > 0: + cVars.append(model.addVar(lb=0, ub=1, vtype=grb.GRB.BINARY, name="slack")) + else: + cVars.append( + model.addVar( + lb=-grb.GRB.INFINITY, + ub=grb.GRB.INFINITY, + vtype=grb.GRB.CONTINUOUS, + name="x", + ) + ) + + # Update model to integrate new variables + model.update() + x = np.array(model.getVars(), copy=False) + + # Inequality constraints + if G.shape[0] > 0: + for i in range(G.shape[0]): + idx = [j for j, el in enumerate(G[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = G[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.LESS_EQUAL, h[i]) + model.update() + + # Equality constraints + if C.shape[0] > 0: + for i in range(C.shape[0]): + idx = [j for j, el in enumerate(C[i].tolist()) if el != 0.0] + variables = x[idx] + coeff = C[i, idx] + expr = grb.LinExpr(coeff, variables) + model.addConstr(expr, grb.GRB.EQUAL, d[i]) + model.update() + + obj = grb.QuadExpr() + rows, cols = P.nonzero() + for i, j in zip(rows, cols): + obj += 0.5 * x[i] * P[i, j] * x[j] + for i in range(n_variables): + obj += q[i] * x[i] + model.setObjective(obj, grb.GRB.MINIMIZE) + + t_init = clock() + model.optimize() + t_end = clock() + try: + res = [variable.x for variable in cVars] + return ResultData(model.Status == grb.GRB.OPTIMAL, ms(t_end - t_init), res) + except: + print("Failed to solve the MIP") + return ResultData(False, ms(t_end - t_init)) + + +def solve_MIP_cvxpy(slack_selection_vector, G=None, h=None, C=None, d=None): + """ + Solve the Mixed-Integer problem using CVXPY + find x + subject to G x <= h + subject to C x = d + """ + t_init = clock() + n_variables = G.shape[1] + variables = cvxpy.Variable(n_variables) + constraints = [] + if G.shape[0] > 0: + ineq_constraints = G * variables <= h + constraints.append(ineq_constraints) + + if C.shape[0] > 0: + eq_constraints = C * variables == d + constraints.append(eq_constraints) + + slack_indices = [i for i, el in enumerate(slack_selection_vector) if el > 0] + n_slack_variables = len([el for el in slack_selection_vector if el > 0]) + obj = cvxpy.Minimize(slack_selection_vector * variables) + + if n_slack_variables > 0: + boolvars = cvxpy.Variable(n_slack_variables, boolean=True) + constraints = constraints + [ + variables[el] <= boolvars[i] for i, el in enumerate(slack_indices) + ] + + currentSum = [] + previousL = 0 + for i, el in enumerate(slack_indices): + if i != 0 and el - previousL > 2.0: + assert len(currentSum) > 0 + constraints = constraints + [sum(currentSum) == len(currentSum) - 1] + currentSum = [boolvars[i]] + elif el != 0: + currentSum = currentSum + [boolvars[i]] + previousL = el + if len(currentSum) > 1: + constraints = constraints + [sum(currentSum) == len(currentSum) - 1] + obj = cvxpy.Minimize(0.0) + prob = cvxpy.Problem(obj, constraints) + prob.solve(solver=cvxpy.CBC, verbose=False) + t_end = clock() + if prob.status not in ["infeasible", "unbounded"]: + res = np.array([v.value for v in variables]) + return ResultData(True, ms(t_end - t_init), res) + else: + return ResultData(False, ms(t_end - t_init)) + + +# -------------------------- L1-Norm minimisation -------------------------------------------------- diff --git a/sl1m/stand_alone_scenarios/anymal_inclined_surfaces.py b/sl1m/stand_alone_scenarios/anymal_inclined_surfaces.py new file mode 100644 index 0000000..6c0ccca --- /dev/null +++ b/sl1m/stand_alone_scenarios/anymal_inclined_surfaces.py @@ -0,0 +1,89 @@ +import numpy as np +import matplotlib.pyplot as plt +from time import perf_counter as clock +from pathlib import Path +import anymal_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.inclined_surfaces import ( + quadruped_surfaces_gait as surfaces,) +from sl1m.stand_alone_scenarios.surfaces.inclined_surfaces import scene + +import sl1m.tools.plot_tools as plot + +if __name__ == "__main__": + # Parameters + USE_SL1M = False # SL1M does not converge, MIP does. + USE_COM = False + GAIT = [ + np.array([1, 1, 1, 0]), + np.array([1, 1, 0, 1]), + np.array([1, 0, 1, 1]), + np.array([0, 1, 1, 1]), + ] + anymal_rbprm_path = (Path(anymal_rbprm.__file__).resolve().parent.parent.parent.parent.parent / "share" / + "anymal-rbprm") + paths = [ + str(anymal_rbprm_path / "com_inequalities" / "feet_quasi_flat" / "anymal_"), + str(anymal_rbprm_path / "relative_effector_positions" / "anymal_"), + ] + + COSTS = { + "step_size": [1.0, [0.3, 0.0]], + "final_com": [10.0, [1.5, -0.2, 0.3]], + } + + limbs = ["RHleg", "RFleg", "LHleg", "LFleg"] + others = [ + "RH_ADAPTER_TO_FOOT", + "RF_ADAPTER_TO_FOOT", + "LH_ADAPTER_TO_FOOT", + "LF_ADAPTER_TO_FOOT", + ] + suffix = "_effector_frame_quasi_static_upscale.obj" + offsets = { + "RFleg": [0.373, -0.264, -0.47], + "LFleg": [0.373, 0.264, -0.47], + "RHleg": [-0.373, -0.264, -0.47], + "LHleg": [-0.373, 0.264, -0.47], + } + + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + q_init = [0., -0.2, 0.47] + + initial_contacts = [np.array(q_init) + offsets[limb] for limb in limbs] + t_2 = clock() + + pb = Problem( + limb_names=limbs, + other_names=others, + constraint_paths=paths, + suffix_com=suffix, + ) + pb.generate_problem(R, surfaces, GAIT, initial_contacts, q_init) + t_3 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, com=USE_COM, costs=COSTS) + else: + result = solve_MIP(pb, com=USE_COM, costs=COSTS) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result(result.all_feet_pos, coms=result.coms, ax=ax, show=True) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/anymal_stairs.py b/sl1m/stand_alone_scenarios/anymal_stairs.py new file mode 100644 index 0000000..a423021 --- /dev/null +++ b/sl1m/stand_alone_scenarios/anymal_stairs.py @@ -0,0 +1,96 @@ +import numpy as np +import matplotlib.pyplot as plt +from time import perf_counter as clock +from pathlib import Path +import anymal_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import ( + quadruped_surfaces_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import scene + +import sl1m.tools.plot_tools as plot + +if __name__ == "__main__": + # Parameters + USE_SL1M = False # SL1M does not converge, MIP does. + USE_COM = True + GAIT = [ + np.array([1, 1, 1, 0]), + np.array([1, 1, 0, 1]), + np.array([1, 0, 1, 1]), + np.array([0, 1, 1, 1]), + ] + anymal_rbprm_path = ( + Path(anymal_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "anymal-rbprm" + ) + paths = [ + str(anymal_rbprm_path / "com_inequalities" / "feet_quasi_flat" / "anymal_"), + str(anymal_rbprm_path / "relative_effector_positions" / "anymal_"), + ] + + COSTS = { + "posture": [1.0], + "step_size": [1.0, [0.3, 0.0]], + "final_com": [10.0, [1.2, 0.3, 0.9]], + } + + limbs = ["RHleg", "RFleg", "LHleg", "LFleg"] + others = [ + "RH_ADAPTER_TO_FOOT", + "RF_ADAPTER_TO_FOOT", + "LH_ADAPTER_TO_FOOT", + "LF_ADAPTER_TO_FOOT", + ] + suffix = "_effector_frame_quasi_static_upscale.obj" + offsets = { + "RFleg": [0.373, -0.264, -0.47], + "LFleg": [0.373, 0.264, -0.47], + "RHleg": [-0.373, -0.264, -0.47], + "LHleg": [-0.373, 0.264, -0.47], + } + + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + q_init = [-0.5, 0.0, 0.47] + + initial_contacts = [np.array(q_init) + offsets[limb] for limb in limbs] + t_2 = clock() + + pb = Problem( + limb_names=limbs, + other_names=others, + constraint_paths=paths, + suffix_com=suffix, + ) + pb.generate_problem(R, surfaces, GAIT, initial_contacts, q_init) + t_3 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, com=USE_COM, costs=COSTS) + else: + result = solve_MIP(pb, com=USE_COM, costs=COSTS) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/anymal_trot.py b/sl1m/stand_alone_scenarios/anymal_trot.py new file mode 100644 index 0000000..9b5cd42 --- /dev/null +++ b/sl1m/stand_alone_scenarios/anymal_trot.py @@ -0,0 +1,195 @@ +import numpy as np +import matplotlib.pyplot as plt +from math import ceil +from time import perf_counter as clock +import os +import anymal_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem + +import sl1m.tools.plot_tools as plot + +USE_SL1M = False # Cf. `anymal_stairs.py` +USE_COM = True +gait = "trot" + +GAITS = {} +# Caracal limb swing order: LH, LF, RH, RF +GAITS["walk"] = [ + np.array([1, 0, 1, 1]), + np.array([0, 1, 1, 1]), + np.array([1, 1, 1, 0]), + np.array([1, 1, 0, 1]), +] + +# Trot pairs: LF-RH, RF-LH -- does not work +GAITS["trot"] = [np.array([1, 0, 0, 1]), np.array([0, 1, 1, 0])] + +# This pattern outputs something with SL1M and CoM optimisation [Pinocchio order] +GAITS["trot"] = [np.array([1, 1, 0, 0]), np.array([0, 0, 1, 1])] + +# # HH, FF: Does not work [works with SL1M order + CoM optimisation] +# GAITS["trot"] = [np.array([1, 0, 1, 0]), +# np.array([0, 1, 0, 1])] + +GAIT = GAITS[gait] + +# 0.2m step length +STEP_LENGTH = [0.2, 0.0] +COSTS = { + "step_size": [ + 1.0, + STEP_LENGTH, + ], # Maintain step size as close as possible to desired. Does not guarantee exceeding. + "posture": [0.01], # Keep relative feet positions as close as possible to initial + "final_com": [ + 10.0, + [0.0, 0.0, 0.47], + ], # Achieve desired CoM at end of plan +} + +paths = [ + os.path.join( + os.path.dirname(anymal_rbprm.__file__), + "../../../..", + "share/anymal-rbprm/com_inequalities/feet_quasi_flat/anymal_", + ), + os.path.join( + os.path.dirname(anymal_rbprm.__file__), + "../../../..", + "share/anymal-rbprm/relative_effector_positions/anymal_", + ), +] +# ## SL1M order +# limbs = ['RHleg', 'RFleg', 'LHleg', 'LFleg'] +# others = ['RH_ADAPTER_TO_FOOT', 'RF_ADAPTER_TO_FOOT', 'LH_ADAPTER_TO_FOOT', 'LF_ADAPTER_TO_FOOT'] +## Pinocchio order +limbs = ["LFleg", "LHleg", "RFleg", "RHleg"] +others = [ + "LF_ADAPTER_TO_FOOT", + "LH_ADAPTER_TO_FOOT", + "RF_ADAPTER_TO_FOOT", + "RH_ADAPTER_TO_FOOT", +] +suffix = "_effector_frame_quasi_static_upscale.obj" + +# The nominal configuration would be 0.37, 0.2 +# --> i.e. this is 6.4cm wider per leg than usual --> 12.8cm total +# offsets = {'RFleg': [0.373, -0.264, -0.47], 'LFleg': [0.373, 0.264, -0.47], +# 'RHleg': [-0.373, -0.264, -0.47], 'LHleg': [-0.373, 0.264, -0.47]} +# Usual nominal config: +offsets = { + "RFleg": [0.373, -0.2, -0.47], + "LFleg": [0.373, 0.2, -0.47], + "RHleg": [-0.373, -0.2, -0.47], + "LHleg": [-0.373, 0.2, -0.47], +} + +if __name__ == "__main__": + # Change this to -3 to make it fail... + q_init = [-2.0, 0.0, 0.47] + + if "final_com" in COSTS: + # Figure out distance + dist = np.linalg.norm(np.asarray(COSTS["final_com"][1]) - q_init) + n_phases = 2 * ceil(dist / STEP_LENGTH[0]) + print("Distance to traverse", dist, "# of phases", n_phases) + else: + n_phases = 20 # dummy value + + surf_size = 4 + surface = np.array( + [ + [-surf_size, -surf_size, surf_size, surf_size], + [surf_size, -surf_size, -surf_size, surf_size], + [0.0, 0.0, 0.0, 0.0], + ] + ) + scene = [[surface], [surface]] + surfaces = [scene] * n_phases + + t_init = clock() + R = [np.identity(3)] * n_phases + + initial_contacts = [np.array(q_init) + offsets[limb] for limb in limbs] + t_1 = clock() + + pb = Problem( + limb_names=limbs, + other_names=others, + constraint_paths=paths, + suffix_com=suffix, + ) + pb.generate_problem(R, surfaces, GAIT, initial_contacts, q_init[:3]) + t_2 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, costs=COSTS, com=USE_COM) + else: + result = solve_MIP(pb, costs=COSTS, com=USE_COM) + t_end = clock() + + print(result) + print("Optimized number of steps: {0:>8}".format(pb.n_phases)) + print( + "Total time is: {0:>8.3f}ms".format( + 1000.0 * (t_end - t_init) + ) + ) + print( + "Initializing the problem takes {0:>8.3f}ms".format( + 1000.0 * (t_1 - t_init) + ) + ) + print( + "Generating the problem dictionary takes {0:>8.3f}ms".format( + 1000.0 * (t_2 - t_1) + ) + ) + print( + "Solving the problem takes {0:>8.3f}ms".format( + 1000.0 * (t_end - t_2) + ) + ) + print("The LP and QP optimizations take {0:>8.3f}ms".format(result.time)) + + # Analyse result: + if result.success: + print("\n\n\n") + # Check final-CoM + if "final_com" in COSTS: + print( + "Desired Final-CoM = ({0:.2f}, {1:.2f}, {2:.2f})" + " vs Actual Final-CoM = ({3:.2f}, {4:.2f}, {5:.2f})".format( + *COSTS["final_com"][1], *result.coms[-1] + ) + ) + + # Check footstep length + for i in range(4): + for j in range(1, len(result.all_feet_pos)): + if result.all_feet_pos[i][j] is None: + continue + if result.all_feet_pos[i][j - 1] is None: + continue + print( + "Foot #{0}, Step #{1} Dist {2:.2f}m".format( + i, + j, + np.linalg.norm( + result.all_feet_pos[i][j] - result.all_feet_pos[i][j - 1] + ), + ) + ) + + if result.success: + ax = plot.draw_scene(scene) + plot.plot_planner_result( + result.all_feet_pos, + coms=result.coms, + step_size=STEP_LENGTH, + ax=ax, + show=True, + ) + plt.show() diff --git a/sl1m/stand_alone_scenarios/complex.py b/sl1m/stand_alone_scenarios/complex.py deleted file mode 100644 index 0c8f5a6..0000000 --- a/sl1m/stand_alone_scenarios/complex.py +++ /dev/null @@ -1,121 +0,0 @@ -import numpy as np - - -from sl1m.constants_and_tools import * - -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate -from numpy.linalg import norm - -from sl1m.planner import * - -from constraints import * - - -start = [[-3., 0.4 , 0. ], [-2.7 , 0.4, 0. ], [-2.7 , 0.1, 0. ], [-03., 0.1, 0. ], ] -floor = [[-0.30, 0.54 , 0. ], [0.01 , 0.54, 0. ], [0.01 , -0.46, 0. ], [-0.30, -0.46, 0. ], ] -step1 = [[ 0.01, 0.54 , 0.1 ], [0.31 , 0.54, 0.1], [0.31 , -0.46, 0.1 ], [ 0.01, -0.46, 0.1 ], ] -step2 = [[ 0.31, 0.54 , 0.2 ], [0.61 , 0.54, 0.2], [0.61 , -0.46, 0.2 ], [ 0.31, -0.46, 0.2 ], ] -step3 = [[ 0.61, 0.54 , 0.3 ], [0.91 , 0.54, 0.3], [0.91 , -0.46, 0.3 ], [ 0.61, -0.46, 0.3 ], ] -step4 = [[ 0.91, 0.54 , 0.4 ], [1.21 , 0.54, 0.4], [1.21 , -0.46, 0.4 ], [ 0.91, -0.46, 0.4 ], ] -step5 = [[ 1.21, 0.54 , 0.5 ], [1.51 , 0.54, 0.5], [1.51 , -0.46, 0.5 ], [ 1.21, -0.46, 0.5 ], ] -step6 = [[ 1.51, 0.54 , 0.6 ], [1.81 , 0.54, 0.6], [1.81 , -0.46, 0.6 ], [ 1.51, -0.46, 0.6 ], ] -step7 = [[ 1.51,-0.46 , 0.6 ], [1.81 , -0.46, 0.6], [1.81 , -0.76, 0.6 ], [ 1.51, -0.76, 0.6 ], ] -bridge = [[ 1.51, -0.46 , 0.6 ], [1.51 , -0.76, 0.6], [-1.49, -0.76, 0.6 ], [-1.49, -0.46, 0.6 ], ] -platfo = [[-1.49, -0.35, 0.6 ], [-1.49, -1.06, 0.6], [-2.49, -1.06, 0.6 ], [-2.49, -0.35, 0.6 ], ] -slope = [[-1.49, -0.06 , 0.6 ], [-1.49, 1.5, 0.], [-2.49, 1.5, 0. ], [-2.49, -0.06, 0.6 ], ] -rub2 = [[ -2.11, 0.30 , 0.05 ], [-2.45 , 0.30, 0.05 ], [ -2.45, 0.53, 0.05 ], [-2.11, 0.53, 0.05 ], ] -rub3 = [[ -1.91, -0.15 , 0.1 ], [-2.25 , -0.15, 0.1 ], [ -2.25, 0.18, 0.1 ], [-1.91, 0.18, 0.1 ], ] -rub4 = [[ -1.69, 0.19 , 0.15 ], [-2.03 , 0.19, 0.15 ], [ -2.03, 0.53, 0.15 ], [-1.69, 0.53, 0.15 ], ] -rub5 = [[ -1.49, -0.15 , 0.2 ], [-1.83 , -0.15, 0.2 ], [ -1.83, 0.18, 0.2 ], [-1.49, 0.18, 0.2 ], ] -rub6 = [[ -1.29, 0.19 , 0.2 ], [-1.63 , 0.19, 0.2 ], [ -1.63, 0.53, 0.2 ], [-1.29, 0.53, 0.2 ], ] -rub7 = [[ -1.09, -0.15 , 0.15 ], [-1.43 , -0.15, 0.15], [ -1.43, 0.18, 0.15], [-1.09, 0.18, 0.15 ], ] -rub75 = [[ -0.89, 0.19 , 0.1 ], [-1.23 , 0.19, 0.1], [ -1.23, 0.53, 0.1], [-0.89, 0.53, 0.1 ], ] -rub8 = [[ -0.89, -0.15 , 0.025 ], [-1.02 , -0.15, 0.025], [ -1.02, 0.18, 0.025], [-0.89, 0.18, 0.025 ], ] -rub9 = [[ -0.35, -0.15 , 0.025 ], [-0.86 , -0.15, 0.025], [-0.86, 0.52, 0.025 ], [ -0.35, 0.52, 0.025], ] -rub8 = [[ -0.89, -0.15 , 0.05 ], [-1.02 , -0.15, 0.05], [ -1.02, 0.18, 0.05], [-0.89, 0.18, 0.05 ], ] -rub9 = [[ -0.35, -0.15 , 0.05 ], [-0.86 , -0.15, 0.05], [-0.86, 0.52, 0.05 ], [ -0.35, 0.52, 0.05], ] - -all_surfaces = [start, floor, step1, step2, step3, step4,step5,step6, step7, bridge, platfo, rub8, rub9,rub7, rub75, rub6, rub5, rub4, rub3, rub2] - -arub9 = array(rub9).T -arub8 = array(rub8).T -arub75 = array(rub75).T -arub7 = array(rub7).T -arub6 = array(rub6).T -arub5 = array(rub5).T -arub4 = array(rub4).T -arub3 = array(rub3).T -arub2 = array(rub2).T -astart = array(start).T -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T -astep5 = array(step5).T -astep6 = array(step6).T -astep7 = array(step7).T -abridge = array(bridge).T -aplatfo = array(platfo).T -aslope = array(slope).T - - -allrub = [arub2,arub3,arub5,arub4,arub6,arub7,arub75,arub9] -allsteps = [astep2,astep1,astep3,astep4,astep5,astep6,astep7] -allstepsandfloor = allsteps + [arub9,afloor] -allrubfloorsteps = allrub + allsteps + [afloor] -end = [astep6, astep7, abridge, aplatfo ] - - - - -#~ surfaces = [[arub2],[arub3,arub2],[arub4,arub5],[arub5],[arub6,arub7],[arub6,arub7],[arub75,arub9],[arub9],[arub9,afloor],[afloor,astep1],[astep1,astep2],[astep2,astep3],[astep3,astep4],[astep4,astep5],[astep5,astep6],[astep6,astep7],[astep6,astep7],[astep7,abridge],[astep7,abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge,aplatfo],[abridge,aplatfo], [aplatfo] ] - -surfaces = [[arub2],[arub3,arub2],[arub4],[arub5],[arub6],[arub7],[arub75],allrub,[afloor],[afloor, astep1],[afloor, astep1],[astep1,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep3],[astep4,astep2,astep5],[astep6,astep2,astep5],[astep6],[astep7],end,end,[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[abridge],[aplatfo] ] - -def gen_stair_pb(): - kinematicConstraints = genKinematicConstraints(left_foot_constraints, right_foot_constraints) - relativeConstraints = genFootRelativeConstraints(right_foot_in_lf_frame_constraints, left_foot_in_rf_frame_constraints) - - nphases = len(surfaces) - - p0 = [array([-2.7805096486250154, 0.33499999999999996, 0.]), array([-2.7805096486250154, 0.145,0.])]; - res = { "p0" : None, "c0" : None, "nphases": nphases} - - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i)%2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - -import mpl_toolkits.mplot3d as a3 -import matplotlib.colors as colors -import scipy as sp - -def draw_rectangle(l, ax): - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(surfaces, ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -if __name__ == '__main__': - from sl1m.fix_sparsity import solveL1, solveMIP - - pb = gen_stair_pb() - solveMIP(pb, surfaces, MIP = True, draw_scene = draw_scene, plot = True) - pb = gen_stair_pb() - solveL1(pb, surfaces, draw_scene, plot = True) - - diff --git a/sl1m/stand_alone_scenarios/constraints.py b/sl1m/stand_alone_scenarios/constraints.py deleted file mode 100644 index 0bc2e39..0000000 --- a/sl1m/stand_alone_scenarios/constraints.py +++ /dev/null @@ -1,61 +0,0 @@ -from sl1m.constants_and_tools import * -import sl1m.stand_alone_scenarios - -import os - -insdir = os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/" - -# load constraints -__ineq_right_foot = None -__ineq_left_foot = None -__ineq_right_foot_reduced = None -__ineq_left_foot_reduced = None - -# add foot offset -def right_foot_constraints(transform): - global __ineq_right_foot - if __ineq_right_foot is None: - filekin = insdir +"COM_constraints_in_RF_effector_frame.obj" - obj = load_obj(filekin) - __ineq_right_foot = as_inequalities(obj) - transform2 = transform.copy() - transform2[2,3] += 0.105 - ine = rotate_inequalities(__ineq_right_foot, transform2) - return (ine.A, ine.b) - -def left_foot_constraints(transform): - global __ineq_left_foot - if __ineq_left_foot is None: - filekin = insdir +"COM_constraints_in_LF_effector_frame.obj" - obj = load_obj(filekin) - __ineq_left_foot = as_inequalities(obj) - transform2 = transform.copy() - transform2[2,3] += 0.105 - ine = rotate_inequalities(__ineq_left_foot, transform2) - return (ine.A, ine.b) - - -__ineq_rf_in_rl = None -__ineq_lf_in_rf = None - -# add foot offset -def right_foot_in_lf_frame_constraints(transform): - global __ineq_rf_in_rl - if __ineq_rf_in_rl is None: - filekin = insdir +"RF_constraints_in_LF.obj" - obj = load_obj(filekin) - __ineq_rf_in_rl = as_inequalities(obj) - transform2 = transform.copy() - ine = rotate_inequalities(__ineq_rf_in_rl, transform2) - return (ine.A, ine.b) - -def left_foot_in_rf_frame_constraints(transform): - global __ineq_lf_in_rf - if __ineq_lf_in_rf is None: - filekin = insdir +"LF_constraints_in_RF.obj" - obj = load_obj(filekin) - __ineq_lf_in_rf = as_inequalities(obj) - transform2 = transform.copy() - ine = rotate_inequalities(__ineq_lf_in_rf, transform2) - return (ine.A, ine.b) - diff --git a/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_LF_effector_frame_quasi_static_reduced.obj b/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_LF_effector_frame_quasi_static_reduced.obj new file mode 100644 index 0000000..4ef422b --- /dev/null +++ b/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_LF_effector_frame_quasi_static_reduced.obj @@ -0,0 +1,74 @@ +# Blender v2.79 (sub 0) OBJ File: '' +# www.blender.org +mtllib hrp2_COM_constraints_in_LF_effector_frame_REDUCED.mtl +o foo.027 +v 0.038395 -0.290074 0.551716 +v -0.149505 -0.203483 0.568595 +v -0.044942 0.003901 0.606689 +v -0.215187 -0.613909 0.300059 +v 0.079828 -0.163866 0.587590 +v 0.341034 -0.081221 0.520543 +v 0.257744 -0.284187 0.499712 +v -0.319260 -0.224750 0.503677 +v 0.064146 -0.377686 0.504203 +v 0.516027 0.006606 0.300065 +v 0.515678 -0.328989 0.299747 +v 0.274168 -0.551651 0.300000 +v -0.645475 0.009988 0.299978 +v 0.241920 0.007852 0.550602 +v 0.457366 0.003112 0.396014 +v -0.161147 -0.300031 0.529137 +v -0.570543 -0.300646 0.300039 +v -0.254403 0.005235 0.562349 +vn -0.3010 -0.3295 0.8949 +vn 0.8532 -0.0014 0.5216 +vn -0.0802 -0.4307 0.8989 +vn -0.0472 -0.1965 0.9794 +vn -0.1080 -0.5739 0.8118 +vn 0.1914 0.0307 0.9810 +vn -0.4458 -0.5057 0.7386 +vn 0.0904 -0.7103 0.6981 +vn 0.0029 1.0000 -0.0018 +vn -0.3668 -0.5064 0.7804 +vn 0.0000 -0.0012 -1.0000 +vn -0.5772 -0.0986 0.8106 +vn 0.2205 -0.4141 0.8831 +vn -0.0003 -0.0004 -1.0000 +vn 0.2259 -0.3343 0.9150 +vn -0.2701 -0.1071 0.9569 +vn 0.5498 0.3569 0.7553 +vn -0.0047 0.9997 0.0251 +vn 0.6545 -0.1907 0.7316 +vn 0.0851 0.9925 0.0882 +vn -0.0041 0.9988 0.0494 +vn 0.4703 -0.5093 0.7207 +vn 0.2891 -0.5613 0.7755 +vn 0.2898 -0.1490 0.9454 +vn -0.0001 0.0004 -1.0000 +usemtl None.030 +s off +f 16//1 2//1 8//1 +f 15//2 11//2 10//2 +f 9//3 1//3 2//3 16//3 +f 5//4 3//4 2//4 1//4 +f 9//5 16//5 4//5 +f 3//6 5//6 14//6 +f 4//7 8//7 17//7 +f 12//8 9//8 4//8 +f 14//9 10//9 13//9 +f 4//10 16//10 8//10 +f 12//11 4//11 11//11 +f 8//12 18//12 13//12 17//12 +f 1//13 9//13 7//13 +f 11//14 4//14 17//14 +f 1//15 7//15 5//15 +f 2//16 3//16 18//16 8//16 +f 6//17 15//17 14//17 +f 18//18 14//18 13//18 +f 6//19 7//19 11//19 15//19 +f 15//20 10//20 14//20 +f 3//21 14//21 18//21 +f 7//22 12//22 11//22 +f 9//23 12//23 7//23 +f 6//24 14//24 5//24 7//24 +f 11//25 17//25 13//25 10//25 diff --git a/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_RF_effector_frame_quasi_static_reduced.obj b/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_RF_effector_frame_quasi_static_reduced.obj new file mode 100644 index 0000000..4e0f668 --- /dev/null +++ b/sl1m/stand_alone_scenarios/constraints_files/COM_constraints_in_RF_effector_frame_quasi_static_reduced.obj @@ -0,0 +1,58 @@ +# Blender v2.79 (sub 0) OBJ File: '' +# www.blender.org +mtllib hrp2_COM_constraints_in_RF_effector_frame_REDUCED.mtl +o foo.000_foo.001 +v -0.153951 0.255618 0.553467 +v -0.044942 -0.003901 0.606689 +v -0.215187 0.613909 0.300059 +v 0.079829 0.163865 0.587590 +v 0.341034 0.081221 0.520543 +v 0.257743 0.284187 0.499712 +v -0.319260 0.224750 0.503677 +v 0.056900 0.335274 0.532251 +v 0.475328 0.443365 0.302048 +v -0.645475 -0.009988 0.299978 +v 0.532470 -0.003379 0.302202 +v 0.241920 -0.007852 0.550602 +v -0.570543 0.300646 0.300039 +v -0.254403 -0.005235 0.562349 +vn 0.2720 0.5808 0.7672 +vn -0.0002 0.0002 -1.0000 +vn 0.6870 0.1713 0.7062 +vn -0.5772 0.0986 0.8106 +vn -0.0770 0.1692 0.9826 +vn -0.0193 0.3048 0.9522 +vn 0.5913 -0.4272 0.6840 +vn 0.1914 -0.0307 0.9810 +vn 0.1775 0.7265 0.6638 +vn -0.1283 0.5580 0.8199 +vn 0.0056 -0.9999 -0.0114 +vn -0.4458 0.5057 0.7386 +vn -0.2520 0.1297 0.9590 +vn -0.0041 -0.9988 0.0494 +vn 0.0022 -0.0008 -1.0000 +vn -0.0047 -0.9997 0.0251 +vn 0.2316 0.3268 0.9163 +vn -0.3340 0.5056 0.7955 +vn 0.2898 0.1490 0.9454 +usemtl None.001 +s off +f 6//1 9//1 8//1 +f 10//2 13//2 3//2 +f 6//3 5//3 11//3 9//3 +f 7//4 13//4 10//4 14//4 +f 2//5 4//5 1//5 +f 4//6 8//6 1//6 +f 5//7 12//7 11//7 +f 4//8 2//8 12//8 +f 8//9 9//9 3//9 +f 1//10 8//10 3//10 +f 11//11 12//11 10//11 +f 13//12 7//12 3//12 +f 1//13 7//13 14//13 2//13 +f 2//14 14//14 12//14 +f 11//15 10//15 3//15 9//15 +f 12//16 14//16 10//16 +f 4//17 6//17 8//17 +f 7//18 1//18 3//18 +f 5//19 6//19 4//19 12//19 diff --git a/sl1m/stand_alone_scenarios/constraints_files/LF_constraints_in_RF.obj b/sl1m/stand_alone_scenarios/constraints_files/LF_constraints_in_RF.obj deleted file mode 100644 index 853f0d7..0000000 --- a/sl1m/stand_alone_scenarios/constraints_files/LF_constraints_in_RF.obj +++ /dev/null @@ -1,46 +0,0 @@ -# Blender v2.79 (sub 0) OBJ File: '' -# www.blender.org -mtllib hrp2_LF_constraints_in_RF_quasi_flat_REDUCED.mtl -o foo -v -0.296233 0.274978 0.141514 -v -0.287472 0.142944 0.137646 -v 0.293593 0.260494 0.157628 -v -0.281891 0.456295 0.155254 -v -0.292398 0.499051 -0.153242 -v -0.285717 0.143971 -0.132349 -v 0.266554 0.451861 -0.155800 -v 0.295180 0.231727 -0.143239 -v 0.284648 0.133310 0.041392 -v 0.282648 0.453987 0.147154 -v 0.293738 0.503263 -0.050931 -v -0.185346 0.489137 0.146593 -vn -0.9978 -0.0660 -0.0067 -vn 0.0149 0.2138 0.9768 -vn -0.3041 -0.0480 0.9514 -vn 0.9486 0.1064 -0.2980 -vn -0.3075 0.9411 0.1409 -vn -0.0089 -0.0142 0.9999 -vn -0.9999 0.0143 -0.0022 -vn 0.0729 0.8879 -0.4541 -vn 0.0180 0.9932 0.1146 -vn 0.9994 0.0220 0.0263 -vn 0.0464 -0.9761 -0.2123 -vn 0.1114 -0.6747 0.7297 -vn 0.9965 -0.0821 0.0131 -vn -0.0097 -0.0587 -0.9982 -usemtl None -s off -f 1//1 6//1 2//1 -f 4//2 10//2 12//2 -f 1//3 2//3 4//3 -f 8//4 7//4 11//4 -f 4//5 12//5 5//5 -f 4//6 2//6 3//6 10//6 -f 6//7 1//7 4//7 5//7 -f 7//8 5//8 11//8 -f 12//9 10//9 11//9 5//9 -f 10//10 3//10 8//10 11//10 -f 6//11 8//11 9//11 2//11 -f 2//12 9//12 3//12 -f 9//13 8//13 3//13 -f 6//14 5//14 7//14 8//14 diff --git a/sl1m/stand_alone_scenarios/constraints_files/RF_constraints_in_LF.obj b/sl1m/stand_alone_scenarios/constraints_files/RF_constraints_in_LF.obj deleted file mode 100644 index 1b3f55d..0000000 --- a/sl1m/stand_alone_scenarios/constraints_files/RF_constraints_in_LF.obj +++ /dev/null @@ -1,41 +0,0 @@ -# Blender v2.79 (sub 0) OBJ File: '' -# www.blender.org -mtllib hrp2_RF_constraints_in_LF_quasi_flat_REDUCED.mtl -o foo.003 -v 0.289139 -0.166685 -0.150686 -v -0.030917 -0.499853 -0.160519 -v -0.299581 -0.180950 -0.158220 -v -0.299640 -0.140275 0.152504 -v 0.299235 -0.166577 0.154751 -v -0.290924 -0.491739 0.047988 -v -0.286598 -0.475018 0.155675 -v 0.294880 -0.492564 -0.134272 -v 0.274790 -0.419324 0.146844 -v 0.178476 -0.493711 0.126348 -v -0.250186 -0.465852 -0.132670 -vn 0.0371 -0.0061 -0.9993 -vn 0.0467 -0.3211 0.9459 -vn 0.9993 -0.0320 0.0210 -vn -0.1728 -0.9688 -0.1778 -vn -0.1424 -0.1129 -0.9834 -vn -0.0152 -0.9977 0.0661 -vn -0.9995 -0.0290 0.0142 -vn 0.0219 -0.9997 0.0054 -vn 0.0059 -0.0082 0.9999 -vn -0.9523 -0.1868 -0.2415 -vn 0.5591 -0.7917 0.2462 -vn 0.0107 0.9977 -0.0662 -usemtl None -s off -f 8//1 2//1 3//1 1//1 -f 10//2 9//2 7//2 -f 8//3 1//3 5//3 9//3 -f 11//4 2//4 6//4 -f 2//5 11//5 3//5 -f 10//6 7//6 6//6 2//6 -f 6//7 7//7 4//7 3//7 -f 8//8 10//8 2//8 -f 5//9 4//9 7//9 9//9 -f 11//10 6//10 3//10 -f 8//11 9//11 10//11 -f 5//12 1//12 3//12 4//12 diff --git a/sl1m/stand_alone_scenarios/data.pickle b/sl1m/stand_alone_scenarios/data.pickle new file mode 100644 index 0000000..e007dfe Binary files /dev/null and b/sl1m/stand_alone_scenarios/data.pickle differ diff --git a/sl1m/stand_alone_scenarios/escaliers.py b/sl1m/stand_alone_scenarios/escaliers.py deleted file mode 100644 index 83c97a1..0000000 --- a/sl1m/stand_alone_scenarios/escaliers.py +++ /dev/null @@ -1,80 +0,0 @@ -import matplotlib.colors as colors -import mpl_toolkits.mplot3d as a3 -import numpy as np -import scipy as sp -from numpy import array, asmatrix, concatenate, cross, dot, hstack, identity, matrix, ones, stack, vstack, zeros -from numpy.linalg import norm - -from sl1m.constants_and_tools import * -from sl1m.planner import * - -from .constraints import * - -floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ] - -step1 = [[0.3, 0.6, 0.15],[0.3, -0.16, 0.15],[0.6, -0.16, 0.15],[0.6, 0.6, 0.15]] -step2 = [[0.6, 0.6, 0.3 ],[0.6, -0.16, 0.3 ],[0.9, -0.16, 0.3 ],[0.9, 0.6, 0.3 ]] -step3 = [[0.9, 0.6, 0.45],[0.9, -0.16, 0.45],[1.2, -0.16, 0.45],[1.2, 0.6, 0.45]] -step4 = [[1.2, 0.6, 0.6 ],[1.2, -0.16, 0.6 ],[1.5, -0.16, 0.6 ],[1.5, 0.6, 0.6 ]] - - -floor = [ [0.16, 1., 0.], [-1.8, 1., 0.], [-1.8, -1., 0.], [0.16, -1., 0.] ] - -step1 = [[0.3, 0.6, 0.1],[0.3, -0.16, 0.1],[0.6, -0.16, 0.1],[0.6, 0.6, 0.1]] -step2 = [[0.6, 0.6, 0.2 ],[0.6, -0.16, 0.2],[0.9, -0.16, 0.2 ],[0.9, 0.6, 0.2 ]] -step3 = [[0.9, 0.6, 0.3],[0.9, -0.16, 0.3],[1.2, -0.16, 0.3],[1.2, 0.6, 0.3]] -step4 = [[1.2, 0.6, 0.4 ],[1.2, -0.16, 0.4 ],[1.5, -0.16, 0.4 ],[1.5, 0.6, 0.4 ]] - - -all_surfaces = [floor, step1, step2, step3, step4] - -afloor = array(floor).T -astep1 = array(step1).T -astep2 = array(step2).T -astep3 = array(step3).T -astep4 = array(step4).T - -surfaces = [[afloor], [afloor], [astep1,astep2,astep3],[astep2,astep3,astep1], [astep3,astep2,astep1,astep4], [astep3,astep4], [astep4],[astep4]] - -def gen_stair_pb(): - kinematicConstraints = genKinematicConstraints(left_foot_constraints, right_foot_constraints) - relativeConstraints = genFootRelativeConstraints(right_foot_in_lf_frame_constraints, left_foot_in_rf_frame_constraints) - nphases = len(surfaces) - p0 = None - p0 = [array([0.,0., 0.]), array([0.,0., 0.])]; - res = { "p0" : p0, "c0" : None, "nphases": nphases} - - phaseData = [ {"moving" : i%2, "fixed" : (i+1) % 2 , "K" : [copyKin(kinematicConstraints) for _ in range(len(surfaces[i]))], "relativeK" : [relativeConstraints[(i) % 2] for _ in range(len(surfaces[i]))], "S" : surfaces[i] } for i in range(nphases)] - res ["phaseData"] = phaseData - return res - - - -def draw_rectangle(l, ax): - #~ plotPoints(ax,l) - lr = l + [l[0]] - cx = [c[0] for c in lr] - cy = [c[1] for c in lr] - cz = [c[2] for c in lr] - ax.plot(cx, cy, cz) - -def draw_scene(surfaces, ax = None, color = "p"): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - [draw_rectangle(l,ax) for l in all_surfaces] - return ax - - - -############# main ################### - -if __name__ == '__main__': - - - from sl1m.fix_sparsity import solveL1, solveMIP - - pb = gen_stair_pb() - solveMIP(pb, surfaces, MIP = True, draw_scene = draw_scene, plot = True) - pb = gen_stair_pb() - solveL1(pb, surfaces, draw_scene, plot = True) diff --git a/sl1m/stand_alone_scenarios/hrp2_complex.py b/sl1m/stand_alone_scenarios/hrp2_complex.py new file mode 100644 index 0000000..b53e15f --- /dev/null +++ b/sl1m/stand_alone_scenarios/hrp2_complex.py @@ -0,0 +1,59 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from time import perf_counter as clock + +import sl1m.stand_alone_scenarios +from sl1m.generic_solver import solve_L1_combinatorial +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.complex_surfaces import ( + surfaces_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.complex_surfaces import scene + +import sl1m.tools.plot_tools as plot + +GAIT = [np.array([1, 0]), np.array([0, 1])] +USE_COM = False + +if __name__ == "__main__": + paths = [ + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", + ] + limbs = ["LF", "RF"] + + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + initial_contacts = [ + np.array([-2.7805096486250154, 0.33499999999999996, 0.0]), + np.array([-2.7805096486250154, 0.145, 0.0]), + ] + t_2 = clock() + + pb = Problem(limb_names=limbs, constraint_paths=paths) + pb.generate_problem(R, surfaces, GAIT, initial_contacts) + t_3 = clock() + + # result = solve_MIP(pb, com=USE_COM) + result = solve_L1_combinatorial(pb, com=USE_COM) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/hrp2_stairs.py b/sl1m/stand_alone_scenarios/hrp2_stairs.py new file mode 100644 index 0000000..8e6dfc2 --- /dev/null +++ b/sl1m/stand_alone_scenarios/hrp2_stairs.py @@ -0,0 +1,59 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from time import perf_counter as clock + +import sl1m.stand_alone_scenarios +from sl1m.generic_solver import solve_L1_combinatorial +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import ( + surfaces_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import scene + +import sl1m.tools.plot_tools as plot + +GAIT = [np.array([1, 0]), np.array([0, 1])] +USE_COM = True + +if __name__ == "__main__": + paths = [ + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", + os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/", + ] + limbs = ["LF", "RF"] + + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + initial_contacts = [ + np.array([-0.0, 0.33499999999999996, 0.0]), + np.array([-0.0, 0.145, 0.0]), + ] + t_2 = clock() + + pb = Problem(limb_names=limbs, constraint_paths=paths) + pb.generate_problem(R, surfaces, GAIT, initial_contacts) + t_3 = clock() + + # result = solve_MIP(pb, com=USE_COM) + result = solve_L1_combinatorial(pb, com=USE_COM) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/problem_definition_hrp2.py b/sl1m/stand_alone_scenarios/problem_definition_hrp2.py new file mode 100644 index 0000000..26ac89d --- /dev/null +++ b/sl1m/stand_alone_scenarios/problem_definition_hrp2.py @@ -0,0 +1,161 @@ +# General sl1m problem definition +# +# pb.n_effectors = number of effectors +# pb.p0 = initial feet positions +# pb.c0 = initial com positions +# pb.nphases = number of phases +# pb.phaseData[i].moving = moving effector in phase i +# pb.phaseData[i].K = Com constraints for phase i, for each limb and each surface +# pb.phaseData[i].allRelativeK = Relative constraints for phase i for each limb and each surface +# pb.phaseData[i].root_orientation = root orientation for phase i +# pb.phaseData[i].S = surfaces of phase i + +from sl1m.constants_and_tools import default_transform_from_pos_normal +from sl1m.tools.obj_to_constraints import ( + load_obj, + as_inequalities, + rotate_inequalities, +) +import numpy as np +import os +import sl1m.stand_alone_scenarios + +LIMB_NAMES = ["LF", "RF"] +Z = np.array([0.0, 0.0, 1.0]) +LF = 0 +RF = 1 +DIR = os.path.dirname(sl1m.stand_alone_scenarios.__file__) + "/constraints_files/" + + +class PhaseData: + def __init__( + self, + i, + R, + surfaces, + moving_foot, + normal, + n_effectors, + com_obj, + foot_obj, + ): + self.moving = moving_foot + self.root_orientation = R[i] + self.S = surfaces + self.n_surfaces = len(self.S) + self.generate_K(i, R, com_obj) + self.generate_relative_K(i, R, foot_obj) + + def generate_K(self, i, R, com_obj): + """ + Generate the constraints on the CoM position for all the effectors as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: com constraint objects + """ + if i == 0: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i]) + trRF = trLF.copy() + else: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i - (i % 2)]) + trRF = default_transform_from_pos_normal( + np.zeros(3), Z, R[i - ((i + 1) % 2)] + ) + + trLF[2, 3] += 0.105 + ine = rotate_inequalities(com_obj[LF], trLF) + KLF = (ine.A, ine.b) + + trRF[2, 3] += 0.105 + ine = rotate_inequalities(com_obj[RF], trRF) + KRF = (ine.A, ine.b) + + KLF = [ + np.vstack([KLF[0], -Z]), + np.concatenate([KLF[1], -np.ones(1) * 0.3]).reshape((-1,)), + ] + KRF = [ + np.vstack([KRF[0], -Z]), + np.concatenate([KRF[1], -np.ones(1) * 0.3]).reshape((-1,)), + ] + + self.K = [KLF, KRF] + + def generate_relative_K(self, i, R, foot_obj): + """ + Generate all the relative position constraints for all limbs as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: foot relative constraints + """ + if i == 0: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i]) + trRF = trLF.copy() + else: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i - (i % 2)]) + trRF = default_transform_from_pos_normal( + np.zeros(3), Z, R[i - ((i + 1) % 2)] + ) + + ineRF = rotate_inequalities(foot_obj[LF], trLF) + ineLF = rotate_inequalities(foot_obj[RF], trRF) + + self.allRelativeK = [None, None] + self.allRelativeK[LF] = [(RF, (ineLF.A, ineLF.b))] + self.allRelativeK[RF] = [(LF, (ineRF.A, ineRF.b))] + + +class Problem: + def __init__(self): + self.n_effectors = 2 + + self.com_objects = [] + self.foot_objects = [] + for foot in range(self.n_effectors): + f = ( + DIR + + "COM_constraints_in_" + + LIMB_NAMES[foot] + + "_effector_frame_quasi_static_reduced.obj" + ) + self.com_objects.append(as_inequalities(load_obj(f))) + + f = ( + DIR + + LIMB_NAMES[(foot + 1) % 2] + + "_constraints_in_" + + LIMB_NAMES[foot] + + "_reduced.obj" + ) + self.foot_objects.append(as_inequalities(load_obj(f))) + + def generate_problem(self, R, surfaces, gait, p0, c0=None): + """ + Build a SL1M problem for the Mixed Integer formulation, + with all the kinematics and foot relative position constraints required + :param Robot: an rbprm robot + :param R: a list of rotation matrix for the base of the robot (must be the same size as surfaces) + :param surfaces: A list of surfaces candidates, with one set of surface candidates for each phase + :param gait: The gait of the robot (list of id of the moving foot) + :param p0: The initial positions of the limbs + :param c0: The initial position of the com + :return: a "res" dictionary with the format required by SL1M + """ + normal = np.array([0, 0, 1]) + self.p0 = p0 + self.c0 = c0 + self.n_phases = len(surfaces) + self.phaseData = [] + for i in range(self.n_phases): + self.phaseData.append( + PhaseData( + i, + R, + surfaces[i], + gait[i % self.n_effectors], + normal, + self.n_effectors, + self.com_objects, + self.foot_objects, + ) + ) diff --git a/sl1m/stand_alone_scenarios/problem_definition_talos.py b/sl1m/stand_alone_scenarios/problem_definition_talos.py new file mode 100644 index 0000000..2503ccc --- /dev/null +++ b/sl1m/stand_alone_scenarios/problem_definition_talos.py @@ -0,0 +1,167 @@ +from sl1m.constants_and_tools import default_transform_from_pos_normal +from sl1m.tools.obj_to_constraints import ( + load_obj, + as_inequalities, + rotate_inequalities, +) +import numpy as np + +Z = np.array([0.0, 0.0, 1.0]) +LF = 0 +RF = 1 + +# General sl1m problem definition +# +# pb.n_effectors = number of effectors +# pb.p0 = initial feet positions +# pb.c0 = initial com positions +# pb.nphases = number of phases +# pb.phaseData[i].moving = moving effector in phase i +# pb.phaseData[i].K = Com constraints for phase i, for each limb and each surface +# pb.phaseData[i].allRelativeK = Relative constraints for phase i for each limb and each surface +# pb.phaseData[i].root_orientation = root orientation for phase i +# pb.phaseData[i].S = surfaces of phase i + + +class TalosPhaseData: + def __init__( + self, + i, + R, + surfaces, + moving_foot, + normal, + n_effectors, + com_obj, + foot_obj, + ): + self.moving = moving_foot + self.root_orientation = R[i] + self.S = surfaces + self.n_surfaces = len(self.S) + self.generate_K(i, R, com_obj) + self.generate_relative_K(i, R, foot_obj) + + def generate_K(self, i, R, com_obj): + """ + Generate the constraints on the CoM position for all the effectors as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: com constraint objects + """ + if i == 0: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i]) + trRF = trLF.copy() + else: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i - (i % 2)]) + trRF = default_transform_from_pos_normal( + np.zeros(3), Z, R[i - ((i + 1) % 2)] + ) + + trLF[2, 3] += 0.105 + ine = rotate_inequalities(com_obj[LF], trLF) + KLF = (ine.A, ine.b) + + trRF[2, 3] += 0.105 + ine = rotate_inequalities(com_obj[RF], trRF) + KRF = (ine.A, ine.b) + + KLF = [ + np.vstack([KLF[0], -Z]), + np.concatenate([KLF[1], -np.ones(1) * 0.3]).reshape((-1,)), + ] + KRF = [ + np.vstack([KRF[0], -Z]), + np.concatenate([KRF[1], -np.ones(1) * 0.3]).reshape((-1,)), + ] + + self.K = [KLF, KRF] + + def generate_relative_K(self, i, R, foot_obj): + """ + Generate all the relative position constraints for all limbs as a list of [A,b] + inequalities, in the form Ax <= b + :param n_effectors: + :param obj: foot relative constraints + """ + if i == 0: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i]) + trRF = trLF.copy() + else: + trLF = default_transform_from_pos_normal(np.zeros(3), Z, R[i - (i % 2)]) + trRF = default_transform_from_pos_normal( + np.zeros(3), Z, R[i - ((i + 1) % 2)] + ) + + ineRF = rotate_inequalities(foot_obj[LF], trLF) + ineLF = rotate_inequalities(foot_obj[RF], trRF) + + self.allRelativeK = [None, None] + self.allRelativeK[LF] = [(RF, (ineLF.A, ineLF.b))] + self.allRelativeK[RF] = [(LF, (ineRF.A, ineRF.b))] + + +class Problem: + def __init__( + self, + limb_names, + constraint_paths, + suffix_com="_effector_frame_quasi_static_reduced.obj", + suffix_feet="_reduced.obj", + ): + effectors = limb_names[:] + kinematic_constraints_path = constraint_paths[0] + relative_feet_constraints_path = constraint_paths[1] + + self.n_effectors = 2 + self.com_objects = [] + self.foot_objects = [] + for foot, foot_name in enumerate(effectors): + filekin = ( + kinematic_constraints_path + + "COM_constraints_in_" + + foot_name + + suffix_com + ) + self.com_objects.append(as_inequalities(load_obj(filekin))) + + o_name = effectors[(foot + 1) % 2] + filekin = ( + relative_feet_constraints_path + + o_name + + "_constraints_in_" + + foot_name + + suffix_feet + ) + self.foot_objects.append(as_inequalities(load_obj(filekin))) + + def generate_problem(self, R, surfaces, gait, p0, c0=None): + """ + Build a SL1M problem for the Mixed Integer formulation, + with all the kinematics and foot relative position constraints required + :param Robot: an rbprm robot + :param R: a list of rotation matrix for the base of the robot (must be the same size as surfaces) + :param surfaces: A list of surfaces candidates, with one set of surface candidates for each phase + :param gait: The gait of the robot (list of id of the moving foot) + :param p0: The initial positions of the limbs + :param c0: The initial position of the com + :return: a "res" dictionary with the format required by SL1M + """ + normal = np.array([0, 0, 1]) + self.p0 = p0 + self.c0 = c0 + self.n_phases = len(surfaces) + self.phaseData = [] + for i in range(self.n_phases): + self.phaseData.append( + TalosPhaseData( + i, + R, + surfaces[i], + gait[i % self.n_effectors], + normal, + self.n_effectors, + self.com_objects, + self.foot_objects, + ) + ) diff --git a/sl1m/stand_alone_scenarios/solo_stairs.py b/sl1m/stand_alone_scenarios/solo_stairs.py new file mode 100644 index 0000000..73f045b --- /dev/null +++ b/sl1m/stand_alone_scenarios/solo_stairs.py @@ -0,0 +1,104 @@ +import numpy as np +import matplotlib.pyplot as plt +from time import perf_counter as clock +from pathlib import Path +import os +import solo_rbprm + +from sl1m.solver import Solvers +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import ( + solo_surfaces_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.stair_surfaces import ( + solo_scene as scene, +) + +import sl1m.tools.plot_tools as plot + +GAIT = [ + np.array([1, 0, 1, 1]), + np.array([1, 1, 0, 1]), + np.array([0, 1, 1, 1]), + np.array([1, 1, 1, 0]), +] +COSTS = {"posture": [1.0]} + +USE_SL1M = True +USE_COM = True +TEST_CBC = False + +solo_rbprm_path = ( + Path(solo_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "solo-rbprm" +) +paths = [ + str(solo_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(solo_rbprm_path / "relative_effector_positions") + os.sep, +] +others = ["HR_FOOT", "HL_FOOT", "FL_FOOT", "FR_FOOT"] +limbs = ["HRleg", "HLleg", "FLleg", "FRleg"] +offsets = { + "FRleg": [0.1946, -0.0875, -0.241], + "FLleg": [0.1946, 0.0875, -0.241], + "HRleg": [-0.1946, -0.0875, -0.241], + "HLleg": [-0.1946, 0.0875, -0.241], +} + +if __name__ == "__main__": + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + q_init = [-0.25, 0.1, 0.241] + initial_contacts = [np.array(q_init) + offsets[limb] for limb in limbs] + t_2 = clock() + + pb = Problem(limb_names=limbs, other_names=others, constraint_paths=paths) + pb.generate_problem(R, surfaces, GAIT, initial_contacts, q_init[:3], com=USE_COM) + t_3 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, costs=COSTS, com=USE_COM) + else: + result = solve_MIP(pb, costs=COSTS, com=USE_COM) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show(block=False) + + if TEST_CBC: + print("CBC results") + t_3 = clock() + result = solve_MIP(pb, costs=COSTS, com=USE_COM, solver=Solvers.CVXPY) + t_end = clock() + + print(result) + + print("Optimized number of steps: ", pb.n_phases) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show(block=False) diff --git a/sl1m/stand_alone_scenarios/solo_trot.py b/sl1m/stand_alone_scenarios/solo_trot.py new file mode 100644 index 0000000..175d30f --- /dev/null +++ b/sl1m/stand_alone_scenarios/solo_trot.py @@ -0,0 +1,120 @@ +import numpy as np +import matplotlib.pyplot as plt +from time import perf_counter as clock +import os +from pathlib import Path +import solo_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.flat_ground import scene + +import sl1m.tools.plot_tools as plot + +""" +Validity of settings: + +For `trot`: + +| USE_SL1M | USE_COM | Result | +| -------- | ------- | -------------------------------- | +| True | True | FAIL | +| False | False | Result returned, but implausible | +| True | False | OK | +| False | True | FAIL | +""" + +USE_SL1M = True +USE_COM = False +gait = "trot" + +GAITS = {} +GAITS["walk"] = [ + np.array([1, 0, 1, 1]), + np.array([1, 1, 0, 1]), + np.array([0, 1, 1, 1]), + np.array([1, 1, 1, 0]), +] +GAITS["trot"] = [np.array([1, 0, 1, 0]), np.array([0, 1, 0, 1])] +GAITS["jumping_trot"] = [ + np.array([1, 0, 1, 0]), + np.zeros(4), + np.array([0, 1, 0, 1]), + np.zeros(4), +] +GAIT = GAITS[gait] + +if gait == "jumping_trot": + from sl1m.stand_alone_scenarios.surfaces.flat_ground import ( + jumping_trot_surfaces as surfaces, + ) +elif gait == "trot": + from sl1m.stand_alone_scenarios.surfaces.flat_ground import ( + trot_surfaces as surfaces, + ) +else: + from sl1m.stand_alone_scenarios.surfaces.flat_ground import ( + walk_surfaces as surfaces, + ) + +STEP_LENGTH = [0.1, 0.0] +COSTS = { + "step_size": [10.0, STEP_LENGTH], + "posture": [1.0], + # "final_com": [10.0, [2.5, 0., 0.241]] +} + +solo_rbprm_path = ( + Path(solo_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "solo-rbprm" +) +paths = [ + str(solo_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(solo_rbprm_path / "relative_effector_positions") + os.sep, +] +others = ["HR_FOOT", "HL_FOOT", "FL_FOOT", "FR_FOOT"] +limbs = ["HRleg", "HLleg", "FLleg", "FRleg"] +offsets = { + "FRleg": [0.1946, -0.0875, -0.241], + "FLleg": [0.1946, 0.0875, -0.241], + "HRleg": [-0.1946, -0.0875, -0.241], + "HLleg": [-0.1946, 0.0875, -0.241], +} + +if __name__ == "__main__": + t_init = clock() + R = [np.identity(3)] * len(surfaces) + q_init = [1.0, 0.0, 0.241] + initial_contacts = [np.array(q_init) + np.array(offsets[limb]) for limb in limbs] + t_1 = clock() + + pb = Problem(limb_names=limbs, other_names=others, constraint_paths=paths) + pb.generate_problem(R, surfaces, GAIT, initial_contacts, q_init[:3]) + t_2 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, costs=COSTS, com=USE_COM) + else: + result = solve_MIP(pb, costs=COSTS, com=USE_COM) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Initializing the problem takes ", 1000.0 * (t_1 - t_init)) + print("Generating the problem dictionary takes ", 1000.0 * (t_2 - t_1)) + print("Solving the problem takes ", 1000.0 * (t_end - t_2)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, + coms=result.coms, + step_size=STEP_LENGTH, + ax=ax, + show=True, + ) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/surfaces/__init__.py b/sl1m/stand_alone_scenarios/surfaces/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/sl1m/stand_alone_scenarios/surfaces/complex_surfaces.py b/sl1m/stand_alone_scenarios/surfaces/complex_surfaces.py new file mode 100644 index 0000000..e85656b --- /dev/null +++ b/sl1m/stand_alone_scenarios/surfaces/complex_surfaces.py @@ -0,0 +1,270 @@ +import numpy as np + +start = [ + [-3.0, 0.4, 0.0], + [-2.7, 0.4, 0.0], + [-2.7, 0.1, 0.0], + [-3.0, 0.1, 0.0], +] +floor = [ + [-0.30, 0.54, 0.0], + [0.01, 0.54, 0.0], + [0.01, -0.46, 0.0], + [-0.30, -0.46, 0.0], +] +step1 = [ + [0.01, 0.54, 0.1], + [0.31, 0.54, 0.1], + [0.31, -0.46, 0.1], + [0.01, -0.46, 0.1], +] +step2 = [ + [0.31, 0.54, 0.2], + [0.61, 0.54, 0.2], + [0.61, -0.46, 0.2], + [0.31, -0.46, 0.2], +] +step3 = [ + [0.61, 0.54, 0.3], + [0.91, 0.54, 0.3], + [0.91, -0.46, 0.3], + [0.61, -0.46, 0.3], +] +step4 = [ + [0.91, 0.54, 0.4], + [1.21, 0.54, 0.4], + [1.21, -0.46, 0.4], + [0.91, -0.46, 0.4], +] +step5 = [ + [1.21, 0.54, 0.5], + [1.51, 0.54, 0.5], + [1.51, -0.46, 0.5], + [1.21, -0.46, 0.5], +] +step6 = [ + [1.51, 0.54, 0.6], + [1.81, 0.54, 0.6], + [1.81, -0.46, 0.6], + [1.51, -0.46, 0.6], +] +step7 = [ + [1.51, -0.46, 0.6], + [1.81, -0.46, 0.6], + [1.81, -0.76, 0.6], + [1.51, -0.76, 0.6], +] +bridge = [ + [1.51, -0.46, 0.6], + [1.51, -0.76, 0.6], + [-1.49, -0.76, 0.6], + [-1.49, -0.46, 0.6], +] +platfo = [ + [-1.49, -0.35, 0.6], + [-1.49, -1.06, 0.6], + [-2.49, -1.06, 0.6], + [-2.49, -0.35, 0.6], +] +slope = [ + [-1.49, -0.06, 0.6], + [-1.49, 1.5, 0.0], + [-2.49, 1.5, 0.0], + [-2.49, -0.06, 0.6], +] +rub1 = [ + [-3.0, -0.15, 0.0], + [-2.45, -0.15, 0.0], + [-2.45, 0.53, 0.0], + [-3.0, 0.53, 0.0], +] +rub2 = [ + [-2.11, 0.30, 0.05], + [-2.45, 0.30, 0.05], + [-2.45, 0.53, 0.05], + [-2.11, 0.53, 0.05], +] +rub3 = [ + [-1.91, -0.15, 0.1], + [-2.25, -0.15, 0.1], + [-2.25, 0.18, 0.1], + [-1.91, 0.18, 0.1], +] +rub4 = [ + [-1.69, 0.19, 0.15], + [-2.03, 0.19, 0.15], + [-2.03, 0.53, 0.15], + [-1.69, 0.53, 0.15], +] +rub5 = [ + [-1.49, -0.15, 0.2], + [-1.83, -0.15, 0.2], + [-1.83, 0.18, 0.2], + [-1.49, 0.18, 0.2], +] +rub6 = [ + [-1.29, 0.19, 0.2], + [-1.63, 0.19, 0.2], + [-1.63, 0.53, 0.2], + [-1.29, 0.53, 0.2], +] +rub7 = [ + [-1.09, -0.15, 0.15], + [-1.43, -0.15, 0.15], + [-1.43, 0.18, 0.15], + [-1.09, 0.18, 0.15], +] +rub75 = [ + [-0.89, 0.19, 0.1], + [-1.23, 0.19, 0.1], + [-1.23, 0.53, 0.1], + [-0.89, 0.53, 0.1], +] +rub8 = [ + [-0.89, -0.15, 0.025], + [-1.02, -0.15, 0.025], + [-1.02, 0.18, 0.025], + [-0.89, 0.18, 0.025], +] +rub9 = [ + [-0.35, -0.15, 0.025], + [-0.86, -0.15, 0.025], + [-0.86, 0.52, 0.025], + [-0.35, 0.52, 0.025], +] +rub8 = [ + [-0.89, -0.15, 0.05], + [-1.02, -0.15, 0.05], + [-1.02, 0.18, 0.05], + [-0.89, 0.18, 0.05], +] +rub9 = [ + [-0.35, -0.15, 0.05], + [-0.86, -0.15, 0.05], + [-0.86, 0.52, 0.05], + [-0.35, 0.52, 0.05], +] + +arub9 = np.array(rub9).T +arub8 = np.array(rub8).T +arub75 = np.array(rub75).T +arub7 = np.array(rub7).T +arub6 = np.array(rub6).T +arub5 = np.array(rub5).T +arub4 = np.array(rub4).T +arub3 = np.array(rub3).T +arub2 = np.array(rub2).T +arub1 = np.array(rub1).T +astart = np.array(start).T +afloor = np.array(floor).T +astep1 = np.array(step1).T +astep2 = np.array(step2).T +astep3 = np.array(step3).T +astep4 = np.array(step4).T +astep5 = np.array(step5).T +astep6 = np.array(step6).T +astep7 = np.array(step7).T +abridge = np.array(bridge).T +aplatfo = np.array(platfo).T +aslope = np.array(slope).T + +scene = [ + [astart], + [afloor], + [astep1], + [astep2], + [astep3], + [astep4], + [astep5], + [astep6], + [astep7], + [abridge], + [aplatfo], + [arub8], + [arub9], + [arub7], + [arub75], + [arub6], + [arub5], + [arub4], + [arub3], + [arub2], + [arub1], +] + +allrub = [arub2, arub3, arub5, arub4, arub6, arub7, arub75, arub9] +allsteps = [astep2, astep1, astep3, astep4, astep5, astep6, astep7] +allstepsandfloor = allsteps + [arub9, afloor] +allrubfloorsteps = allrub + allsteps + [afloor] +end = [astep6, astep7, abridge, aplatfo] + +surfaces = [ + [arub1, arub2], + [arub1, arub2], + [arub1, arub2], + [arub1, arub3, arub2], + [arub4], + [arub5], + [arub6], + [arub7], + [arub75], + allrub, + [arub7, arub8, arub9], + [arub7, arub8, arub9], + [afloor], + [afloor, astep1], + [afloor, astep1], + [astep1, astep2, astep3], + [astep4, astep2, astep3], + [astep4, astep2, astep3], + [astep4, astep2, astep5], + [astep6, astep2, astep5], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6], + [astep6, astep7], + [astep7], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [abridge], + [aplatfo], +] +surfaces_gait = [[surface] for surface in surfaces] + +rubble_stairs = [ + [arub1, arub2, arub3], + [arub1, arub2, arub3], + [arub1, arub2, arub3], + [arub3, arub2], + [arub4, arub3, arub5], + [arub5, arub4, arub3, arub6], + [arub6], + [arub7], + [arub75], + [arub9, afloor], + [arub9, afloor], + [afloor, arub9], + [astep1], + [astep2], + [astep3], + [astep4], + [astep5], + [astep6], + [astep6], +] +rubble_stairs_gait = [[surface] for surface in rubble_stairs] diff --git a/sl1m/stand_alone_scenarios/surfaces/flat_ground.py b/sl1m/stand_alone_scenarios/surfaces/flat_ground.py new file mode 100644 index 0000000..6c81634 --- /dev/null +++ b/sl1m/stand_alone_scenarios/surfaces/flat_ground.py @@ -0,0 +1,60 @@ +import numpy as np + +start = [ + [1.95, 1.0, 0.0], + [-2.5, 1.0, 0.0], + [-2.5, -1.0, 0.0], + [1.95, -1.0, 0.0], +] +end = [[2.0, 1.0, 0.0], [4.0, 1.0, 0.0], [4.0, -1.0, 0.0], [2.0, -1.0, 0.0]] + +astart = np.array(start).T +aend = np.array(end).T + +scene = [[astart], [aend]] +jumping_trot_surfaces = [ + [[astart], [astart]], + [], + [[astart, aend], [astart, aend]], + [], + [[astart, aend], [astart, aend]], + [], + [[astart, aend], [astart, aend]], + [], + [[aend], [aend]], +] +trot_surfaces = [ + [[astart], [astart]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[astart, aend], [astart, aend]], + [[aend], [aend]], +] +walk_surfaces = [ + [[astart]], + [[astart]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[astart, aend]], + [[aend]], + [[aend]], +] + + +# trot_surfaces = [[[astart, aend], [astart, aend]], [[astart, aend], [astart, aend]]] diff --git a/sl1m/stand_alone_scenarios/surfaces/inclined_surfaces.py b/sl1m/stand_alone_scenarios/surfaces/inclined_surfaces.py new file mode 100644 index 0000000..54c1971 --- /dev/null +++ b/sl1m/stand_alone_scenarios/surfaces/inclined_surfaces.py @@ -0,0 +1,34 @@ +import numpy as np + +floor = [[0.5, -0.74, 0.], [0.5, 0.74, 0.], [-0.5, 0.74, 0.], [-0.5, -0.74, 0.]] + +step1 = [[1.72, -0.48, -0.15], [1.72, 0.3, -0.15], [1.16, 0.3, -0.15], [1.16, -0.48, -0.15]] +step2 = [[0.66, 0.45, -0.126], [0.66, 0.07, -0.126], [0.98, 0.07, -0.011], [0.98, 0.45, -0.01]] +step3 = [[0.8, -0.03, -0.01], [0.58, -0.24, -0.126], [0.84, -0.51, -0.126], [1.06, -0.3, -0.01]] + +afloor = np.array(floor).T +astep1 = np.array(step1).T +astep2 = np.array(step2).T +astep3 = np.array(step3).T +scene = [[afloor], [astep1], [astep2], [astep3]] + +quadruped_surfaces = [ + [afloor], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], + [afloor, astep1, astep2, astep3], +] +quadruped_surfaces_gait = [[surface] for surface in quadruped_surfaces] \ No newline at end of file diff --git a/sl1m/stand_alone_scenarios/surfaces/ramp_surfaces.py b/sl1m/stand_alone_scenarios/surfaces/ramp_surfaces.py new file mode 100644 index 0000000..c2df40a --- /dev/null +++ b/sl1m/stand_alone_scenarios/surfaces/ramp_surfaces.py @@ -0,0 +1,24 @@ +import numpy as np + +begin = np.array([[1.75, 1.65, 1.65, 1.75], [0.3, 0.3, 0.1, 0.1], [0.6, 0.6, 0.6, 0.6]]) + +platform = np.array( + [[2.5, 1.5, 1.5, 2.5], [0.9, 0.9, -1.1, -1.1], [0.6, 0.6, 0.6, 0.6]] +) + +bridge = np.array( + [[-1.5, -1.5, 1.49, 1.49], [-0.5, -0.8, -0.8, -0.5], [0.6, 0.6, 0.6, 0.6]] +) + +end = np.array( + [[-1.5, -2.0, -2.0, -1.5], [-0.4, -0.4, -1.1, -1.1], [0.6, 0.6, 0.6, 0.6]] +) + +scene = [[begin], [platform], [bridge], [end]] + +surfaces = [] +for i in range(20): + surfaces += [[platform, bridge, end]] +surfaces += [[end], [end]] + +surfaces_gait = [[surface] for surface in surfaces] diff --git a/sl1m/stand_alone_scenarios/surfaces/stair_surfaces.py b/sl1m/stand_alone_scenarios/surfaces/stair_surfaces.py new file mode 100644 index 0000000..4ab9c7a --- /dev/null +++ b/sl1m/stand_alone_scenarios/surfaces/stair_surfaces.py @@ -0,0 +1,210 @@ +import numpy as np + +floor = [ + [0.16, 1.0, 0.0], + [-1.8, 1.0, 0.0], + [-1.8, -1.0, 0.0], + [0.16, -1.0, 0.0], +] + +step1 = [ + [0.3, 0.6, 0.1], + [0.3, -0.16, 0.1], + [0.6, -0.16, 0.1], + [0.6, 0.6, 0.1], +] +step2 = [ + [0.6, 0.6, 0.2], + [0.6, -0.16, 0.2], + [0.9, -0.16, 0.2], + [0.9, 0.6, 0.2], +] +step3 = [ + [0.9, 0.6, 0.3], + [0.9, -0.16, 0.3], + [1.2, -0.16, 0.3], + [1.2, 0.6, 0.3], +] +step4 = [ + [1.2, 0.6, 0.4], + [1.2, -0.16, 0.4], + [1.8, -0.16, 0.4], + [1.8, 0.6, 0.4], +] + +afloor = np.array(floor).T +astep1 = np.array(step1).T +astep2 = np.array(step2).T +astep3 = np.array(step3).T +astep4 = np.array(step4).T + +scene = [[afloor], [astep1], [astep2], [astep3], [astep4]] + +surfaces = [ + [afloor], + [afloor], + [astep1, astep2, astep3], + [astep2, astep3, astep1], + [astep3, astep2, astep1, astep4], + [astep3, astep4], + [astep4], + [astep4], +] +surfaces_gait = [[surface] for surface in surfaces] + +walk_surfaces = [ + [[afloor]], + [[afloor, astep1, astep2]], + [[afloor, astep1, astep2]], + [[afloor, astep1, astep2]], + [[afloor, astep1, astep2]], + [[afloor, astep1, astep2]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3]], + [[astep1, astep2, astep3, astep4]], + [[astep1, astep2, astep3, astep4]], + [[astep1, astep2, astep3, astep4]], + [[astep1, astep2, astep3, astep4]], + [[astep3, astep4]], + [[astep3, astep4]], + [[astep3, astep4]], + [[astep3, astep4]], + [[astep4]], + [[astep4]], + [[astep4]], + [[astep4]], +] + +quadruped_surfaces = [ + [afloor], + [afloor, astep1, astep2], + [afloor, astep1, astep2], + [afloor, astep1, astep2], + [afloor, astep1, astep2], + [afloor, astep1, astep2], + [astep1, astep2, astep3], + [astep1, astep2, astep3], + [astep1, astep2, astep3], + [astep1, astep2, astep3], + [astep1, astep2, astep3], + [astep1, astep2, astep3], + [astep1, astep2, astep3, astep4], + [astep1, astep2, astep3, astep4], + [astep1, astep2, astep3, astep4], + [astep1, astep2, astep3, astep4], + [astep3, astep4], + [astep3, astep4], + [astep3, astep4], + [astep3, astep4], + [astep4], + [astep4], + [astep4], + [astep4], +] +quadruped_surfaces_gait = [[surface] for surface in quadruped_surfaces] + +floor_small = [ + [0.29, 1.0, 0.0], + [-1.8, 1.0, 0.0], + [-1.8, -1.0, 0.0], + [0.29, -1.0, 0.0], +] +step1_small = [ + [0.3, 0.6, 0.05], + [0.3, -0.16, 0.05], + [0.59, -0.16, 0.05], + [0.59, 0.6, 0.05], +] +step2_small = [ + [0.6, 0.6, 0.1], + [0.6, -0.16, 0.1], + [0.89, -0.16, 0.1], + [0.89, 0.6, 0.1], +] +step3_small = [ + [0.9, 0.6, 0.15], + [0.9, -0.16, 0.15], + [1.19, -0.16, 0.15], + [1.19, 0.6, 0.15], +] +step4_small = [ + [1.2, 0.6, 0.2], + [1.2, -0.16, 0.2], + [1.8, -0.16, 0.2], + [1.8, 0.6, 0.2], +] + +afloor_small = np.array(floor_small).T +astep1_small = np.array(step1_small).T +astep2_small = np.array(step2_small).T +astep3_small = np.array(step3_small).T +astep4_small = np.array(step4_small).T + +solo_scene = [ + [afloor_small], + [astep1_small], + [astep2_small], + [astep3_small], + [astep4_small], +] +solo_surfaces = [ + [afloor_small], + [afloor_small], + [afloor_small], + [afloor_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [afloor_small, astep1_small, astep2_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep1_small, astep2_small, astep3_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep2_small, astep3_small, astep4_small], + [astep3_small, astep4_small], + [astep3_small, astep4_small], + [astep3_small, astep4_small], + [astep3_small, astep4_small], + [astep4_small], + [astep4_small], + [astep4_small], + [astep4_small], +] +solo_surfaces_gait = [[surface] for surface in solo_surfaces] diff --git a/sl1m/stand_alone_scenarios/talos_ramp.py b/sl1m/stand_alone_scenarios/talos_ramp.py new file mode 100644 index 0000000..dad85ba --- /dev/null +++ b/sl1m/stand_alone_scenarios/talos_ramp.py @@ -0,0 +1,83 @@ +import numpy as np +from pathlib import Path +import os +import matplotlib.pyplot as plt +from time import perf_counter as clock +import talos_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.ramp_surfaces import ( + surfaces_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.ramp_surfaces import scene + +import sl1m.tools.plot_tools as plot + +USE_SL1M = True +USE_COM = False + +GAIT = [np.array([1, 0]), np.array([0, 1])] + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + initial_contacts = [ + np.array([1.7, 0.285, 0.6]), + np.array([1.7, 0.115, 0.6]), + ] + t_2 = clock() + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, GAIT, initial_contacts) + t_3 = clock() + + COSTS = { + "step_size": [10.0, [0.1, 0]], + "posture": [1.0], + # "final_com": [10.0, [2.5, 0., 0.241]] + } + + if USE_SL1M: + result = solve_L1_combinatorial(pb, com=USE_COM, costs=COSTS) + else: + result = solve_MIP(pb, com=USE_COM, costs=COSTS) + t_end = clock() + + print(result) + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + plot.plot_initial_contacts(initial_contacts, ax=ax) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/stand_alone_scenarios/talos_rubble_stairs.py b/sl1m/stand_alone_scenarios/talos_rubble_stairs.py new file mode 100644 index 0000000..7549634 --- /dev/null +++ b/sl1m/stand_alone_scenarios/talos_rubble_stairs.py @@ -0,0 +1,76 @@ +import os +from pathlib import Path +import numpy as np +import matplotlib.pyplot as plt +from time import perf_counter as clock +import talos_rbprm + +from sl1m.generic_solver import solve_L1_combinatorial, solve_MIP +from sl1m.problem_definition import Problem +from sl1m.stand_alone_scenarios.surfaces.complex_surfaces import ( + rubble_stairs_gait as surfaces, +) +from sl1m.stand_alone_scenarios.surfaces.complex_surfaces import scene +import sl1m.tools.plot_tools as plot + +USE_SL1M = True +USE_COM = True +GAIT = [np.array([1, 0]), np.array([0, 1])] + +talos_rbprm_path = ( + Path(talos_rbprm.__file__).resolve().parent.parent.parent.parent.parent + / "share" + / "talos-rbprm" +) +paths = [ + str(talos_rbprm_path / "com_inequalities" / "feet_quasi_flat") + os.sep, + str(talos_rbprm_path / "relative_effector_positions") + os.sep, +] +limbs = ["LF", "RF"] +suffix_com = "_effector_frame_REDUCED.obj" +suffix_feet = "_quasi_flat_REDUCED.obj" + +if __name__ == "__main__": + t_init = clock() + R = [np.identity(3)] * len(surfaces) + t_1 = clock() + + initial_contacts = [ + np.array([-2.7805096486250154, 0.335, 0.0]), + np.array([-2.7805096486250154, 0.145, 0.0]), + ] + t_2 = clock() + + pb = Problem( + limb_names=limbs, + constraint_paths=paths, + suffix_com=suffix_com, + suffix_feet=suffix_feet, + ) + pb.generate_problem(R, surfaces, GAIT, initial_contacts) + t_3 = clock() + + if USE_SL1M: + result = solve_L1_combinatorial(pb, com=USE_COM) + else: + result = solve_MIP(pb, com=USE_COM) + t_end = clock() + + print(result) + + print("Optimized number of steps: ", pb.n_phases) + print("Total time is: ", 1000.0 * (t_end - t_init)) + print("Computing the surfaces takes ", 1000.0 * (t_1 - t_init)) + print("Computing the initial contacts takes ", 1000.0 * (t_2 - t_1)) + print("Generating the problem dictionary takes ", 1000.0 * (t_3 - t_2)) + print("Solving the problem takes ", 1000.0 * (t_end - t_3)) + print("The LP and QP optimizations take ", result.time) + + ax = plot.draw_scene(scene) + plot.plot_initial_contacts(initial_contacts, ax=ax) + if result.success: + plot.plot_planner_result( + result.all_feet_pos, coms=result.coms, ax=ax, show=True + ) + else: + plt.show() diff --git a/sl1m/tools/geom_utils.py b/sl1m/tools/geom_utils.py index 3b0e658..f144bc7 100644 --- a/sl1m/tools/geom_utils.py +++ b/sl1m/tools/geom_utils.py @@ -1,172 +1,199 @@ -#from polytope_conversion_utils import * +# from polytope_conversion_utils import * from numpy import zeros, sqrt, array, vstack from .transformations import euler_matrix import numpy as np -#from math import cos, sin, tan, atan, pi + +# from math import cos, sin, tan, atan, pi import matplotlib.pyplot as plt import cdd -from . import plot_utils as plut - -NUMBER_TYPE = 'float' # 'float' or 'fraction' - -''' Compute the projection matrix of the cross product. -''' -def crossMatrix( v ): - VP = np.array( [[ 0, -v[2], v[1] ], - [ v[2], 0, -v[0] ], - [-v[1], v[0], 0 ]] ); - return VP; - -''' Check whether v is inside a 3d cone with the specified normal direction +import sl1m.tools.plot_utils as plut + +NUMBER_TYPE = "float" # 'float' or 'fraction' + +""" Compute the projection matrix of the cross product. +""" + + +def crossMatrix(v): + VP = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) + return VP + + +""" Check whether v is inside a 3d cone with the specified normal direction and friction coefficient. -''' +""" + + def is_vector_inside_cone(v, mu, n): - P = np.eye(3) - np.outer(n, n); - return (np.linalg.norm(np.dot(P,v)) - mu*np.dot(n,v)<=0.0); + P = np.eye(3) - np.outer(n, n) + return np.linalg.norm(np.dot(P, v)) - mu * np.dot(n, v) <= 0.0 -''' Generate the 4 contact points and the associated normal directions + +""" Generate the 4 contact points and the associated normal directions for a rectangular contact. -''' +""" + + def generate_rectangle_contacts(lx, ly, pos, rpy): # contact points in local frame - p = array([[ lx, ly, 0], - [ lx, -ly, 0], - [-lx, -ly, 0], - [-lx, ly, 0]]); + p = array([[lx, ly, 0], [lx, -ly, 0], [-lx, -ly, 0], [-lx, ly, 0]]) # normal direction in local frame - n = array([0, 0, 1]); + n = array([0, 0, 1]) # compute rotation matrix - R = euler_matrix(rpy[0], rpy[1], rpy[2], 'sxyz'); - R = R[:3,:3]; + R = euler_matrix(rpy[0], rpy[1], rpy[2], "sxyz") + R = R[:3, :3] # contact points in world frame - p[0,:] = pos.reshape(3) + np.dot(R,p[0,:]); - p[1,:] = pos.reshape(3) + np.dot(R,p[1,:]); - p[2,:] = pos.reshape(3) + np.dot(R,p[2,:]); - p[3,:] = pos.reshape(3) + np.dot(R,p[3,:]); + p[0, :] = pos.reshape(3) + np.dot(R, p[0, :]) + p[1, :] = pos.reshape(3) + np.dot(R, p[1, :]) + p[2, :] = pos.reshape(3) + np.dot(R, p[2, :]) + p[3, :] = pos.reshape(3) + np.dot(R, p[3, :]) # normal directions in world frame - n = np.dot(R,n); - N = vstack([n, n, n, n]); - return (p,N); - -''' Find the intersection between two lines: + n = np.dot(R, n) + N = vstack([n, n, n, n]) + return (p, N) + + +""" Find the intersection between two lines: a1^T x + b1 = 0 a1^T x + b1 = 0 -''' +""" + + def find_intersection(a1, c1, a2, c2): - x = np.zeros(2); - den = (a1[0]*a2[1] - a2[0]*a1[1]); - if(abs(den)<1e-6): - print("ERROR: Impossible to find intersection between two lines that are parallel"); - return x; - - if(np.abs(a1[0])>np.abs(a2[0])): - x[1] = (a2[0]*c1 - a1[0]*c2)/den; - x[0] = (-c1-a1[1]*x[1])/a1[0]; + x = np.zeros(2) + den = a1[0] * a2[1] - a2[0] * a1[1] + if abs(den) < 1e-6: + print( + "ERROR: Impossible to find intersection between two lines that are parallel" + ) + return x + + if np.abs(a1[0]) > np.abs(a2[0]): + x[1] = (a2[0] * c1 - a1[0] * c2) / den + x[0] = (-c1 - a1[1] * x[1]) / a1[0] else: - x[1] = (a2[0]*c1 - a1[0]*c2)/den; - x[0] = (-c2-a2[1]*x[1])/a2[0]; - return x; - -''' Find the line passing through two points: + x[1] = (a2[0] * c1 - a1[0] * c2) / den + x[0] = (-c2 - a2[1] * x[1]) / a2[0] + return x + + +""" Find the line passing through two points: a^T x1 + b = 0 a^T x2 + b = 0 -''' +""" + + def find_line(x1, x2): - den = (x1[0]*x2[1] - x2[0]*x1[1]); - if(abs(den)<1e-4): -# print "ERROR: x1 and x2 are too close, x1=(%f,%f), x2=(%f,%f)" % (x1[0],x1[1],x2[0],x2[1]); - return (zeros(2),-1); -# a = np.array([-(x1[1] - x2[1])/den, -(x2[0] - x1[0])/den]); -# a_norm = np.linalg.norm(a); -# a /= a_norm; -# b = -1.0/a_norm; - - a = np.array([x2[1]-x1[1], x1[0]-x2[0]]); - a /= np.linalg.norm(a); - b = -a[0]*x1[0] - a[1]*x1[1]; -# print "a=(%f,%f), a2=(%f,%f), b=%f, b2=%f" % (a[0],a[1],a2[0],a2[1],b,b2); - return (a,b); - - -''' Compute the area of a 2d triangle with vertices a, b and c. -''' + den = x1[0] * x2[1] - x2[0] * x1[1] + if abs(den) < 1e-4: + # print "ERROR: x1 and x2 are too close, x1=(%f,%f), x2=(%f,%f)" % (x1[0],x1[1],x2[0],x2[1]); + return (zeros(2), -1) + # a = np.array([-(x1[1] - x2[1])/den, -(x2[0] - x1[0])/den]); + # a_norm = np.linalg.norm(a); + # a /= a_norm; + # b = -1.0/a_norm; + + a = np.array([x2[1] - x1[1], x1[0] - x2[0]]) + a /= np.linalg.norm(a) + b = -a[0] * x1[0] - a[1] * x1[1] + # print "a=(%f,%f), a2=(%f,%f), b=%f, b2=%f" % (a[0],a[1],a2[0],a2[1],b,b2); + return (a, b) + + +""" Compute the area of a 2d triangle with vertices a, b and c. +""" + + def compute_triangle_area(a, b, c): - la = np.linalg.norm(a-b); - lb = np.linalg.norm(b-c); - lc = np.linalg.norm(c-a); - s = 0.5*(la+lb+lc); - return sqrt(s*(s-la)*(s-lb)*(s-lc)); - - -''' Plot inequalities F_com*x+f_com=0 on x-y plane. -''' -def plot_inequalities(F_com, f_com, x_bounds, y_bounds, ls='--', color='k', ax=None, lw=8): - if(F_com.shape[1]!=2): - print("[ERROR in plot_inequalities] matrix does not have 2 columns"); - return; -# if(F_com.shape[0]!=len(f_com)): -# print "[ERROR in plot_inequalities] matrix and vector does not have the same number of rows"; -# return; - - if(ax==None): - f, ax = plut.create_empty_figure(); - com = np.zeros(2); # com height - com_x = np.zeros(2); - com_y = np.zeros(2); + la = np.linalg.norm(a - b) + lb = np.linalg.norm(b - c) + lc = np.linalg.norm(c - a) + s = 0.5 * (la + lb + lc) + return sqrt(s * (s - la) * (s - lb) * (s - lc)) + + +""" Plot inequalities F_com*x+f_com=0 on x-y plane. +""" + + +def plot_inequalities( + F_com, f_com, x_bounds, y_bounds, ls="--", color="k", ax=None, lw=8 +): + if F_com.shape[1] != 2: + print("[ERROR in plot_inequalities] matrix does not have 2 columns") + return + # if(F_com.shape[0]!=len(f_com)): + # print "[ERROR in plot_inequalities] matrix and vector does not have the same number of rows"; + # return; + + if ax == None: + f, ax = plut.create_empty_figure() + com = np.zeros(2) + # com height + com_x = np.zeros(2) + com_y = np.zeros(2) for i in range(F_com.shape[0]): - if(np.abs(F_com[i,1])>1e-13): - com_x[0] = x_bounds[0]; # com x coordinate - com_x[1] = x_bounds[1]; # com x coordinate - com[0] = com_x[0]; - com[1] = 0; - com_y[0] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,1]; - - com[0] = com_x[1]; - com_y[1] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,1]; - - ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw); - elif(np.abs(F_com[i,0])>1e-13): - com_y[0] = y_bounds[0]; - com_y[1] = y_bounds[1]; - com[0] = 0; - com[1] = com_y[0]; - com_x[0] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,0]; - - com[1] = com_y[1]; - com_x[1] = (-f_com[i] - np.dot(F_com[i,:],com) )/F_com[i,0]; - ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw); + if np.abs(F_com[i, 1]) > 1e-13: + com_x[0] = x_bounds[0] + # com x coordinate + com_x[1] = x_bounds[1] + # com x coordinate + com[0] = com_x[0] + com[1] = 0 + com_y[0] = (-f_com[i] - np.dot(F_com[i, :], com)) / F_com[i, 1] + + com[0] = com_x[1] + com_y[1] = (-f_com[i] - np.dot(F_com[i, :], com)) / F_com[i, 1] + + ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw) + elif np.abs(F_com[i, 0]) > 1e-13: + com_y[0] = y_bounds[0] + com_y[1] = y_bounds[1] + com[0] = 0 + com[1] = com_y[0] + com_x[0] = (-f_com[i] - np.dot(F_com[i, :], com)) / F_com[i, 0] + + com[1] = com_y[1] + com_x[1] = (-f_com[i] - np.dot(F_com[i, :], com)) / F_com[i, 0] + ax.plot(com_x, com_y, ls=ls, color=color, linewidth=lw) else: - pass; -# print "[WARNING] Could not print one inequality as all coefficients are 0: F_com[%d,:]=[%f,%f]" % (i,F_com[i,0],F_com[i,1]); - return ax; - -''' Plot the polytope A*x+b>=0 with vectices V ''' -def plot_polytope(A, b, V=None, color='b', ax=None, plotLines=True, lw=4): - if(ax==None): - f, ax = plut.create_empty_figure(); - - if(plotLines): - plot_inequalities(A, b, [-1,1], [-1,1], color=color, ls='--', ax=ax, lw=lw); - n = b.shape[0]; - if(n<2): - return (ax,None); - - if(V==None): - V = np.zeros((n,2)); + pass + # print "[WARNING] Could not print one inequality as all coefficients are 0: F_com[%d,:]=[%f,%f]" % (i,F_com[i,0],F_com[i,1]); + return ax + + +""" Plot the polytope A*x+b>=0 with vectices V """ + + +def plot_polytope(A, b, V=None, color="b", ax=None, plotLines=True, lw=4): + if ax == None: + f, ax = plut.create_empty_figure() + + if plotLines: + plot_inequalities(A, b, [-1, 1], [-1, 1], color=color, ls="--", ax=ax, lw=lw) + n = b.shape[0] + if n < 2: + return (ax, None) + + if V == None: + V = np.zeros((n, 2)) for i in range(n): - V[i,:] = find_intersection(A[i,:], b[i], A[(i+1)%n,:], b[(i+1)%n]); - - xx = np.zeros(2); - yy = np.zeros(2); + V[i, :] = find_intersection( + A[i, :], b[i], A[(i + 1) % n, :], b[(i + 1) % n] + ) + + xx = np.zeros(2) + yy = np.zeros(2) for i in range(n): - xx[0] = V[i,0]; - xx[1] = V[(i+1)%n,0]; - yy[0] = V[i,1]; - yy[1] = V[(i+1)%n,1]; - line, = ax.plot(xx, yy, color=color, ls='-', lw=2*lw); - - return (ax, line); - + xx[0] = V[i, 0] + xx[1] = V[(i + 1) % n, 0] + yy[0] = V[i, 1] + yy[1] = V[(i + 1) % n, 1] + (line,) = ax.plot(xx, yy, color=color, ls="-", lw=2 * lw) + + return (ax, line) + + def compute_convex_hull(S): """ Returns the matrix A and the vector b such that: @@ -179,4 +206,4 @@ def compute_convex_hull(S): P = cdd.Polyhedron(V_cdd) H = np.array(P.get_inequalities()) b, A = H[:, 0], H[:, 1:] - return (A,b) \ No newline at end of file + return (A, b) diff --git a/sl1m/tools/heightmap_generator.py b/sl1m/tools/heightmap_generator.py new file mode 100644 index 0000000..fd9200d --- /dev/null +++ b/sl1m/tools/heightmap_generator.py @@ -0,0 +1,74 @@ +import numpy as np +import matplotlib.pyplot as plt +import pickle +import os + +import sl1m.tools.plot_tools as plot +from sl1m.tools.heightmap_tools import Heightmap + +from solo_rbprm.solo_abstract import Robot + +from hpp.corbaserver.affordance.affordance import AffordanceTool +from hpp.corbaserver.problem_solver import ProblemSolver +from hpp.gepetto import ViewerFactory + +# --------------------------------- PROBLEM DEFINITION --------------------------------------------------------------- + +# ENV_URDF = "/local/users/frisbourg/install/share/hpp_environments/urdf/Solo3D/floor_sparse.urdf" +# ENV_HEIGHTMAP = "/local/users/frisbourg/install/share/hpp_environments/heightmaps/Solo3D/floor_sparse.pickle" + +# ENV_URDF = "/local/users/frisbourg/install/share/hpp_environments/urdf/Solo3D/floor_sparse.urdf" +# ENV_HEIGHTMAP = "/local/users/frisbourg/install/share/hpp_environments/heightmaps/Solo3D/floor_sparse.pickle" + +ENV_URDF = "/local/users/frisbourg/install/share/hpp_environments/urdf/Solo3D/floor_angles.urdf" +ENV_HEIGHTMAP = "/local/users/frisbourg/install/share/hpp_environments/heightmaps/Solo3D/floor_angles.pickle" + +# ENV_URDF = "/local/users/frisbourg/install/share/hpp_environments/urdf/Solo3D/stairs_rotation.urdf" +# ENV_HEIGHTMAP = "/local/users/frisbourg/install/share/hpp_environments/heightmaps/Solo3D/stairs_rotation.pickle" + +N_X = 100 +N_Y = 100 +X_BOUNDS = [-4.0, 4.0] +Y_BOUNDS = [-4.0, 4.0] + +LIMBS = [ + "solo_RHleg_rom", + "solo_LHleg_rom", + "solo_LFleg_rom", + "solo_RFleg_rom", +] + +# --------------------------------- METHODS --------------------------------------------------------------- + + +def init_affordance(): + """ + Initialize the affordance tool and return the solo abstract rbprm builder, the surface + dictionary and all the affordance points + """ + robot = Robot() + robot.setJointBounds("root_joint", [-5.0, 5.0, -5.0, 5.0, 0.241, 1.5]) + robot.boundSO3([-3.14, 3.14, -0.01, 0.01, -0.01, 0.01]) + robot.setFilter(LIMBS) + for limb in LIMBS: + robot.setAffordanceFilter(limb, ["Support"]) + ps = ProblemSolver(robot) + vf = ViewerFactory(ps) + afftool = AffordanceTool() + afftool.setAffordanceConfig("Support", [0.5, 0.03, 0.00005]) + afftool.loadObstacleModel(ENV_URDF, "environment", vf) + ps.selectPathValidation("RbprmPathValidation", 0.05) + + return afftool.getAffordancePoints("Support") + + +# --------------------------------- MAIN --------------------------------------------------------------- +if __name__ == "__main__": + affordances = init_affordance() + + heightmap = Heightmap(N_X, N_Y, X_BOUNDS, Y_BOUNDS) + heightmap.build(affordances) + heightmap.save_pickle(ENV_HEIGHTMAP) + + ax_heightmap = plot.plot_heightmap(heightmap) + plt.show() diff --git a/sl1m/tools/heightmap_tools.py b/sl1m/tools/heightmap_tools.py new file mode 100644 index 0000000..cdaa721 --- /dev/null +++ b/sl1m/tools/heightmap_tools.py @@ -0,0 +1,145 @@ +from numpy import identity, zeros, ones, array +import numpy as np +import hppfcl +from time import perf_counter as clock +import trimesh +import pickle + + +class Heightmap: + def __init__(self, n_x, n_y, x_lim, y_lim): + """ + :param n_x number of samples in x + :param n_y number of samples in y + :param x_lim bounds in x + :param y_lim bounds in y + """ + + self.n_x = n_x + self.n_y = n_y + + self.x = np.linspace(x_lim[0], x_lim[1], n_x) + self.y = np.linspace(y_lim[0], y_lim[1], n_y) + + self.z = np.zeros((n_x, n_y)) + + def save_pickle(self, filename): + filehandler = open(filename, "wb") + pickle.dump(self, filehandler) + + def build(self, affordances): + """ + Build the heightmap and return it + For each slot in the grid create a vertical segment and check its collisions with the + affordances until one is found + :param affordances list of affordances + """ + for i in range(self.n_x): + for j in range(self.n_y): + p1 = np.array([self.x[i], self.y[j], -1.0]) + p2 = np.array([self.x[i], self.y[j], 10.0]) + segment = np.array([p1, p2]) + fcl_segment = convex(segment, [0, 1, 0]) + + intersections = [] + for affordance in affordances: + fcl_affordance = affordance_to_convex(affordance) + if distance(fcl_affordance, fcl_segment) < 0: + for triangle_list in affordance: + triangle = [np.array(p) for p in triangle_list] + if intersect_line_triangle(segment, triangle): + intersections.append( + get_point_intersect_line_triangle( + segment, triangle + )[2] + ) + + if len(intersections) != 0: + self.z[i, j] = np.max(np.array(intersections)) + + def map_index(self, x, y): + """ + Get the i, j indices of a given position in the heightmap + """ + i = np.searchsorted(self.x, x) - 1 + j = np.searchsorted(self.y, y) - 1 + return i, j + + +def affordance_to_convex(affordance): + """ + Creates a hpp-FCL convex object with an affordance + """ + vertices = hppfcl.StdVec_Vec3f() + faces = hppfcl.StdVec_Triangle() + for triangle_list in affordance: + [vertices.append(np.array(p)) for p in triangle_list] + faces.append(hppfcl.Triangle(0, 1, 2)) + return hppfcl.Convex(vertices, faces) + + +def convex(points, indices): + """ + Creates a hpp-FCL convex object with a list of points and three indices of the vertices of the + triangle (or segment) + """ + vertices = hppfcl.StdVec_Vec3f() + faces = hppfcl.StdVec_Triangle() + vertices.extend(points) + faces.append(hppfcl.Triangle(indices[0], indices[1], indices[2])) + return hppfcl.Convex(vertices, faces) + + +def distance(object1, object2): + """ + Returns the distance between object1 and object2 + """ + guess = np.array([1.0, 0.0, 0.0]) + support_hint = np.array([0, 0], dtype=np.int32) + + shape = hppfcl.MinkowskiDiff() + shape.set(object1, object2, hppfcl.Transform3f(), hppfcl.Transform3f()) + gjk = hppfcl.GJK(150, 1e-8) + gjk.evaluate(shape, guess, support_hint) + return gjk.distance + + +# Method to intersect triangle and segment +def signed_tetra_volume(a, b, c, d): + return np.sign(np.dot(np.cross(b - a, c - a), d - a) / 6.0) + + +def intersect_line_triangle(segment, triangle): + s1 = signed_tetra_volume(segment[0], triangle[0], triangle[1], triangle[2]) + s2 = signed_tetra_volume(segment[1], triangle[0], triangle[1], triangle[2]) + + if s1 != s2: + s3 = signed_tetra_volume(segment[0], segment[1], triangle[0], triangle[1]) + s4 = signed_tetra_volume(segment[0], segment[1], triangle[1], triangle[2]) + s5 = signed_tetra_volume(segment[0], segment[1], triangle[2], triangle[0]) + + if s3 == s4 and s4 == s5: + return True + else: + return False + else: + return False + + +def get_point_intersect_line_triangle(segment, triangle): + s1 = signed_tetra_volume(segment[0], triangle[0], triangle[1], triangle[2]) + s2 = signed_tetra_volume(segment[1], triangle[0], triangle[1], triangle[2]) + + if s1 != s2: + s3 = signed_tetra_volume(segment[0], segment[1], triangle[0], triangle[1]) + s4 = signed_tetra_volume(segment[0], segment[1], triangle[1], triangle[2]) + s5 = signed_tetra_volume(segment[0], segment[1], triangle[2], triangle[0]) + + if s3 == s4 and s4 == s5: + n = np.cross(triangle[1] - triangle[0], triangle[2] - triangle[0]) + t = np.dot(triangle[0] - segment[0], n) / np.dot(segment[1] - segment[0], n) + return segment[0] + t * (segment[1] - segment[0]) + else: + return np.zeros(3) + else: + return np.zeros(3) diff --git a/sl1m/tools/obj_to_constraints.py b/sl1m/tools/obj_to_constraints.py index 20629f9..ced85f1 100644 --- a/sl1m/tools/obj_to_constraints.py +++ b/sl1m/tools/obj_to_constraints.py @@ -1,135 +1,145 @@ -#do the loading of the obj file +# do the loading of the obj file import numpy as np from collections import namedtuple + ObjectData = namedtuple("ObjectData", "V T N F") Inequalities = namedtuple("Inequality", "A b N V") __EPS = 0.0 + def toFloat(stringArray): - res= np.zeros(len(stringArray)) - for i in range(0,len(stringArray)): - res[i] = float(stringArray[i]) - return res - -def load_obj(filename) : - V = [] #vertex - T = [] #texcoords - N = [] #normals - F = [] #face indexies - - fh = open(filename) - for line in fh : - if line[0] == '#' : continue - - line = line.strip().split(' ') - if line[0] == 'v' : #vertex - V.append(toFloat(line[1:])) - elif line[0] == 'vt' : #tex-coord - T.append(line[1:]) - elif line[0] == 'vn' : #normal vector - N.append(toFloat(line[1:])) - elif line[0] == 'f' : #face - face = line[1:] - for i in range(0, len(face)) : - face[i] = face[i].split('/') - # OBJ indexies are 1 based not 0 based hence the -1 - # convert indexies to integer - for j in range(0, len(face[i])): - if j!=1: - face[i][j] = int(face[i][j]) - 1 - F.append(face) - fh.close() - return ObjectData(V, T, N, F) - - -#find a point such that ax + by + cz = d that is the closest to the origin -def find_point_on_plane(a,b,c,d): - #project 0 to the plane - m = np.zeros((4,4)) - m[:3,:3] = np.identity(3) - m[:,3] = [-a, -b, -c, 0] - m[3,:3] = [a,b,c] - n = np.zeros(4); n[-1] = d - res = np.linalg.inv(m).dot(n)[:-1] - - - return res - - - -#Create an Inequalities object given the inequalities matrices A,b, s.t. Ax <=b -def inequalities_to_Inequalities_object(A,b): - nrows = A.shape[0] - V = np.ones([nrows, 4]) - N = np.empty([nrows, 3]) - i = 0 - for ai, bi in zip(A,b): - N[i,:] = ai - V[i,:3] = find_point_on_plane(ai[0],ai[1],ai[2],bi) - i+=1 - return Inequalities(A,b, N, V) - - -def inequality(v, n): - #the plan has for equation ax + by + cz = d, with a b c coordinates of the normal - #inequality is then ax + by +cz -d <= 0 - # last var is v because we need it - return [n[0], n[1], n[2], np.array(v).dot(np.array(n)) + __EPS] - + res = np.zeros(len(stringArray)) + for i in range(0, len(stringArray)): + res[i] = float(stringArray[i]) + return res + + +def load_obj(filename): + V = [] # vertex + T = [] # texcoords + N = [] # normals + F = [] # face indexies + + fh = open(filename) + for line in fh: + if line[0] == "#": + continue + + line = line.strip().split(" ") + if line[0] == "v": # vertex + V.append(toFloat(line[1:])) + elif line[0] == "vt": # tex-coord + T.append(line[1:]) + elif line[0] == "vn": # normal vector + N.append(toFloat(line[1:])) + elif line[0] == "f": # face + face = line[1:] + for i in range(0, len(face)): + face[i] = face[i].split("/") + # OBJ indexies are 1 based not 0 based hence the -1 + # convert indexies to integer + for j in range(0, len(face[i])): + if j != 1: + face[i][j] = int(face[i][j]) - 1 + F.append(face) + fh.close() + return ObjectData(V, T, N, F) + + +# find a point such that ax + by + cz = d that is the closest to the origin +def find_point_on_plane(a, b, c, d): + # project 0 to the plane + m = np.zeros((4, 4)) + m[:3, :3] = np.identity(3) + m[:, 3] = [-a, -b, -c, 0] + m[3, :3] = [a, b, c] + n = np.zeros(4) + n[-1] = d + res = np.linalg.inv(m).dot(n)[:-1] + + return res + + +# Create an Inequalities object given the inequalities matrices A,b, s.t. Ax <=b +def inequalities_to_Inequalities_object(A, b): + nrows = A.shape[0] + V = np.ones([nrows, 4]) + N = np.empty([nrows, 3]) + i = 0 + for ai, bi in zip(A, b): + N[i, :] = ai + V[i, :3] = find_point_on_plane(ai[0], ai[1], ai[2], bi) + i += 1 + return Inequalities(A, b, N, V) + + +def inequality(v, n): + # the plan has for equation ax + by + cz = d, with a b c coordinates of the normal + # inequality is then ax + by +cz -d <= 0 + # last var is v because we need it + return [n[0], n[1], n[2], np.array(v).dot(np.array(n)) + __EPS] + + def as_inequalities(obj): - #for each face, find first three points and deduce plane - #inequality is given by normal - A= np.empty([len(obj.F), 3]) - b = np.empty(len(obj.F)) - V = np.ones([len(obj.F), 4]) - N = np.empty([len(obj.F), 3]) - for f in range(0, len(obj.F)): - face = obj.F[f] - v = obj.V[face[0][0]] - # assume normals are in obj - n = obj.N[face[0][2]] - ineq = inequality(v,n) - A[f,:] = ineq[0:3] - b[f] = ineq[3] - V[f,0:3] = v - N[f,:] = n - return Inequalities(A,b, N, V) - + # for each face, find first three points and deduce plane + # inequality is given by normal + A = np.empty([len(obj.F), 3]) + b = np.empty(len(obj.F)) + V = np.ones([len(obj.F), 4]) + N = np.empty([len(obj.F), 3]) + for f in range(0, len(obj.F)): + face = obj.F[f] + v = obj.V[face[0][0]] + # assume normals are in obj + n = obj.N[face[0][2]] + ineq = inequality(v, n) + A[f, :] = ineq[0:3] + b[f] = ineq[3] + V[f, 0:3] = v + N[f, :] = n + return Inequalities(A, b, N, V) + + def is_inside(inequalities, pt): - return ((inequalities.A.dot(pt) - inequalities.b) < 0).all() + return ((inequalities.A.dot(pt) - inequalities.b) < 0).all() -#~ def rotate_inequalities_q(): + +# ~ def rotate_inequalities_q(): # TODO this is naive, should be a way to simply update d def rotate_inequalities(ineq, transform): - #for each face, find first three points and deduce plane - #inequality is given by normal - A = np.empty([len(ineq.A), 3]) - b = np.empty(len(ineq.b)) - V = np.ones([len(ineq.V), 4]) - N = np.ones([len(ineq.N), 3]) - for i in range(0, len(b)): - v = transform.dot(ineq.V[i,:]) - n = transform[0:3,0:3].dot(ineq.N[i,0:3]) - ine = inequality(v[0:3],n[0:3]) - A[i,:] = ine[0:3] - b[i] = ine[3] - V[i,:] = v - N[i,:] = n - return Inequalities(A,b, N, V) + # for each face, find first three points and deduce plane + # inequality is given by normal + A = np.empty([len(ineq.A), 3]) + b = np.empty(len(ineq.b)) + V = np.ones([len(ineq.V), 4]) + N = np.ones([len(ineq.N), 3]) + for i in range(0, len(b)): + v = transform.dot(ineq.V[i, :]) + n = transform[0:3, 0:3].dot(ineq.N[i, 0:3]) + ine = inequality(v[0:3], n[0:3]) + A[i, :] = ine[0:3] + b[i] = ine[3] + V[i, :] = v + N[i, :] = n + return Inequalities(A, b, N, V) + from pickle import dump + + def ineq_to_file(ineq, filename): - f1=open(filename, 'w+') - res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V} - dump(res, f1) - f1.close() - + f1 = open(filename, "w+") + res = {"A": ineq.A, "b": ineq.b, "N": ineq.N, "V": ineq.V} + dump(res, f1) + f1.close() + + from pickle import load -def ineq_from_file(filename): - f1=open(filename, 'r') - res = load(f1) - return Inequalities(res['A'], res['b'],res['N'],res['V']) - + +def ineq_from_file(filename): + f1 = open(filename, "r") + res = load(f1) + return Inequalities(res["A"], res["b"], res["N"], res["V"]) diff --git a/sl1m/tools/plot_plytopes.py b/sl1m/tools/plot_plytopes.py deleted file mode 100644 index 7aca6b5..0000000 --- a/sl1m/tools/plot_plytopes.py +++ /dev/null @@ -1,59 +0,0 @@ -import numpy as np -from hpp_centroidal_dynamics import * -from numpy import array, asmatrix, matrix, zeros, ones -from numpy import array, dot, vstack, hstack, asmatrix, identity, cross -from numpy.linalg import norm - -from scipy.spatial import ConvexHull -from hpp_bezier_com_traj import * -#~ from qp import solve_lp - -import eigenpy -import cdd -from random import random as rd -from random import randint as rdi -from numpy import squeeze, asarray - -eigenpy.switchToNumpyArray() - - - -from sl1m.constants_and_tools import * - - - -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D - - -def plot_hull_in_subplot(hull, pts, apts, ax, color = "r", just_pts = False): - # Plot defining corner points - #~ ax.plot(apts.T[0], apts.T[1], apts.T[2], "ko") - if not just_pts: - for s in hull.simplices: - s = np.append(s, s[0]) # Here we cycle back to the first coordinate - ax.plot(apts[s, 0], apts[s, 1], apts[s, 2], color+"-") - - -def plot_hull(hull, pts, apts, color = "r", just_pts = False, ax = None): - if ax is None: - fig = plt.figure() - ax = fig.add_subplot(111, projection="3d") - plot_hull_in_subplot(hull, pts, array(pts), ax, color, just_pts) - #~ plt.show() - -def plot_polytope_H_rep(A_in,b_in, color = "r", just_pts = False, ax = None): - hull, pts, apts, cd = genPolytope(A_in,b_in) - plot_hull(hull, pts, apts, color, just_pts, ax = ax) - -def plot_polytope_V_rep(pts, color = "r", just_pts = False, ax = None): - pts = [array(el) for el in pts] - apts = array(pts) - hull = ConvexHull(apts, qhull_options='Q12') - plot_hull(hull, pts, apts, color, just_pts, ax = ax) - -def plot_polytope_CDD_PolyHeron(H, color = "r", just_pts = False): - g = H.get_generators() - pts = [array(g[el][1:]) for el in range(g.row_size)] - plot_polytope_V_rep(pts, color, just_pts) - diff --git a/sl1m/tools/plot_tools.py b/sl1m/tools/plot_tools.py new file mode 100644 index 0000000..1565644 --- /dev/null +++ b/sl1m/tools/plot_tools.py @@ -0,0 +1,232 @@ +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import numpy as np + + +COLORS = [ + "#1f77b4", + "#ff7f0e", + "#2ca02c", + "#d62728", + "#e377c2", + "#7f7f7f", + "#bcbd22", + "#17becf", +] + + +def plot_point(ax, point, color="b", linewidth=2): + """ + Plot a point + """ + x = np.array(point)[0] + y = np.array(point)[1] + z = np.array(point)[2] + ax.scatter(x, y, z, color=color, marker="o", linewidth=linewidth) + + +def plot_surface(points, ax, color_id=0, alpha=1.0): + """ + Plot a surface + """ + xs = np.append(points[0, :], points[0, 0]).tolist() + ys = np.append(points[1, :], points[1, 0]).tolist() + zs = np.append(points[2, :], points[2, 0]).tolist() + if color_id == -1: + ax.plot(xs, ys, zs) + else: + ax.plot(xs, ys, zs, color=COLORS[color_id % len(COLORS)], alpha=alpha) + + +def draw_potential_surfaces(surfaces, gait, phase, ax=None, alpha=1.0): + """ + Plot all the potential surfaces of one phase of the problem + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + for surface in surfaces: + plot_surface(surface, ax, gait[phase % (len(gait))]) + return ax + + +def draw_potential_surfaces_gait(surfaces, phase, foot_index, ax=None, title=None): + """ + Plot all the potential surfaces of one phase of the problem + """ + if ax is None: + fig = plt.figure() + if title is not None: + fig.suptitle(title, fontsize=16) + ax = fig.add_subplot(111, projection="3d") + for surface in surfaces[phase][foot_index]: + plot_surface(surface, ax, foot_index) + return ax + + +def draw_whole_scene(surface_dict, ax=None, title=None): + """ + Plot all the potential surfaces + """ + if ax is None: + fig = plt.figure() + if title is not None: + fig.suptitle(title, fontsize=16) + ax = fig.add_subplot(111, projection="3d") + for key in surface_dict.keys(): + plot_surface(np.array(surface_dict[key][0]).T, ax, 5) + return ax + + +def draw_scene(surfaces, gait=False, ax=None, alpha=1.0): + """ + Plot all the potential surfaces of the problem + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + for surfaces_phase in surfaces: + for surface in surfaces_phase: + if gait: + for foot_surface in surface: + plot_surface(foot_surface, ax, 0, alpha=alpha) + else: + plot_surface(surface, ax, 0, alpha=alpha) + return ax + + +def draw_surface(surfaces, foot, ax=None): + """ + Plot all the potential surfaces + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + for surface in surfaces[0]: + plot_surface(surface, ax, foot) + return ax + + +def plot_initial_contacts(initial_feet_pose, ax=None): + """ + Plot the initial feet positions + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.grid(False) + + for i, pose in enumerate(initial_feet_pose): + if pose is not None: + plot_point(ax, pose, color=COLORS[i]) + + +def plot_new_contact(moving_feet, moving_feet_pos, ax=None): + """ + Plot the initial feet positions + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.grid(False) + + for i in range(len(moving_feet)): + plot_point(ax, moving_feet_pos[i], color=COLORS[moving_feet[i]]) + + +def plot_first_step(configs, coms, moving_foot_pos, gait, ax=None): + """ + Plot the moving feet positions + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.grid(False) + + plot_point(ax, configs[0], color=COLORS[len(gait) + 1]) + plot_point(ax, coms[0], color=COLORS[len(gait) + 2]) + plot_point(ax, moving_foot_pos[0], color=COLORS[gait[0]]) + + +def plot_selected_surfaces(surfaces, surface_indices, gait, ax=None): + """ + Plot the surface with the minimum alpha + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + for i, surfaces_phase in enumerate(surfaces): + plot_surface(surfaces_phase[surface_indices[i]], ax, gait[i % len(gait)]) + return ax + + +def plot_heightmap(heightmap, alpha=1.0, ax=None): + """ + Plot the heightmap + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + i = 0 + if alpha != 1.0: + i = 1 + + xv, yv = np.meshgrid(heightmap.x, heightmap.y, sparse=False, indexing="ij") + ax.plot_surface(xv, yv, heightmap.z, color=COLORS[i], alpha=alpha) + + ax.set_xlabel("x") + ax.set_ylabel("y") + ax.set_zlabel("z") + ax.set_zlim([np.min(heightmap.z), np.max(heightmap.z) + 1.0]) + + return ax + + +def plot_point_list(ax, wps, color="b", D3=True, linewidth=2): + """ + Plot a list of points + """ + x = np.array(wps)[:, 0] + y = np.array(wps)[:, 1] + if D3: + z = np.array(wps)[:, 2] + ax.scatter(x, y, z, c=color, marker="o", linewidth=5) + else: + ax.scatter(x, y, color=color, linewidth=linewidth) + + +def plot_planner_result(all_feet_pos, coms=None, step_size=None, ax=None, show=True): + """ + Plot the feet positions and com positions + """ + if ax is None: + fig = plt.figure() + ax = fig.add_subplot(111, projection="3d") + ax.grid(False) + ax.view_init(elev=8.776933438381377, azim=-99.32358055821186) + + for foot, foot_pose in enumerate(all_feet_pos): + px = [c[0] for c in foot_pose if c is not None] + py = [c[1] for c in foot_pose if c is not None] + pz = [c[2] for c in foot_pose if c is not None] + ax.scatter(px, py, pz, color=COLORS[foot], marker="o", linewidth=5) + ax.plot(px, py, pz, color=COLORS[foot]) + + if step_size is not None: + for foot_pose in all_feet_pos: + px = [foot_pose[0][0] + step_size[0]] + py = [foot_pose[0][1] + step_size[1]] + pz = [foot_pose[0][2]] + ax.scatter(px, py, pz, color=COLORS[5], marker="o", linewidth=5) + ax.plot(px, py, pz, color=COLORS[5]) + + if coms is not None: + plot_point_list(ax, coms, color=COLORS[len(all_feet_pos) + 1]) + cx = [c[0] for c in coms] + cy = [c[1] for c in coms] + cz = [c[2] for c in coms] + ax.plot(cx, cy, cz, color=COLORS[len(all_feet_pos) + 1]) + + if show: + plt.draw() + plt.show() diff --git a/sl1m/tools/plot_utils.py b/sl1m/tools/plot_utils.py index 02c8e3a..04624a1 100644 --- a/sl1m/tools/plot_utils.py +++ b/sl1m/tools/plot_utils.py @@ -9,242 +9,423 @@ import matplotlib.ticker as ticker import numpy as np -DEFAULT_FONT_SIZE = 40; -DEFAULT_AXIS_FONT_SIZE = DEFAULT_FONT_SIZE; -DEFAULT_LINE_WIDTH = 8; #13; -DEFAULT_MARKER_SIZE = 6; -DEFAULT_FONT_FAMILY = 'sans-serif' -DEFAULT_FONT_SIZE = DEFAULT_FONT_SIZE; -DEFAULT_FONT_SERIF = ['Times New Roman', 'Times','Bitstream Vera Serif', 'DejaVu Serif', 'New Century Schoolbook', 'Century Schoolbook L', 'Utopia', 'ITC Bookman', 'Bookman', 'Nimbus Roman No9 L', 'Palatino', 'Charter', 'serif']; -DEFAULT_FIGURE_FACE_COLOR = 'white' # figure facecolor; 0.75 is scalar gray -DEFAULT_LEGEND_FONT_SIZE = DEFAULT_FONT_SIZE; -DEFAULT_AXES_LABEL_SIZE = DEFAULT_FONT_SIZE; # fontsize of the x any y labels -DEFAULT_TEXT_USE_TEX = True; -LINE_ALPHA = 0.9; -SAVE_FIGURES = False; -FILE_EXTENSIONS = ['png']; #,'eps']; -FIGURES_DPI = 150; -SHOW_LEGENDS = False; -LEGEND_ALPHA = 0.5; -SHOW_FIGURES = False; -FIGURE_PATH = './'; -LINE_WIDTH_RED = 0; # reduction of line width when plotting multiple lines on same plot -LINE_WIDTH_MIN = 1; -BOUNDS_COLOR = 'silver'; - -#legend.framealpha : 1.0 # opacity of of legend frame -#axes.hold : True # whether to clear the axes by default on -#axes.linewidth : 1.0 # edge linewidth -#axes.titlesize : large # fontsize of the axes title -#axes.color_cycle : b, g, r, c, m, y, k # color cycle for plot lines -#xtick.labelsize : medium # fontsize of the tick labels -#figure.dpi : 80 # figure dots per inch -#image.cmap : jet # gray | jet etc... -#savefig.dpi : 100 # figure dots per inch -#savefig.facecolor : white # figure facecolor when saving -#savefig.edgecolor : white # figure edgecolor when saving -#savefig.format : png # png, ps, pdf, svg -#savefig.jpeg_quality: 95 # when a jpeg is saved, the default quality parameter. -#savefig.directory : ~ # default directory in savefig dialog box, - # leave empty to always use current working directory - -def create_empty_figure(nRows=1, nCols=1, spinesPos=None,sharex=True): - f, ax = plt.subplots(nRows,nCols,sharex=sharex); +DEFAULT_FONT_SIZE = 40 +DEFAULT_AXIS_FONT_SIZE = DEFAULT_FONT_SIZE +DEFAULT_LINE_WIDTH = 8 +# 13; +DEFAULT_MARKER_SIZE = 6 +DEFAULT_FONT_FAMILY = "sans-serif" +DEFAULT_FONT_SIZE = DEFAULT_FONT_SIZE +DEFAULT_FONT_SERIF = [ + "Times New Roman", + "Times", + "Bitstream Vera Serif", + "DejaVu Serif", + "New Century Schoolbook", + "Century Schoolbook L", + "Utopia", + "ITC Bookman", + "Bookman", + "Nimbus Roman No9 L", + "Palatino", + "Charter", + "serif", +] +DEFAULT_FIGURE_FACE_COLOR = "white" # figure facecolor; 0.75 is scalar gray +DEFAULT_LEGEND_FONT_SIZE = DEFAULT_FONT_SIZE +DEFAULT_AXES_LABEL_SIZE = DEFAULT_FONT_SIZE +# fontsize of the x any y labels +DEFAULT_TEXT_USE_TEX = True +LINE_ALPHA = 0.9 +SAVE_FIGURES = False +FILE_EXTENSIONS = ["png"] +# ,'eps']; +FIGURES_DPI = 150 +SHOW_LEGENDS = False +LEGEND_ALPHA = 0.5 +SHOW_FIGURES = False +FIGURE_PATH = "./" +LINE_WIDTH_RED = 0 +# reduction of line width when plotting multiple lines on same plot +LINE_WIDTH_MIN = 1 +BOUNDS_COLOR = "silver" + +# legend.framealpha : 1.0 # opacity of of legend frame +# axes.hold : True # whether to clear the axes by default on +# axes.linewidth : 1.0 # edge linewidth +# axes.titlesize : large # fontsize of the axes title +# axes.color_cycle : b, g, r, c, m, y, k # color cycle for plot lines +# xtick.labelsize : medium # fontsize of the tick labels +# figure.dpi : 80 # figure dots per inch +# image.cmap : jet # gray | jet etc... +# savefig.dpi : 100 # figure dots per inch +# savefig.facecolor : white # figure facecolor when saving +# savefig.edgecolor : white # figure edgecolor when saving +# savefig.format : png # png, ps, pdf, svg +# savefig.jpeg_quality: 95 # when a jpeg is saved, the default quality parameter. +# savefig.directory : ~ # default directory in savefig dialog box, +# leave empty to always use current working directory + + +def create_empty_figure(nRows=1, nCols=1, spinesPos=None, sharex=True): + f, ax = plt.subplots(nRows, nCols, sharex=sharex) mngr = plt.get_current_fig_manager() - mngr.window.setGeometry(50,50,1080,720); + mngr.window.setGeometry(50, 50, 1080, 720) - if(spinesPos!=None): - if(nRows*nCols>1): - for axis in ax.reshape(nRows*nCols): - movePlotSpines(axis, spinesPos); + if spinesPos != None: + if nRows * nCols > 1: + for axis in ax.reshape(nRows * nCols): + movePlotSpines(axis, spinesPos) else: - movePlotSpines(ax, spinesPos); - return (f, ax); - + movePlotSpines(ax, spinesPos) + return (f, ax) + + def movePlotSpines(ax, spinesPos): - ax.spines['right'].set_color('none') - ax.spines['top'].set_color('none') - ax.xaxis.set_ticks_position('bottom') - ax.spines['bottom'].set_position(('data',spinesPos[0])) - ax.yaxis.set_ticks_position('left') - ax.spines['left'].set_position(('data',spinesPos[1])) - + ax.spines["right"].set_color("none") + ax.spines["top"].set_color("none") + ax.xaxis.set_ticks_position("bottom") + ax.spines["bottom"].set_position(("data", spinesPos[0])) + ax.yaxis.set_ticks_position("left") + ax.spines["left"].set_position(("data", spinesPos[1])) + + def setAxisFontSize(ax, size): for label in ax.get_xticklabels() + ax.get_yticklabels(): label.set_fontsize(size) - label.set_bbox(dict(facecolor='white', edgecolor='None', alpha=0.65)) - + label.set_bbox(dict(facecolor="white", edgecolor="None", alpha=0.65)) + + mpl.rcdefaults() -mpl.rcParams['lines.linewidth'] = DEFAULT_LINE_WIDTH; -mpl.rcParams['lines.markersize'] = DEFAULT_MARKER_SIZE; -mpl.rcParams['font.family'] = DEFAULT_FONT_FAMILY; -mpl.rcParams['font.size'] = DEFAULT_FONT_SIZE; -mpl.rcParams['font.serif'] = DEFAULT_FONT_SERIF; -mpl.rcParams['text.usetex'] = DEFAULT_TEXT_USE_TEX; -mpl.rcParams['axes.labelsize'] = DEFAULT_AXES_LABEL_SIZE; -mpl.rcParams['legend.fontsize'] = DEFAULT_LEGEND_FONT_SIZE; -mpl.rcParams['figure.facecolor'] = DEFAULT_FIGURE_FACE_COLOR; -mpl.rcParams['figure.figsize'] = 12, 9 #23, 12 # - -def plot3dQuantity(quantity, title, ax=None, boundUp=None, boundLow=None, yscale='linear', linestyle='k'): - return plotNdQuantity(3, 1, quantity, title, ax, boundUp, boundLow, yscale, linestyle); - -def plotNdQuantity(nRows, nCols, quantity, title="", ax=None, boundUp=None, boundLow=None, yscale='linear', - linestyle='k--', sharey=False, margins=None): - t = quantity.shape[0]; - n = quantity.shape[1]; - if(margins!=None): - if(type(margins) is list): - margins = [margins[0].reshape(t,1,n), margins[1].reshape(t,1,n)]; +mpl.rcParams["lines.linewidth"] = DEFAULT_LINE_WIDTH +mpl.rcParams["lines.markersize"] = DEFAULT_MARKER_SIZE +mpl.rcParams["font.family"] = DEFAULT_FONT_FAMILY +mpl.rcParams["font.size"] = DEFAULT_FONT_SIZE +mpl.rcParams["font.serif"] = DEFAULT_FONT_SERIF +mpl.rcParams["text.usetex"] = DEFAULT_TEXT_USE_TEX +mpl.rcParams["axes.labelsize"] = DEFAULT_AXES_LABEL_SIZE +mpl.rcParams["legend.fontsize"] = DEFAULT_LEGEND_FONT_SIZE +mpl.rcParams["figure.facecolor"] = DEFAULT_FIGURE_FACE_COLOR +mpl.rcParams["figure.figsize"] = 12, 9 # 23, 12 # + + +def plot3dQuantity( + quantity, + title, + ax=None, + boundUp=None, + boundLow=None, + yscale="linear", + linestyle="k", +): + return plotNdQuantity( + 3, 1, quantity, title, ax, boundUp, boundLow, yscale, linestyle + ) + + +def plotNdQuantity( + nRows, + nCols, + quantity, + title="", + ax=None, + boundUp=None, + boundLow=None, + yscale="linear", + linestyle="k--", + sharey=False, + margins=None, +): + t = quantity.shape[0] + n = quantity.shape[1] + if margins != None: + if type(margins) is list: + margins = [ + margins[0].reshape(t, 1, n), + margins[1].reshape(t, 1, n), + ] else: - margins = margins.reshape(t,1,n); - return plotNdQuantityPerSolver(nRows, nCols, quantity.reshape(t,1,n), title, None, [linestyle], ax, - boundUp, boundLow, yscale, None, None, sharey, margins); - -def plotNdQuantityPerSolver(nRows, nCols, quantity, title, solver_names, line_styles, ax=None, boundUp=None, boundLow=None, - yscale='linear', subplot_titles=None, ylabels=None, sharey=False, margins=None, x=None): - if(ax==None): - f, ax = plt.subplots(nRows, nCols, sharex=True, sharey=sharey); - ax = ax.reshape(nRows, nCols); - k = 0; - if(x==None): - x = range(quantity.shape[0]); + margins = margins.reshape(t, 1, n) + return plotNdQuantityPerSolver( + nRows, + nCols, + quantity.reshape(t, 1, n), + title, + None, + [linestyle], + ax, + boundUp, + boundLow, + yscale, + None, + None, + sharey, + margins, + ) + + +def plotNdQuantityPerSolver( + nRows, + nCols, + quantity, + title, + solver_names, + line_styles, + ax=None, + boundUp=None, + boundLow=None, + yscale="linear", + subplot_titles=None, + ylabels=None, + sharey=False, + margins=None, + x=None, +): + if ax == None: + f, ax = plt.subplots(nRows, nCols, sharex=True, sharey=sharey) + ax = ax.reshape(nRows, nCols) + k = 0 + if x == None: + x = range(quantity.shape[0]) for j in range(nCols): for i in range(nRows): - if(k2*ymin): - ymin = np.min([ymin,boundLow[k]]); - ax[i,j].plot([0, quantity.shape[0]-1], [boundLow[k], boundLow[k]], '--', color=BOUNDS_COLOR, alpha=LINE_ALPHA); + if k < quantity.shape[2]: + if subplot_titles != None: + ax[i, j].set_title(subplot_titles[k]) + elif i == 0: + ax[i, j].set_title(str(k)) + # set titles on first row only + if ylabels != None: + ax[i, j].set_ylabel(ylabels[k]) + + ymin = np.min(quantity[:, :, k]) + ymax = np.max(quantity[:, :, k]) + if boundUp != None: + if len(boundUp.shape) == 1: # constant bound + if boundUp[k] < 2 * ymax: + ymax = np.max([ymax, boundUp[k]]) + ax[i, j].plot( + [0, quantity.shape[0] - 1], + [boundUp[k], boundUp[k]], + "--", + color=BOUNDS_COLOR, + alpha=LINE_ALPHA, + ) + elif ( + len(boundUp.shape) == 2 + ): # bound variable in time but constant for each solver + if np.max(boundUp[:, k]) < 2 * ymax: + ymax = np.max(np.concatenate(([ymax], boundUp[:, k]))) + ax[i, j].plot( + boundUp[:, k], + "--", + color=BOUNDS_COLOR, + label="Upper bound", + alpha=LINE_ALPHA, + ) + if boundLow != None: + if len(boundLow.shape) == 1: + if boundLow[k] > 2 * ymin: + ymin = np.min([ymin, boundLow[k]]) + ax[i, j].plot( + [0, quantity.shape[0] - 1], + [boundLow[k], boundLow[k]], + "--", + color=BOUNDS_COLOR, + alpha=LINE_ALPHA, + ) else: - if(np.min(boundLow[:,k])>2*ymin): - ymin = np.min(np.concatenate(([ymin],boundLow[:,k]))); - ax[i,j].plot(boundLow[:,k], '--', color=BOUNDS_COLOR, label='Lower bound', alpha=LINE_ALPHA); - lw = DEFAULT_LINE_WIDTH; + if np.min(boundLow[:, k]) > 2 * ymin: + ymin = np.min(np.concatenate(([ymin], boundLow[:, k]))) + ax[i, j].plot( + boundLow[:, k], + "--", + color=BOUNDS_COLOR, + label="Lower bound", + alpha=LINE_ALPHA, + ) + lw = DEFAULT_LINE_WIDTH for s in range(quantity.shape[1]): - p, = ax[i,j].plot(x, quantity[:,s,k], line_styles[s], alpha=LINE_ALPHA, linewidth=lw); - if(margins!=None): - if(type(margins) is list): - mp = margins[0]; - mn = margins[1]; + (p,) = ax[i, j].plot( + x, + quantity[:, s, k], + line_styles[s], + alpha=LINE_ALPHA, + linewidth=lw, + ) + if margins != None: + if type(margins) is list: + mp = margins[0] + mn = margins[1] else: - mp = margins; - mn = margins; - ymax = np.max(np.concatenate(([ymax],quantity[:,s,k]+mp[:,s,k]))); - ymin = np.min(np.concatenate(([ymin],quantity[:,s,k]-mn[:,s,k]))); - ax[i,j].fill_between(x, quantity[:,s,k]+mp[:,s,k], quantity[:,s,k]-mn[:,s,k], alpha=0.15, linewidth=0, facecolor='green'); - if(solver_names!=None): - p.set_label(solver_names[s]); - lw=max(LINE_WIDTH_MIN,lw-LINE_WIDTH_RED); - ax[i,j].set_yscale(yscale); - ax[i,j].xaxis.set_ticks(np.arange(0, x[-1], x[-1]/2)); - ax[i,j].yaxis.set_ticks([ymin, ymax]); - if(ymax-ymin>5.0): - ax[i,j].yaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0f')); - elif(ymax-ymin>0.5): - ax[i,j].yaxis.set_major_formatter(ticker.FormatStrFormatter('%0.1f')); + mp = margins + mn = margins + ymax = np.max( + np.concatenate(([ymax], quantity[:, s, k] + mp[:, s, k])) + ) + ymin = np.min( + np.concatenate(([ymin], quantity[:, s, k] - mn[:, s, k])) + ) + ax[i, j].fill_between( + x, + quantity[:, s, k] + mp[:, s, k], + quantity[:, s, k] - mn[:, s, k], + alpha=0.15, + linewidth=0, + facecolor="green", + ) + if solver_names != None: + p.set_label(solver_names[s]) + lw = max(LINE_WIDTH_MIN, lw - LINE_WIDTH_RED) + ax[i, j].set_yscale(yscale) + ax[i, j].xaxis.set_ticks(np.arange(0, x[-1], x[-1] / 2)) + ax[i, j].yaxis.set_ticks([ymin, ymax]) + if ymax - ymin > 5.0: + ax[i, j].yaxis.set_major_formatter( + ticker.FormatStrFormatter("%0.0f") + ) + elif ymax - ymin > 0.5: + ax[i, j].yaxis.set_major_formatter( + ticker.FormatStrFormatter("%0.1f") + ) else: - ax[i,j].yaxis.set_major_formatter(ticker.FormatStrFormatter('%0.2f')); - if(sharey==False): - ax[i,j].set_ylim([ymin-0.1*(ymax-ymin), ymax+0.1*(ymax-ymin)]); - k += 1; + ax[i, j].yaxis.set_major_formatter( + ticker.FormatStrFormatter("%0.2f") + ) + if sharey == False: + ax[i, j].set_ylim( + [ + ymin - 0.1 * (ymax - ymin), + ymax + 0.1 * (ymax - ymin), + ] + ) + k += 1 else: - ax[i,j].yaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0f')); - - if(SAVE_FIGURES): + ax[i, j].yaxis.set_major_formatter(ticker.FormatStrFormatter("%0.0f")) + + if SAVE_FIGURES: for ext in FILE_EXTENSIONS: - plt.gcf().savefig(FIGURE_PATH+title.replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); + plt.gcf().savefig( + FIGURE_PATH + title.replace(" ", "_") + "." + ext, + format=ext, + dpi=FIGURES_DPI, + bbox_inches="tight", + ) else: - ax[nRows/2,0].set_ylabel(title); - if(SHOW_LEGENDS): -# leg = ax[0,0].legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=2, mode="expand", borderaxespad=0.) - leg = ax[0,0].legend(loc='best'); -# leg.get_frame().set_alpha(LEGEND_ALPHA) - return ax; - -def plotQuantityPerSolver(quantity, title, solver_names, line_styles, yscale='linear', ylabel='', - x=None, xlabel='', legend_location='best'): - f, ax = plt.subplots(); - lw = DEFAULT_LINE_WIDTH; - if(x==None): - x = range(quantity.shape[0]); - + ax[nRows / 2, 0].set_ylabel(title) + if SHOW_LEGENDS: + # leg = ax[0,0].legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=2, mode="expand", borderaxespad=0.) + leg = ax[0, 0].legend(loc="best") + # leg.get_frame().set_alpha(LEGEND_ALPHA) + return ax + + +def plotQuantityPerSolver( + quantity, + title, + solver_names, + line_styles, + yscale="linear", + ylabel="", + x=None, + xlabel="", + legend_location="best", +): + f, ax = plt.subplots() + lw = DEFAULT_LINE_WIDTH + if x == None: + x = range(quantity.shape[0]) + for i in range(len(solver_names)): - ax.plot(x, quantity[:,i], line_styles[i], alpha=LINE_ALPHA, linewidth=lw); - lw=max(lw-LINE_WIDTH_RED,LINE_WIDTH_MIN); - ax.set_yscale(yscale); - ax.set_ylabel(ylabel); - ax.set_xlabel(xlabel); - ymin = np.min(quantity); - ymax = np.max(quantity); - ax.set_ylim([ymin-0.1*(ymax-ymin), ymax+0.1*(ymax-ymin)]); - if(SHOW_LEGENDS): - leg = ax.legend(solver_names, loc=legend_location); + ax.plot(x, quantity[:, i], line_styles[i], alpha=LINE_ALPHA, linewidth=lw) + lw = max(lw - LINE_WIDTH_RED, LINE_WIDTH_MIN) + ax.set_yscale(yscale) + ax.set_ylabel(ylabel) + ax.set_xlabel(xlabel) + ymin = np.min(quantity) + ymax = np.max(quantity) + ax.set_ylim([ymin - 0.1 * (ymax - ymin), ymax + 0.1 * (ymax - ymin)]) + if SHOW_LEGENDS: + leg = ax.legend(solver_names, loc=legend_location) leg.get_frame().set_alpha(LEGEND_ALPHA) - if(SAVE_FIGURES): + if SAVE_FIGURES: for ext in FILE_EXTENSIONS: - plt.gcf().savefig(FIGURE_PATH+title.replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); - elif(ylabel==''): - ax.set_ylabel(title); - - -def plotQuantityVsQuantityPerSolver(quantity, quantityPerSolver, legend, solver_names, line_styles, yscale='linear'): - r=0; - c=0; - if(len(solver_names)==4 or len(solver_names)==3): - r=2; - c=2; - elif(len(solver_names)==5 or len(solver_names)==6): - r=2; - c=3; + plt.gcf().savefig( + FIGURE_PATH + title.replace(" ", "_") + "." + ext, + format=ext, + dpi=FIGURES_DPI, + bbox_inches="tight", + ) + elif ylabel == "": + ax.set_ylabel(title) + + +def plotQuantityVsQuantityPerSolver( + quantity, + quantityPerSolver, + legend, + solver_names, + line_styles, + yscale="linear", +): + r = 0 + c = 0 + if len(solver_names) == 4 or len(solver_names) == 3: + r = 2 + c = 2 + elif len(solver_names) == 5 or len(solver_names) == 6: + r = 2 + c = 3 else: - print("ERROR in plotQuantityVsQuantityPerSolver, number of solvers not managed"); - return; - f, ax = plt.subplots(r, c, sharex=True, sharey=True); + print("ERROR in plotQuantityVsQuantityPerSolver, number of solvers not managed") + return + f, ax = plt.subplots(r, c, sharex=True, sharey=True) for i in range(len(solver_names)): - ax[i/c,i%c].plot(quantity[:,i], 'kx-', quantityPerSolver[:,i], line_styles[i], alpha=LINE_ALPHA); - ax[i/c,i%c].set_ylabel(solver_names[i]); - ax[i/c,i%c].set_yscale(yscale); - if(SAVE_FIGURES): + ax[i / c, i % c].plot( + quantity[:, i], + "kx-", + quantityPerSolver[:, i], + line_styles[i], + alpha=LINE_ALPHA, + ) + ax[i / c, i % c].set_ylabel(solver_names[i]) + ax[i / c, i % c].set_yscale(yscale) + if SAVE_FIGURES: for ext in FILE_EXTENSIONS: - f.savefig(FIGURE_PATH+(legend[0]+'_VS_'+legend[1]).replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); - if(SHOW_LEGENDS): - leg = ax[0,0].legend(legend, loc='best'); + f.savefig( + FIGURE_PATH + + (legend[0] + "_VS_" + legend[1]).replace(" ", "_") + + "." + + ext, + format=ext, + dpi=FIGURES_DPI, + bbox_inches="tight", + ) + if SHOW_LEGENDS: + leg = ax[0, 0].legend(legend, loc="best") leg.get_frame().set_alpha(LEGEND_ALPHA) + def grayify_cmap(cmap): """Return a grayscale version of the colormap""" cmap = plt.cm.get_cmap(cmap) colors = cmap(np.arange(cmap.N)) - + # convert RGBA to perceived greyscale luminance # cf. http://alienryderflex.com/hsp.html RGB_weight = [0.299, 0.587, 0.114] luminance = np.sqrt(np.dot(colors[:, :3] ** 2, RGB_weight)) colors[:, :3] = luminance[:, np.newaxis] - + return cmap.from_list(cmap.name + "_grayscale", colors, cmap.N) - + + def saveFigure(title): - if(SAVE_FIGURES): + if SAVE_FIGURES: for ext in FILE_EXTENSIONS: - plt.gcf().savefig(FIGURE_PATH+title.replace(' ', '_')+'.'+ext, format=ext, dpi=FIGURES_DPI, bbox_inches='tight'); \ No newline at end of file + plt.gcf().savefig( + FIGURE_PATH + title.replace(" ", "_") + "." + ext, + format=ext, + dpi=FIGURES_DPI, + bbox_inches="tight", + ) diff --git a/sl1m/tools/polytope_conversion_utils.py b/sl1m/tools/polytope_conversion_utils.py deleted file mode 100644 index 24dab4f..0000000 --- a/sl1m/tools/polytope_conversion_utils.py +++ /dev/null @@ -1,195 +0,0 @@ -from cdd import Matrix, Polyhedron, RepType -from numpy import array, hstack, zeros, ones -import numpy as np - -NUMBER_TYPE = 'float' # 'float' or 'fraction' - -# CROSSMATRIX Compute the projection matrix of the cross product -def crossMatrix( v ): - VP = np.array( [[ 0, -v[2], v[1] ], - [ v[2], 0, -v[0] ], - [-v[1], v[0], 0 ]] ); - return VP; - -class ConeException(Exception): - - def __init__(self, M): - self.M = M - - -class NotConeFace(ConeException): - - def __str__(self): - return "Matrix is not a cone face" - - -class NotConeSpan(ConeException): - - def __str__(self): - return "Matrix is not a cone span" - -class NotPolyFace(ConeException): - - def __str__(self): - return "Matrix is not a polytope face" - - -def cone_span_to_face(S, eliminate_redundancies=False): - """ - - Returns the face matrix S^F of the span matrix S, - that is, a matrix such that - - {x = S z, z >= 0} if and only if {S^F x <= 0}. - - """ - V = hstack([ones((S.shape[1], 1)), S.T]) - # V-representation: first column is 0 for rays - V_cdd = Matrix(V, number_type=NUMBER_TYPE) - V_cdd.rep_type = RepType.GENERATOR - P = Polyhedron(V_cdd) - H_matrix = P.get_inequalities(); - if(eliminate_redundancies): - H_matrix.canonicalize(); - H = array(H_matrix); - if(H.shape[1]>1): - b = H[:, 0] - A = H[:, 1:] - else: - b, A = H[:, 0], zeros((H.shape[0],S.shape[0])); - #~ for i in xrange(H.shape[0]): - #~ if b[i] != 0: - #~ raise NotConeSpan(S) - return -A, b - -def poly_span_to_face(S): - """ - - Returns the face matrix S^F of the span matrix S, - that is, a matrix such that - - {x = S z, z >= 0, sum(z)=1} if and only if {S^F x <= s}. - - """ - V = hstack([ones((S.shape[1], 1)), S.T]) - # V-representation: first column is 0 for rays, 1 for vertices - V_cdd = Matrix(V, number_type=NUMBER_TYPE) - V_cdd.rep_type = RepType.GENERATOR - P = Polyhedron(V_cdd) - H = array(P.get_inequalities()) # H-representation: A x + b >= 0 - b, A = H[:, 0], H[:, 1:] - return (-A,b) - -def arbitrary_span_to_face(S, rv): - """ - - Returns the face matrix S^F of the span matrix S, - that is, a matrix such that - - {x = S z, z >= 0} if and only if {S^F x <= f}. - - The vector rv specifies whether the corresponding column in S - is a vertex (1) or a ray (0). - """ - V = hstack([rv.reshape((S.shape[1], 1)), S.T]) - # V-representation: first column is 0 for rays - V_cdd = Matrix(V, number_type=NUMBER_TYPE) - V_cdd.rep_type = RepType.GENERATOR - P = Polyhedron(V_cdd) - H = array(P.get_inequalities()) - b, A = H[:, 0], H[:, 1:] - return (-A,b) - -def cone_face_to_span(F): - """ - - Compute the span matrix F^S of the face matrix F, - that is, a matrix such that - - {F x <= 0} if and only if {x = F^S z, z >= 0}. - - """ - b, A = zeros((F.shape[0], 1)), -F - # H-representation: A x + b >= 0 - F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE) - F_cdd.rep_type = RepType.INEQUALITY - P = Polyhedron(F_cdd) - V = array(P.get_generators()) - for i in xrange(V.shape[0]): - if V[i, 0] != 0: # 1 = vertex, 0 = ray - raise NotConeFace(F) - return V[:, 1:].T - - -def poly_face_to_span(F,f): - """ - - Compute the span matrix F^S of the face matrix F, - that is, a matrix such that - - {F x <= f} if and only if {x = F^S z, z >= 0, sum(z)=1}. - - """ - b, A = f.reshape((F.shape[0],1)), -F - # H-representation: A x + b >= 0 - F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE) - F_cdd.rep_type = RepType.INEQUALITY - P = Polyhedron(F_cdd) - V = array(P.get_generators()) - for i in xrange(V.shape[0]): - if V[i, 0] != 1: # 1 = vertex, 0 = ray - raise NotPolyFace(F) - return V[:, 1:].T - -def arbitrary_face_to_span(F,f): - """ - - Compute the span matrix F^S of the face matrix F, - that is, a matrix such that - - {F x <= f} if and only if {x = F^S z, z >= 0, sum(z)=1}. - - Works for both polytopes and cones. - - """ - b, A = f.reshape((F.shape[0],1)), -F - # H-representation: A x + b >= 0 - F_cdd = Matrix(hstack([b, A]), number_type=NUMBER_TYPE) - F_cdd.rep_type = RepType.INEQUALITY - P = Polyhedron(F_cdd) - V = array(P.get_generators()) - return (V[:, 1:].T, V[:, 0]); - -def eliminate_redundant_inequalities(A,b): - ''' Input format is A x + b <= 0''' - # H-representation: A x + b >= 0 - A_plot = Matrix(hstack([-b.reshape((A.shape[0],1)), -A]), number_type=NUMBER_TYPE) - A_plot.rep_type = RepType.INEQUALITY - A_plot.canonicalize() - A_plot = np.array(A_plot) - b_plot, A_plot = -A_plot[:, 0], -A_plot[:, 1:] - return (A_plot, b_plot); - -if __name__ == "__main__": - print("Gonna test polytope conversion utils") - - print(" *** Test cone span to face ***") - S = np.array([[1,-1, 1], - [1, 1, 2]]); - F = cone_span_to_face(S); - print("Span") - print(S) - print("Face") - print(F) - - print("\n *** Test polytope span to face ***") - S = np.array([[1, 1,-1, -1, 0.8,-0.3], - [1,-1, 1, -1,-0.9, 0.1]]); - (F,f) = poly_span_to_face(S); - print("Span") - print(S) - print("Face") - print(F) - print(f) - - diff --git a/sl1m/tools/transformations.py b/sl1m/tools/transformations.py index dfc6fca..2462fd5 100644 --- a/sl1m/tools/transformations.py +++ b/sl1m/tools/transformations.py @@ -1,5 +1,3 @@ - - # -*- coding: utf-8 -*- # transformations.py @@ -201,8 +199,8 @@ import numpy -__version__ = '2015.07.18' -__docformat__ = 'restructuredtext en' +__version__ = "2015.07.18" +__docformat__ = "restructuredtext en" __all__ = () @@ -333,9 +331,13 @@ def rotation_matrix(angle, direction, point=None): R = numpy.diag([cosa, cosa, cosa]) R += numpy.outer(direction, direction) * (1.0 - cosa) direction *= sina - R += numpy.array([[ 0.0, -direction[2], direction[1]], - [ direction[2], 0.0, -direction[0]], - [-direction[1], direction[0], 0.0]]) + R += numpy.array( + [ + [0.0, -direction[2], direction[1]], + [direction[2], 0.0, -direction[0]], + [-direction[1], direction[0], 0.0], + ] + ) M = numpy.identity(4) M[:3, :3] = R if point is not None: @@ -376,11 +378,11 @@ def rotation_from_matrix(matrix): # rotation angle depending on direction cosa = (numpy.trace(R33) - 1.0) / 2.0 if abs(direction[2]) > 1e-8: - sina = (R[1, 0] + (cosa-1.0)*direction[0]*direction[1]) / direction[2] + sina = (R[1, 0] + (cosa - 1.0) * direction[0] * direction[1]) / direction[2] elif abs(direction[1]) > 1e-8: - sina = (R[0, 2] + (cosa-1.0)*direction[0]*direction[2]) / direction[1] + sina = (R[0, 2] + (cosa - 1.0) * direction[0] * direction[2]) / direction[1] else: - sina = (R[2, 1] + (cosa-1.0)*direction[1]*direction[2]) / direction[0] + sina = (R[2, 1] + (cosa - 1.0) * direction[1] * direction[2]) / direction[0] angle = math.atan2(sina, cosa) return angle, direction, point @@ -460,8 +462,7 @@ def scale_from_matrix(matrix): return factor, origin, direction -def projection_matrix(point, normal, direction=None, - perspective=None, pseudo=False): +def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False): """Return matrix to project onto plane defined by point and normal. Using either perspective point, projection direction, or none of both. @@ -497,14 +498,13 @@ def projection_matrix(point, normal, direction=None, normal = unit_vector(normal[:3]) if perspective is not None: # perspective projection - perspective = numpy.array(perspective[:3], dtype=numpy.float64, - copy=False) - M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective-point, normal) + perspective = numpy.array(perspective[:3], dtype=numpy.float64, copy=False) + M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective - point, normal) M[:3, :3] -= numpy.outer(perspective, normal) if pseudo: # preserve relative depth M[:3, :3] -= numpy.outer(normal, normal) - M[:3, 3] = numpy.dot(point, normal) * (perspective+normal) + M[:3, 3] = numpy.dot(point, normal) * (perspective + normal) else: M[:3, 3] = numpy.dot(point, normal) * perspective M[3, :3] = -normal @@ -584,11 +584,10 @@ def projection_from_matrix(matrix, pseudo=False): # perspective projection i = numpy.where(abs(numpy.real(w)) > 1e-8)[0] if not len(i): - raise ValueError( - "no eigenvector not corresponding to eigenvalue 0") + raise ValueError("no eigenvector not corresponding to eigenvalue 0") point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] - normal = - M[3, :3] + normal = -M[3, :3] perspective = M[:3, 3] / numpy.dot(point[:3], normal) if pseudo: perspective -= normal @@ -635,15 +634,19 @@ def clip_matrix(left, right, bottom, top, near, far, perspective=False): if near <= _EPS: raise ValueError("invalid frustum: near <= 0") t = 2.0 * near - M = [[t/(left-right), 0.0, (right+left)/(right-left), 0.0], - [0.0, t/(bottom-top), (top+bottom)/(top-bottom), 0.0], - [0.0, 0.0, (far+near)/(near-far), t*far/(far-near)], - [0.0, 0.0, -1.0, 0.0]] + M = [ + [t / (left - right), 0.0, (right + left) / (right - left), 0.0], + [0.0, t / (bottom - top), (top + bottom) / (top - bottom), 0.0], + [0.0, 0.0, (far + near) / (near - far), t * far / (far - near)], + [0.0, 0.0, -1.0, 0.0], + ] else: - M = [[2.0/(right-left), 0.0, 0.0, (right+left)/(left-right)], - [0.0, 2.0/(top-bottom), 0.0, (top+bottom)/(bottom-top)], - [0.0, 0.0, 2.0/(far-near), (far+near)/(near-far)], - [0.0, 0.0, 0.0, 1.0]] + M = [ + [2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)], + [0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)], + [0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)], + [0.0, 0.0, 0.0, 1.0], + ] return numpy.array(M) @@ -763,7 +766,7 @@ def decompose_matrix(matrix): if not numpy.linalg.det(P): raise ValueError("matrix is singular") - scale = numpy.zeros((3, )) + scale = numpy.zeros((3,)) shear = [0.0, 0.0, 0.0] angles = [0.0, 0.0, 0.0] @@ -801,15 +804,16 @@ def decompose_matrix(matrix): angles[0] = math.atan2(row[1, 2], row[2, 2]) angles[2] = math.atan2(row[0, 1], row[0, 0]) else: - #angles[0] = math.atan2(row[1, 0], row[1, 1]) + # angles[0] = math.atan2(row[1, 0], row[1, 1]) angles[0] = math.atan2(-row[2, 1], row[1, 1]) angles[2] = 0.0 return scale, shear, angles, translate, perspective -def compose_matrix(scale=None, shear=None, angles=None, translate=None, - perspective=None): +def compose_matrix( + scale=None, shear=None, angles=None, translate=None, perspective=None +): """Return transformation matrix from sequence of transformations. This is the inverse of the decompose_matrix function. @@ -843,7 +847,7 @@ def compose_matrix(scale=None, shear=None, angles=None, translate=None, T[:3, 3] = translate[:3] M = numpy.dot(M, T) if angles is not None: - R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + R = euler_matrix(angles[0], angles[1], angles[2], "sxyz") M = numpy.dot(M, R) if shear is not None: Z = numpy.identity(4) @@ -881,11 +885,14 @@ def orthogonalization_matrix(lengths, angles): sina, sinb, _ = numpy.sin(angles) cosa, cosb, cosg = numpy.cos(angles) co = (cosa * cosb - cosg) / (sina * sinb) - return numpy.array([ - [ a*sinb*math.sqrt(1.0-co*co), 0.0, 0.0, 0.0], - [-a*sinb*co, b*sina, 0.0, 0.0], - [ a*cosb, b*cosa, c, 0.0], - [ 0.0, 0.0, 0.0, 1.0]]) + return numpy.array( + [ + [a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0], + [-a * sinb * co, b * sina, 0.0, 0.0], + [a * cosb, b * cosa, c, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): @@ -938,11 +945,11 @@ def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): # move centroids to origin t0 = -numpy.mean(v0, axis=1) - M0 = numpy.identity(ndims+1) + M0 = numpy.identity(ndims + 1) M0[:ndims, ndims] = t0 v0 += t0.reshape(ndims, 1) t1 = -numpy.mean(v1, axis=1) - M1 = numpy.identity(ndims+1) + M1 = numpy.identity(ndims + 1) M1[:ndims, ndims] = t1 v1 += t1.reshape(ndims, 1) @@ -952,10 +959,10 @@ def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): u, s, vh = numpy.linalg.svd(A.T) vh = vh[:ndims].T B = vh[:ndims] - C = vh[ndims:2*ndims] + C = vh[ndims : 2 * ndims] t = numpy.dot(C, numpy.linalg.pinv(B)) t = numpy.concatenate((t, numpy.zeros((ndims, 1))), axis=1) - M = numpy.vstack((t, ((0.0,)*ndims) + (1.0,))) + M = numpy.vstack((t, ((0.0,) * ndims) + (1.0,))) elif usesvd or ndims != 3: # Rigid transformation via SVD of covariance matrix u, s, vh = numpy.linalg.svd(numpy.dot(v1, v0.T)) @@ -963,10 +970,10 @@ def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): R = numpy.dot(u, vh) if numpy.linalg.det(R) < 0.0: # R does not constitute right handed system - R -= numpy.outer(u[:, ndims-1], vh[ndims-1, :]*2.0) + R -= numpy.outer(u[:, ndims - 1], vh[ndims - 1, :] * 2.0) s[-1] *= -1.0 # homogeneous transformation matrix - M = numpy.identity(ndims+1) + M = numpy.identity(ndims + 1) M[:ndims, :ndims] = R else: # Rigid transformation matrix via quaternion @@ -974,10 +981,12 @@ def affine_matrix_from_points(v0, v1, shear=True, scale=True, usesvd=True): xx, yy, zz = numpy.sum(v0 * v1, axis=1) xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) - N = [[xx+yy+zz, 0.0, 0.0, 0.0], - [yz-zy, xx-yy-zz, 0.0, 0.0], - [zx-xz, xy+yx, yy-xx-zz, 0.0], - [xy-yx, zx+xz, yz+zy, zz-xx-yy]] + N = [ + [xx + yy + zz, 0.0, 0.0, 0.0], + [yz - zy, xx - yy - zz, 0.0, 0.0], + [zx - xz, xy + yx, yy - xx - zz, 0.0], + [xy - yx, zx + xz, yz + zy, zz - xx - yy], + ] # quaternion: eigenvector corresponding to most positive eigenvalue w, V = numpy.linalg.eigh(N) q = V[:, numpy.argmax(w)] @@ -1044,11 +1053,10 @@ def superimposition_matrix(v0, v1, scale=False, usesvd=True): """ v0 = numpy.array(v0, dtype=numpy.float64, copy=False)[:3] v1 = numpy.array(v1, dtype=numpy.float64, copy=False)[:3] - return affine_matrix_from_points(v0, v1, shear=False, - scale=scale, usesvd=usesvd) + return affine_matrix_from_points(v0, v1, shear=False, scale=scale, usesvd=usesvd) -def euler_matrix(ai, aj, ak, axes='sxyz'): +def euler_matrix(ai, aj, ak, axes="sxyz"): """Return homogeneous rotation matrix from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles @@ -1074,8 +1082,8 @@ def euler_matrix(ai, aj, ak, axes='sxyz'): firstaxis, parity, repetition, frame = axes i = firstaxis - j = _NEXT_AXIS[i+parity] - k = _NEXT_AXIS[i-parity+1] + j = _NEXT_AXIS[i + parity] + k = _NEXT_AXIS[i - parity + 1] if frame: ai, ak = ak, ai @@ -1084,34 +1092,34 @@ def euler_matrix(ai, aj, ak, axes='sxyz'): si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak) ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak) - cc, cs = ci*ck, ci*sk - sc, ss = si*ck, si*sk + cc, cs = ci * ck, ci * sk + sc, ss = si * ck, si * sk M = numpy.identity(4) if repetition: M[i, i] = cj - M[i, j] = sj*si - M[i, k] = sj*ci - M[j, i] = sj*sk - M[j, j] = -cj*ss+cc - M[j, k] = -cj*cs-sc - M[k, i] = -sj*ck - M[k, j] = cj*sc+cs - M[k, k] = cj*cc-ss + M[i, j] = sj * si + M[i, k] = sj * ci + M[j, i] = sj * sk + M[j, j] = -cj * ss + cc + M[j, k] = -cj * cs - sc + M[k, i] = -sj * ck + M[k, j] = cj * sc + cs + M[k, k] = cj * cc - ss else: - M[i, i] = cj*ck - M[i, j] = sj*sc-cs - M[i, k] = sj*cc+ss - M[j, i] = cj*sk - M[j, j] = sj*ss+cc - M[j, k] = sj*cs-sc + M[i, i] = cj * ck + M[i, j] = sj * sc - cs + M[i, k] = sj * cc + ss + M[j, i] = cj * sk + M[j, j] = sj * ss + cc + M[j, k] = sj * cs - sc M[k, i] = -sj - M[k, j] = cj*si - M[k, k] = cj*ci + M[k, j] = cj * si + M[k, k] = cj * ci return M -def euler_from_matrix(matrix, axes='sxyz'): +def euler_from_matrix(matrix, axes="sxyz"): """Return Euler angles from rotation matrix for specified axis sequence. axes : One of 24 axis sequences as string or encoded tuple @@ -1137,29 +1145,29 @@ def euler_from_matrix(matrix, axes='sxyz'): firstaxis, parity, repetition, frame = axes i = firstaxis - j = _NEXT_AXIS[i+parity] - k = _NEXT_AXIS[i-parity+1] + j = _NEXT_AXIS[i + parity] + k = _NEXT_AXIS[i - parity + 1] M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] if repetition: - sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) + sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k]) if sy > _EPS: - ax = math.atan2( M[i, j], M[i, k]) - ay = math.atan2( sy, M[i, i]) - az = math.atan2( M[j, i], -M[k, i]) + ax = math.atan2(M[i, j], M[i, k]) + ay = math.atan2(sy, M[i, i]) + az = math.atan2(M[j, i], -M[k, i]) else: - ax = math.atan2(-M[j, k], M[j, j]) - ay = math.atan2( sy, M[i, i]) + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(sy, M[i, i]) az = 0.0 else: - cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) + cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i]) if cy > _EPS: - ax = math.atan2( M[k, j], M[k, k]) - ay = math.atan2(-M[k, i], cy) - az = math.atan2( M[j, i], M[i, i]) + ax = math.atan2(M[k, j], M[k, k]) + ay = math.atan2(-M[k, i], cy) + az = math.atan2(M[j, i], M[i, i]) else: - ax = math.atan2(-M[j, k], M[j, j]) - ay = math.atan2(-M[k, i], cy) + ax = math.atan2(-M[j, k], M[j, j]) + ay = math.atan2(-M[k, i], cy) az = 0.0 if parity: @@ -1169,7 +1177,7 @@ def euler_from_matrix(matrix, axes='sxyz'): return ax, ay, az -def euler_from_quaternion(quaternion, axes='sxyz'): +def euler_from_quaternion(quaternion, axes="sxyz"): """Return Euler angles from quaternion for specified axis sequence. >>> angles = euler_from_quaternion([0.99810947, 0.06146124, 0, 0]) @@ -1180,7 +1188,7 @@ def euler_from_quaternion(quaternion, axes='sxyz'): return euler_from_matrix(quaternion_matrix(quaternion), axes) -def quaternion_from_euler(ai, aj, ak, axes='sxyz'): +def quaternion_from_euler(ai, aj, ak, axes="sxyz"): """Return quaternion from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles @@ -1198,8 +1206,8 @@ def quaternion_from_euler(ai, aj, ak, axes='sxyz'): firstaxis, parity, repetition, frame = axes i = firstaxis + 1 - j = _NEXT_AXIS[i+parity-1] + 1 - k = _NEXT_AXIS[i-parity] + 1 + j = _NEXT_AXIS[i + parity - 1] + 1 + k = _NEXT_AXIS[i - parity] + 1 if frame: ai, ak = ak, ai @@ -1215,22 +1223,22 @@ def quaternion_from_euler(ai, aj, ak, axes='sxyz'): sj = math.sin(aj) ck = math.cos(ak) sk = math.sin(ak) - cc = ci*ck - cs = ci*sk - sc = si*ck - ss = si*sk + cc = ci * ck + cs = ci * sk + sc = si * ck + ss = si * sk - q = numpy.empty((4, )) + q = numpy.empty((4,)) if repetition: - q[0] = cj*(cc - ss) - q[i] = cj*(cs + sc) - q[j] = sj*(cc + ss) - q[k] = sj*(cs - sc) + q[0] = cj * (cc - ss) + q[i] = cj * (cs + sc) + q[j] = sj * (cc + ss) + q[k] = sj * (cs - sc) else: - q[0] = cj*cc + sj*ss - q[i] = cj*sc - sj*cs - q[j] = cj*ss + sj*cc - q[k] = cj*cs - sj*sc + q[0] = cj * cc + sj * ss + q[i] = cj * sc - sj * cs + q[j] = cj * ss + sj * cc + q[k] = cj * cs - sj * sc if parity: q[j] *= -1.0 @@ -1248,8 +1256,8 @@ def quaternion_about_axis(angle, axis): q = numpy.array([0.0, axis[0], axis[1], axis[2]]) qlen = vector_norm(q) if qlen > _EPS: - q *= math.sin(angle/2.0) / qlen - q[0] = math.cos(angle/2.0) + q *= math.sin(angle / 2.0) / qlen + q[0] = math.cos(angle / 2.0) return q @@ -1273,11 +1281,29 @@ def quaternion_matrix(quaternion): return numpy.identity(4) q *= math.sqrt(2.0 / n) q = numpy.outer(q, q) - return numpy.array([ - [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], - [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], - [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], - [ 0.0, 0.0, 0.0, 1.0]]) + return numpy.array( + [ + [ + 1.0 - q[2, 2] - q[3, 3], + q[1, 2] - q[3, 0], + q[1, 3] + q[2, 0], + 0.0, + ], + [ + q[1, 2] + q[3, 0], + 1.0 - q[1, 1] - q[3, 3], + q[2, 3] - q[1, 0], + 0.0, + ], + [ + q[1, 3] - q[2, 0], + q[2, 3] + q[1, 0], + 1.0 - q[1, 1] - q[2, 2], + 0.0, + ], + [0.0, 0.0, 0.0, 1.0], + ] + ) def quaternion_from_matrix(matrix, isprecise=False): @@ -1318,7 +1344,7 @@ def quaternion_from_matrix(matrix, isprecise=False): """ M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] if isprecise: - q = numpy.empty((4, )) + q = numpy.empty((4,)) t = numpy.trace(M) if t > M[3, 3]: q[0] = t @@ -1348,10 +1374,14 @@ def quaternion_from_matrix(matrix, isprecise=False): m21 = M[2, 1] m22 = M[2, 2] # symmetric matrix K - K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], - [m01+m10, m11-m00-m22, 0.0, 0.0], - [m02+m20, m12+m21, m22-m00-m11, 0.0], - [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) + K = numpy.array( + [ + [m00 - m11 - m22, 0.0, 0.0, 0.0], + [m01 + m10, m11 - m00 - m22, 0.0, 0.0], + [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0], + [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22], + ] + ) K /= 3.0 # quaternion is eigenvector of K that corresponds to largest eigenvalue w, V = numpy.linalg.eigh(K) @@ -1371,10 +1401,15 @@ def quaternion_multiply(quaternion1, quaternion0): """ w0, x0, y0, z0 = quaternion0 w1, x1, y1, z1 = quaternion1 - return numpy.array([-x1*x0 - y1*y0 - z1*z0 + w1*w0, - x1*w0 + y1*z0 - z1*y0 + w1*x0, - -x1*z0 + y1*w0 + z1*x0 + w1*y0, - x1*y0 - y1*x0 + z1*w0 + w1*z0], dtype=numpy.float64) + return numpy.array( + [ + -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, + x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, + -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, + x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, + ], + dtype=numpy.float64, + ) def quaternion_conjugate(quaternion): @@ -1490,8 +1525,14 @@ def random_quaternion(rand=None): pi2 = math.pi * 2.0 t1 = pi2 * rand[1] t2 = pi2 * rand[2] - return numpy.array([numpy.cos(t2)*r2, numpy.sin(t1)*r1, - numpy.cos(t1)*r1, numpy.sin(t2)*r2]) + return numpy.array( + [ + numpy.cos(t2) * r2, + numpy.sin(t1) * r1, + numpy.cos(t1) * r1, + numpy.sin(t2) * r2, + ] + ) def random_rotation_matrix(rand=None): @@ -1532,6 +1573,7 @@ class Arcball(object): >>> ball.next() """ + def __init__(self, initial=None): """Initialize virtual trackball control. @@ -1550,7 +1592,7 @@ def __init__(self, initial=None): initial = numpy.array(initial, dtype=numpy.float64) if initial.shape == (4, 4): self._qdown = quaternion_from_matrix(initial) - elif initial.shape == (4, ): + elif initial.shape == (4,): initial /= vector_norm(initial) self._qdown = initial else: @@ -1612,7 +1654,7 @@ def drag(self, point): def next(self, acceleration=0.0): """Continue rotation in direction of last drag.""" - q = quaternion_slerp(self._qpre, self._qnow, 2.0+acceleration, False) + q = quaternion_slerp(self._qpre, self._qnow, 2.0 + acceleration, False) self._qpre, self._qnow = self._qnow, q def matrix(self): @@ -1624,11 +1666,11 @@ def arcball_map_to_sphere(point, center, radius): """Return unit sphere coordinates from window coordinates.""" v0 = (point[0] - center[0]) / radius v1 = (center[1] - point[1]) / radius - n = v0*v0 + v1*v1 + n = v0 * v0 + v1 * v1 if n > 1.0: # position outside of sphere n = math.sqrt(n) - return numpy.array([v0/n, v1/n, 0.0]) + return numpy.array([v0 / n, v1 / n, 0.0]) else: return numpy.array([v0, v1, math.sqrt(1.0 - n)]) @@ -1670,14 +1712,31 @@ def arcball_nearest_axis(point, axes): # map axes strings to/from tuples of inner axis, parity, repetition, frame _AXES2TUPLE = { - 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), - 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), - 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), - 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), - 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), - 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), - 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), - 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + "sxyz": (0, 0, 0, 0), + "sxyx": (0, 0, 1, 0), + "sxzy": (0, 1, 0, 0), + "sxzx": (0, 1, 1, 0), + "syzx": (1, 0, 0, 0), + "syzy": (1, 0, 1, 0), + "syxz": (1, 1, 0, 0), + "syxy": (1, 1, 1, 0), + "szxy": (2, 0, 0, 0), + "szxz": (2, 0, 1, 0), + "szyx": (2, 1, 0, 0), + "szyz": (2, 1, 1, 0), + "rzyx": (0, 0, 0, 1), + "rxyx": (0, 0, 1, 1), + "ryzx": (0, 1, 0, 1), + "rxzx": (0, 1, 1, 1), + "rxzy": (1, 0, 0, 1), + "ryzy": (1, 0, 1, 1), + "rzxy": (1, 1, 0, 1), + "ryxy": (1, 1, 1, 1), + "ryxz": (2, 0, 0, 1), + "rzxz": (2, 0, 1, 1), + "rxyz": (2, 1, 0, 1), + "rzyz": (2, 1, 1, 1), +} _TUPLE2AXES = dict((v, k) for k, v in list(_AXES2TUPLE.items())) @@ -1756,7 +1815,7 @@ def unit_vector(data, axis=None, out=None): if out is not data: out[:] = numpy.array(data, copy=False) data = out - length = numpy.atleast_1d(numpy.sum(data*data, axis)) + length = numpy.atleast_1d(numpy.sum(data * data, axis)) numpy.sqrt(length, length) if axis is not None: length = numpy.expand_dims(length, axis) @@ -1880,7 +1939,7 @@ def is_same_transform(matrix0, matrix1): return numpy.allclose(matrix0, matrix1) -def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): +def _import_module(name, package=None, warn=True, prefix="_py_", ignore="_"): """Try import all public attributes from module into global namespace. Existing attributes with name clashes are renamed with prefix. @@ -1891,11 +1950,12 @@ def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): """ import warnings from importlib import import_module + try: if not package: module = import_module(name) else: - module = import_module('.' + name, package=package) + module = import_module("." + name, package=package) except ImportError: if warn: warnings.warn("failed to import module %s" % name) @@ -1912,11 +1972,11 @@ def _import_module(name, package=None, warn=True, prefix='_py_', ignore='_'): return True -_import_module('transformations') +_import_module("transformations") if __name__ == "__main__": import doctest import random # used in doctests + numpy.set_printoptions(suppress=True, precision=5) doctest.testmod() -