├── examples ├── .gitignore ├── iterative_projection │ ├── context.pyc │ ├── context.py │ ├── IP_test_three_contacts.py │ ├── one_contact.py │ ├── pymanoid_double_support.py │ ├── bretl_projection.py │ ├── pymanoid_ladder_climbing.py │ ├── sequentialIterativeProjection.py │ └── path_sequential_IP_example.py ├── context.py ├── figures_code │ └── context.py ├── jvrc1_double_support.json ├── kinematics │ ├── anymal_ik_example.py │ ├── ikpy_hyqreal_example.py │ └── ikpy_example.py ├── jvrc1_ladder_climbing.json ├── feasible_region │ ├── example_polytope_projection.py │ ├── four_generators_four_contacts.py │ ├── eight_generators_four_contacts.py │ └── actuation_region_example.py ├── static_equilibrium_check │ ├── single_LP_example.py │ └── check_stability_lp_example.py ├── foothold_planning │ ├── foothold_planning_example.py │ └── single_foothold_optimization_example.py └── force_polytopes │ ├── maximum_contact_force.py │ └── compute_force_polytopes.py ├── MANIFEST.in ├── figs ├── anymal.png ├── four_stance.png ├── 3contacts_F&A.png ├── 4contacts_F&A.png ├── 3contacts_onlyA.png ├── 4contacts_onlyA.png ├── feasible_region.png ├── force_polygons.png └── foothold_planning.png ├── requirements.txt ├── jet_leg ├── __pycache__ │ └── __init__.cpython-38.pyc ├── maths │ ├── __pycache__ │ │ ├── __init__.cpython-38.pyc │ │ └── computational_geometry.cpython-38.pyc │ ├── geometry.py │ ├── __init__.py │ ├── computational_geometry.py │ └── math_tools.py ├── plotting │ ├── __pycache__ │ │ ├── __init__.cpython-38.pyc │ │ └── plotting_tools.cpython-38.pyc │ ├── arrow3D.py │ ├── __init__.py │ └── plotting_tools.py ├── figures_code │ └── context.py ├── dynamics │ ├── rigid_body_dynamics.py │ └── __init__.py ├── robots │ ├── dog_interface.py │ ├── __init__.py │ ├── hyq │ │ ├── __init__.py │ │ └── urdf │ │ │ ├── leg │ │ │ ├── hyq_leg_RH.urdf │ │ │ ├── hyq_leg_LF.urdf │ │ │ ├── hyq_leg_LH.urdf │ │ │ └── hyq_leg_RF.urdf │ │ │ └── hyq.urdf │ ├── anymal │ │ ├── __init__.py │ │ └── anymal_kinematics.py │ └── hyqreal │ │ ├── __init__.py │ │ └── hyqreal_kinematics.py ├── optimization │ ├── bilinear_constraints.py │ ├── __init__.py │ └── directed_optimization.py ├── __init__.py ├── constraints │ └── __init__.py └── kinematics │ ├── __init__.py │ └── kinematics_interface.py ├── unit_tests ├── context.py ├── unit_test_main.py ├── test_cvxopt.py ├── test_pypoman.py ├── test_directed_optimization_variable_constraints.py ├── test_pyipopt.py ├── test_bilinear_constraints.py ├── actuation_region_main.py ├── test_intersection.py └── test_hyq_kinematics.py ├── Dockerfile ├── run.sh ├── setup.py ├── README.md └── LICENSE /examples/.gitignore: -------------------------------------------------------------------------------- 1 | HRP4*.dae 2 | hrp4_description.py 3 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include jet_leg/robots/anymal/urdf/anymal_boxy.urdf 2 | -------------------------------------------------------------------------------- /figs/anymal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/anymal.png -------------------------------------------------------------------------------- /figs/four_stance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/four_stance.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pytest 2 | numpy 3 | scipy 4 | matplotlib 5 | pycddlib 6 | pypoman 7 | pin -------------------------------------------------------------------------------- /figs/3contacts_F&A.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/3contacts_F&A.png -------------------------------------------------------------------------------- /figs/4contacts_F&A.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/4contacts_F&A.png -------------------------------------------------------------------------------- /figs/3contacts_onlyA.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/3contacts_onlyA.png -------------------------------------------------------------------------------- /figs/4contacts_onlyA.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/4contacts_onlyA.png -------------------------------------------------------------------------------- /figs/feasible_region.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/feasible_region.png -------------------------------------------------------------------------------- /figs/force_polygons.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/force_polygons.png -------------------------------------------------------------------------------- /figs/foothold_planning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/figs/foothold_planning.png -------------------------------------------------------------------------------- /examples/iterative_projection/context.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/examples/iterative_projection/context.pyc -------------------------------------------------------------------------------- /jet_leg/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/jet_leg/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /jet_leg/maths/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/jet_leg/maths/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /jet_leg/plotting/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/jet_leg/plotting/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /jet_leg/plotting/__pycache__/plotting_tools.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/jet_leg/plotting/__pycache__/plotting_tools.cpython-38.pyc -------------------------------------------------------------------------------- /jet_leg/maths/__pycache__/computational_geometry.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orsoromeo/jet-leg/HEAD/jet_leg/maths/__pycache__/computational_geometry.cpython-38.pyc -------------------------------------------------------------------------------- /examples/context.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 16:33:23 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import os 9 | import sys 10 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) 11 | 12 | import jet_leg -------------------------------------------------------------------------------- /unit_tests/context.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jul 3 19:52:23 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import os 9 | import sys 10 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) 11 | 12 | import jet_leg -------------------------------------------------------------------------------- /examples/figures_code/context.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 16:33:23 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import os 9 | import sys 10 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '../..'))) 11 | 12 | import jet_leg -------------------------------------------------------------------------------- /jet_leg/figures_code/context.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 16:33:23 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import os 9 | import sys 10 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '../..'))) 11 | 12 | import jet_leg -------------------------------------------------------------------------------- /examples/iterative_projection/context.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 16:33:23 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import os 9 | import sys 10 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) 11 | 12 | import jet_leg -------------------------------------------------------------------------------- /unit_tests/unit_test_main.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jul 3 20:20:38 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import unittest 9 | loader = unittest.TestLoader() 10 | tests = loader.discover('.') 11 | testRunner = unittest.runner.TextTestRunner() 12 | testRunner.run(tests) 13 | 14 | 15 | -------------------------------------------------------------------------------- /jet_leg/dynamics/rigid_body_dynamics.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Dec 17 11:39:13 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | class RigidBodyDynamics: 11 | def __init__(self): 12 | self.LX = 0 13 | self.LY = 1 14 | self.LZ = 2 15 | self.AX = 3 16 | self.AY = 4 17 | self.AZ = 5 -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # syntax=docker/dockerfile:1 2 | 3 | FROM python:3.8-slim-buster 4 | 5 | WORKDIR /app 6 | 7 | RUN apt-get update && apt-get install -y cython libglpk-dev python3-tk 8 | 9 | COPY requirements.txt requirements.txt 10 | COPY setup.py setup.py 11 | 12 | RUN pip3 install -r requirements.txt 13 | 14 | COPY /examples examples 15 | COPY /jet_leg jet_leg 16 | COPY /resources resources 17 | COPY /unit_tests unit_tests 18 | 19 | CMD [ "python3", "-m" , "flask", "run", "--host=0.0.0.0"] 20 | -------------------------------------------------------------------------------- /examples/jvrc1_double_support.json: -------------------------------------------------------------------------------- 1 | { 2 | "com": { 3 | "pos": [0.0, 0.0, 0.8], 4 | "mass": 62.0 5 | }, 6 | "left_foot": { 7 | "shape": [0.12, 0.065], 8 | "friction": 0.5, 9 | "pos": [0.20, 0.15, 0.1], 10 | "robot_link": 20, 11 | "rpy": [0.4, 0, 0] 12 | }, 13 | "right_foot": { 14 | "shape": [0.12, 0.065], 15 | "friction": 0.5, 16 | "pos": [-0.2, -0.195, 0.0], 17 | "robot_link": 13, 18 | "rpy": [-0.4, 0.0, 0.0] 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | 2 | DOCKER_VOLUMES=" 3 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 4 | --volume="${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority" \ 5 | --volume="./examples/:/app/examples/" \ 6 | --volume="./jet_leg/:/app/jet_leg/" \ 7 | --volume="./resources/:/app/resources/" \ 8 | --volume="./unit_tests/:/app/unit_tests/" \ 9 | " 10 | 11 | DOCKER_ENV_VARS=" 12 | --env="DISPLAY" \ 13 | --env="QT_X11_NO_MITSHM=1" \ 14 | --env="PYTHONPATH="/app/ \ 15 | " 16 | 17 | DOCKER_ARGS=${DOCKER_VOLUMES}" "${DOCKER_ENV_VARS} 18 | 19 | docker run -i -t ${DOCKER_ARGS} jet-leg:latest /bin/bash 20 | -------------------------------------------------------------------------------- /examples/kinematics/anymal_ik_example.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from jet_leg.kinematics.kinematics_interface import KinematicsInterface 3 | 4 | kin = KinematicsInterface('anymal') 5 | 6 | LF_foot = np.array([0.3735, 0.33, -0.5]) 7 | RF_foot = np.array([0.3735, -0.33, -0.5]) 8 | LH_foot = np.array([-0.3735, 0.33, -0.5]) 9 | RH_foot = np.array([-0.3735, -0.33, -0.5]) 10 | starting_contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 11 | foot_vel = np.array([0.0, 0.0, 0.0, 0.0]) 12 | 13 | q = kin.inverse_kin(starting_contacts, foot_vel) 14 | print("Pinocchio's IK for Anymal: ", q) 15 | 16 | -------------------------------------------------------------------------------- /jet_leg/robots/dog_interface.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Dec 17 11:30:12 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | class DogInterface: 11 | def __init__(self): 12 | self.LF = 0 13 | self.RF = 1 14 | self.LH = 2 15 | self.RH = 3 16 | 17 | self.HAA = 0 18 | self.HFE = 1 19 | self.KFE = 2 20 | 21 | self.legJoints = [self.HAA, self.HFE, self.KFE] 22 | 23 | self.legs = [self.LF, self.RF, self.LH, self.RH] 24 | 25 | temp = [0.0, 0.0, 0.0] 26 | self.footPos =[[temp],[temp],[temp],[temp]] -------------------------------------------------------------------------------- /jet_leg/plotting/arrow3D.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Feb 03 09:32:29 2025 4 | 5 | @author: rorsolino 6 | """ 7 | import numpy as np 8 | from matplotlib.patches import FancyArrowPatch 9 | from mpl_toolkits.mplot3d import proj3d 10 | 11 | class Arrow3D(FancyArrowPatch): 12 | def __init__(self, xs, ys, zs, *args, **kwargs): 13 | FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs) 14 | self._verts3d = xs, ys, zs 15 | 16 | def do_3d_projection(self, renderer=None): 17 | xs3d, ys3d, zs3d = self._verts3d 18 | xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, self.axes.M) 19 | self.set_positions((xs[0],ys[0]),(xs[1],ys[1])) 20 | 21 | return np.min(zs) -------------------------------------------------------------------------------- /examples/jvrc1_ladder_climbing.json: -------------------------------------------------------------------------------- 1 | { 2 | "com": { 3 | "pos": [-0.2, 0.0, 0.77], 4 | "mass": 38.0 5 | }, 6 | "left_foot": { 7 | "shape": [0.01, 0.065], 8 | "friction": 0.7, 9 | "pos": [0.2, 0.1, 0.0], 10 | "robot_link": 18, 11 | "rpy": [0.0, -0.2, 0] 12 | }, 13 | "right_foot": { 14 | "shape": [0.01, 0.065], 15 | "friction": 0.7, 16 | "pos": [0.2, -0.1, 0.0], 17 | "robot_link": 12, 18 | "rpy": [0.0, -0.2, 0.0] 19 | }, 20 | "left_hand": { 21 | "shape": [0.01, 0.05], 22 | "friction": 10.0, 23 | "pos": [0.2, 0.1, 1.2], 24 | "robot_link": 44, 25 | "rpy": [0.0, -0.0, 0.0] 26 | }, 27 | "right_hand": { 28 | "shape": [0.01, 0.05], 29 | "friction": 10.0, 30 | "pos": [0.2, -0.1, 1.2], 31 | "robot_link": 31, 32 | "rpy": [0.0, -0.0, 0.0] 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /examples/feasible_region/example_polytope_projection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jun 4 13:20:07 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import pypoman 9 | from numpy import array, eye, ones, vstack, zeros 10 | 11 | n = 10 # dimension of the original polytope 12 | p = 2 # dimension of the projected polytope 13 | 14 | # Original polytope: 15 | # - inequality constraints: \forall i, |x_i| <= 1 16 | # - equality constraint: sum_i x_i = 0 17 | A = vstack([+eye(n), -eye(n)]) 18 | b = ones(2 * n) 19 | C = ones(n).reshape((1, n)) 20 | d = array([0]) 21 | print A,b 22 | ineq = (A, b) # A * x <= b 23 | eq = (C, d) # C * x == d 24 | 25 | # Projection is proj(x) = [x_0 x_1] 26 | E = zeros((p, n)) 27 | E[0, 0] = 1. 28 | E[1, 1] = 1. 29 | f = zeros(p) 30 | proj = (E, f) # proj(x) = E * x + f 31 | 32 | vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl') 33 | 34 | if __name__ == "__main__": # plot projected polytope 35 | import pylab 36 | pylab.ion() 37 | pylab.figure() 38 | pypoman.plot_polygon(vertices) -------------------------------------------------------------------------------- /jet_leg/maths/geometry.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Oct 30 15:18:59 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | class Geometry: 11 | 12 | def clockwise_sort(self, polygon): 13 | 14 | vertices_number = np.size(polygon,0) 15 | angle = np.zeros((1,vertices_number)) 16 | centroidX = np.sum(polygon[:,0]) 17 | centroidX = centroidX/float(vertices_number) 18 | centroidY = np.sum(polygon[:,1]) 19 | centroidY = centroidY/float(vertices_number) 20 | 21 | for j in range(0,vertices_number): 22 | angle[0,j] = np.arctan2(polygon[j,0] - centroidX, polygon[j,1] - centroidY) 23 | 24 | index = np.array(np.argsort(angle)) 25 | sorted_vertices = np.zeros((vertices_number,2)) 26 | for j in range(0,vertices_number): 27 | sorted_vertices[j,:] = polygon[index[0,j],:] 28 | 29 | # adding an extra point to close the polytop (last point equal to the first) 30 | sorted_vertices = np.vstack([sorted_vertices, sorted_vertices[0,:]]) 31 | 32 | return sorted_vertices -------------------------------------------------------------------------------- /jet_leg/optimization/bilinear_constraints.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Aug 5 19:11:34 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | from mpl_toolkits.mplot3d import Axes3D 9 | import matplotlib.pyplot as plt 10 | from matplotlib import cm 11 | from matplotlib.ticker import LinearLocator, FormatStrFormatter 12 | import numpy as np 13 | import random as rnd 14 | 15 | 16 | class BilinearConstraints: 17 | def make_convex(self, value): 18 | resolution = 0.1 19 | # Make data. 20 | x = np.arange(-5.0, 5.0, resolution) 21 | f = np.arange(-5.0, 5.0, resolution) 22 | X, F = np.meshgrid(x, f) 23 | self.Tau = X*F 24 | 25 | p = X+F 26 | q = X-F 27 | p_hat = p*p 28 | q_hat = q*q 29 | Conv_plus = 0.25*np.power(p,2.0) 30 | Conv_minus = -0.25*np.power(q,2.0) 31 | 32 | Tau_approx = Conv_plus + Conv_minus 33 | 34 | sigma = 0.0 35 | p_hat_relaxation = value*value +2.0*value*(p - value) + sigma 36 | 37 | return X, F, p_hat, p_hat_relaxation, q_hat 38 | 39 | def get_torque(self): 40 | return self.Tau -------------------------------------------------------------------------------- /examples/kinematics/ikpy_hyqreal_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jul 2 06:10:18 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | from jet_leg.hyqreal_kinematics import HyQRealKinematics 9 | import numpy as np 10 | 11 | 12 | epsilon = 1e-03 13 | LF_foot = np.array([0.4571, 0.3612, -0.5074]) 14 | RF_foot = np.array([0.4571, -0.3612, -0.5074]) 15 | LH_foot = np.array([-0.429867, 0.3612, -0.5074]) 16 | RH_foot = np.array([-0.429867, -0.3612, -0.5074]) 17 | starting_contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 18 | hyqrealKin = HyQRealKinematics() 19 | foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]) 20 | 21 | q_leg_ikpy = hyqrealKin.leg_inverse_kin_ikpy(0, starting_contacts) 22 | 23 | #for legID in range(0,4): 24 | # q_leg_ikpy = hyqrealKin.leg_inverse_kin_ikpy(legID, starting_contacts) 25 | 26 | print "joint pos ", q_leg_ikpy 27 | 28 | q_leg_ikpy = hyqrealKin.leg_inverse_kin_ikpy(1, starting_contacts) 29 | print "joint pos ", q_leg_ikpy 30 | 31 | q_leg_ikpy = hyqrealKin.leg_inverse_kin_ikpy(2, starting_contacts) 32 | print "joint pos ", q_leg_ikpy 33 | 34 | q_leg_ikpy = hyqrealKin.leg_inverse_kin_ikpy(3, starting_contacts) 35 | print "joint pos ", q_leg_ikpy 36 | 37 | des_q = [-0.2, 0.75, -1.5] -------------------------------------------------------------------------------- /jet_leg/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/maths/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/robots/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/constraints/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/dynamics/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/kinematics/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/plotting/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/optimization/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/robots/anymal/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /jet_leg/robots/hyqreal/__init__.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of pypoman . 7 | # 8 | # pypoman is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # pypoman is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with pypoman. If not, see . 20 | 21 | import pypoman 22 | 23 | #__all__ = [ 24 | # 'compute_cone_face_matrix', 25 | # 'compute_polytope_halfspaces', 26 | # 'compute_polytope_vertices', 27 | # 'intersect_line_cylinder', 28 | # 'intersect_line_polygon', 29 | # 'intersect_polygons', 30 | # 'plot_polygon', 31 | # 'project_polytope', 32 | # 'project_polytope_bretl', 33 | #] 34 | -------------------------------------------------------------------------------- /unit_tests/test_cvxopt.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Aug 6 12:07:22 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | from cvxopt import matrix, solvers 9 | import unittest 10 | 11 | class TestCvxopt(unittest.TestCase): 12 | 13 | def testLinearProgram(self): 14 | A = matrix([ [-1.0, -1.0, 0.0, 1.0], [1.0, -1.0, -1.0, -2.0] ]) 15 | b = matrix([ 1.0, -2.0, 0.0, 4.0 ]) 16 | c = matrix([ 2.0, 1.0 ]) 17 | sol=solvers.lp(c,A,b) 18 | #print(sol['x']) 19 | self.assertTrue(sol) 20 | 21 | def testQuadraticProgram(self): 22 | Q = 2*matrix([ [2, .5], [.5, 1] ]) 23 | p = matrix([1.0, 1.0]) 24 | G = matrix([[-1.0,0.0],[0.0,-1.0]]) 25 | h = matrix([0.0,0.0]) 26 | A = matrix([1.0, 1.0], (1,2)) 27 | b = matrix(1.0) 28 | sol=solvers.qp(Q, p, G, h, A, b) 29 | #print(sol['x']) 30 | self.assertTrue(sol) 31 | 32 | def testSecondOrderConeProgram(self): 33 | # min c 34 | # subject to: ||Q||^2 <= h 35 | c = matrix([-2., 1., 5.]) 36 | G = [ matrix( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] 37 | G += [ matrix( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] 38 | h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] 39 | sol = solvers.socp(c, Gq = G, hq = h) 40 | #print(sol['x']) 41 | self.assertTrue(sol) 42 | 43 | 44 | -------------------------------------------------------------------------------- /unit_tests/test_pypoman.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Aug 6 11:52:42 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import numpy 9 | import pypoman 10 | import unittest 11 | 12 | #class TestPypoman(unittest.TestCase): 13 | # def testPolytopeProjection(self): 14 | A = numpy.array([ 15 | [-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 16 | [0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], 17 | [0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0], 18 | [0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0], 19 | [0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0], 20 | [0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 0], 21 | [0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0], 22 | [0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0], 23 | [0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, 0], 24 | [0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0], 25 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0], 26 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1], 27 | [1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0], 28 | [0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0], 29 | [0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0], 30 | [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1], 31 | [1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0], 32 | [0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0], 33 | [0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1]]) 34 | b = numpy.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 1, 2, 2, 1, 2, 3]) 35 | vertices = pypoman.compute_polytope_vertices(A, b) 36 | print vertices 37 | #unitTest = unittest() 38 | #unitTest.assertAlmostEquals(388, numpy.size(vertices,0))self 39 | -------------------------------------------------------------------------------- /examples/iterative_projection/IP_test_three_contacts.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat May 5 14:35:48 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | import time 8 | import pylab 9 | import pypoman 10 | import numpy as np 11 | 12 | from numpy import array, cross, dot, eye, hstack, vstack, zeros 13 | from numpy.linalg import norm 14 | from scipy.linalg import block_diag 15 | from plotting_tools import Plotter 16 | from constraints import Constraints 17 | from kinematics import Kinematics 18 | from math_tools import Math 19 | from computational_dynamics import ComputationalDynamics 20 | 21 | 22 | start_time = time.time() 23 | # number of contacts 24 | nc = 3 25 | # number of generators, i.e. rays used to linearize the friction cone 26 | ng = 8 27 | 28 | constraint_mode = 'only_friction' 29 | # number of decision variables of the problem 30 | n = nc*6 31 | 32 | # contact positions 33 | """ contact points """ 34 | LF_foot = np.array([0.3, 0.2, -.5]) 35 | print LF_foot 36 | RF_foot = np.array([0.3, -0.2, -.5]) 37 | LH_foot = np.array([-0.3, 0.2, -.5]) 38 | RH_foot = np.array([-0.3, -0.2, -.5]) 39 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 40 | 41 | ''' parameters to be tuned''' 42 | g = 9.81 43 | mass = 100. 44 | mu = 1. 45 | 46 | n1 = array([0.0, 0.0, 1.0]) 47 | n2 = array([0.0, 0.0, 1.0]) 48 | n3 = array([0.0, 0.0, 1.0]) 49 | math = Math() 50 | n1, n2, n3 = (math.normalize(n) for n in [n1, n2, n3]) 51 | normals = np.vstack([n1, n2, n3]) 52 | 53 | comp_dyn = ComputationalDynamics() 54 | comp_dyn.iterative_projection_bretl(constraint_mode, contacts, normals, mass, ng, mu) 55 | 56 | print("--- %s seconds ---" % (time.time() - start_time)) 57 | 58 | # pypoman.plot_polygon(points) 59 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Romeo Orsolino 5 | # 6 | # This file is part of jet_leg. 7 | # 8 | # jet_leg is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # jet_leg is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with jet_leg. If not, see . 20 | 21 | from setuptools import find_packages, setup 22 | 23 | 24 | setup( 25 | name='jet_leg', 26 | version='0.4.0', 27 | description="Python Polyhedron Manipulation", 28 | url="https://github.com/orsoromeo/jet-leg", 29 | author="Romeo Orsolino", 30 | author_email="orso.romeo@gmail.com", 31 | license="LGPL", 32 | classifiers=[ 33 | 'Development Status :: 4 - Beta', 34 | 'Intended Audience :: Developers', 35 | 'Intended Audience :: Education', 36 | 'Intended Audience :: Science/Research', 37 | 'License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)', 38 | 'Operating System :: POSIX :: Linux', 39 | 'Programming Language :: Python :: 3.8', 40 | 'Topic :: Scientific/Engineering :: Mathematics'], 41 | packages=find_packages(), 42 | include_package_data=True, # This tells setuptools to use MANIFEST.in 43 | ) 44 | -------------------------------------------------------------------------------- /jet_leg/kinematics/kinematics_interface.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jul 2 05:34:42 2018 4 | 5 | @author: romeo orsolino 6 | """ 7 | import numpy as np 8 | 9 | from jet_leg.robots.dog_interface import DogInterface 10 | from jet_leg.dynamics.rigid_body_dynamics import RigidBodyDynamics 11 | from jet_leg.robots.hyq.hyq_kinematics import HyQKinematics 12 | from jet_leg.robots.anymal.anymal_kinematics import anymalKinematics 13 | from jet_leg.robots.hyqreal.hyqreal_kinematics import hyqrealKinematics 14 | 15 | 16 | class KinematicsInterface: 17 | def __init__(self, robot_name): 18 | 19 | self.dog = DogInterface() 20 | self.rbd = RigidBodyDynamics() 21 | self.robotName = robot_name 22 | if self.robotName == 'hyq': 23 | self.hyqKin = HyQKinematics() 24 | if self.robotName == 'anymal': 25 | self.anymalKin = anymalKinematics() 26 | elif self.robotName == 'hyqreal': 27 | self.hyqrealKin = hyqrealKinematics() 28 | 29 | def get_jacobians(self): 30 | if self.robotName == 'hyq': 31 | return self.hyqKin.getLegJacobians() 32 | elif self.robotName == 'hyqreal': 33 | return self.hyqrealKin.getLegJacobians() 34 | elif self.robotName == 'anymal': 35 | return self.anymalKin.getLegJacobians() 36 | 37 | def inverse_kin(self, contactsBF, foot_vel): 38 | 39 | if self.robotName == 'hyq': 40 | q = self.hyqKin.fixedBaseInverseKinematics(contactsBF, foot_vel) 41 | return q 42 | elif self.robotName == 'hyqreal': 43 | q = self.hyqrealKin.fixedBaseInverseKinematics(contactsBF) 44 | return q 45 | elif self.robotName == 'anymal': 46 | q = self.anymalKin.fixedBaseInverseKinematics(contactsBF) 47 | return q 48 | 49 | -------------------------------------------------------------------------------- /examples/kinematics/ikpy_example.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from context import jet_leg 3 | import time 4 | from numpy import array 5 | import ikpy 6 | from jet_leg.hyq_kinematics import HyQKinematics 7 | 8 | 9 | ik_LH = ikpy.chain.Chain.from_urdf_file("../resources/urdfs/hyq/urdf/leg/hyq_leg_LH.urdf") 10 | ik_RH = ikpy.chain.Chain.from_urdf_file("../resources/urdfs/hyq/urdf/leg/hyq_leg_RH.urdf") 11 | 12 | 13 | LF_foot = np.array([0.3735, 0.33, -0.5]) 14 | RF_foot = np.array([0.3735, -0.33, -0.5]) 15 | LH_foot = np.array([-0.3735, 0.33, -0.5]) 16 | RH_foot = np.array([-0.3735, -0.33, -0.5]) 17 | starting_contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 18 | 19 | target_frame = np.eye(4) 20 | target_frame[:3, 3] = LF_foot 21 | print target_frame 22 | ik_LF = ikpy.chain.Chain.from_urdf_file("../resources/urdfs/hyq/urdf/leg/hyq_leg_LF.urdf") 23 | q = ik_LF.inverse_kinematics(target_frame) 24 | print "ikpy LF: ", q 25 | 26 | target_frame = np.eye(4) 27 | target_frame[:3, 3] = RF_foot 28 | print target_frame 29 | ik_RF = ikpy.chain.Chain.from_urdf_file("../resources/urdfs/hyq/urdf/leg/hyq_leg_RF.urdf") 30 | q = ik_RF.inverse_kinematics(target_frame) 31 | print "ikpy RF: ", q 32 | 33 | target_frame = np.eye(4) 34 | target_frame[:3, 3] = LH_foot 35 | print target_frame 36 | q = ik_LH.inverse_kinematics(target_frame) 37 | print "ikpy LH: ", q 38 | 39 | target_frame = np.eye(4) 40 | target_frame[:3, 3] = RH_foot 41 | print target_frame 42 | q = ik_RH.inverse_kinematics(target_frame) 43 | print "ikpy RH: ", q 44 | #real_frame = ik_LF.forward_kinematics(q) 45 | #print("Computed position vector : %s, original position vector : %s" % (real_frame[:3, 3], target_frame[:3, 3])) 46 | 47 | hyqKin = HyQKinematics() 48 | 49 | hyqKin.init_jacobians() 50 | hyqKin.init_homogeneous() 51 | 52 | foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]) 53 | 54 | #print starting_contacts 55 | 56 | q = hyqKin.inverse_kin(starting_contacts, foot_vel) 57 | print "hardcoded ik: ", q -------------------------------------------------------------------------------- /unit_tests/test_directed_optimization_variable_constraints.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Aug 16 14:27:10 2018 4 | 5 | @author: romeoorsolino 6 | 7 | This code performs a test on the exploration of the actuation region along an arbitrary direction. 8 | The direction is given by the variable "angle" that tell the desired yaw direction of the robot; 9 | the other angles are assumed here to be zero. 10 | 11 | The same test can be repeated N number of times to emulate the estimation of the actuation region which 12 | requires the solution of this problem multiple times along different directions. 13 | 14 | The use can change the following parameters: 15 | 16 | 1- comWF: position of the Center of Mass wrt to the world frame 17 | 18 | 2- contstraint_mode: type of constraint to be applied on the contact forces (this 19 | can be either 'ONLY_ACTUATION' or only 'ONLY_FRICTION') 20 | 21 | 3- N: number of times that the execution of the variable constraints search is called 22 | 23 | 4- yaw_angle: desired yaw angle in which we wish the robot to move (in radiants) 24 | """ 25 | 26 | import numpy as np 27 | 28 | from context import jet_leg 29 | from jet_leg.sequential_iterative_projection import SequentialIterativeProjection 30 | 31 | '''User parameters''' 32 | comWF = np.array([0.2, 0.05, 0.0]) 33 | constraint_mode = 'ONLY_ACTUATION' 34 | N = 1; 35 | yaw_angle = 0.04 36 | 37 | 38 | 39 | '''Implementation ''' 40 | print "angle is: ", yaw_angle 41 | dir_x = np.cos(yaw_angle) 42 | dir_y = np.sin(yaw_angle) 43 | desired_direction = np.array([dir_x, dir_y, 0.0]) 44 | 45 | numberOfContactPoints = 3; 46 | """ contact points """ 47 | LF_foot = np.array([0.4, 0.3, -0.5]) 48 | RF_foot = np.array([0.4, -0.3, -0.5]) 49 | LH_foot = np.array([-0.4, 0.3, -0.5]) 50 | RH_foot = np.array([-0.4, -0.3, -0.5]) 51 | 52 | contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 53 | contacts = contactsToStack[0:numberOfContactPoints, :] 54 | 55 | sequentialIP = SequentialIterativeProjection() 56 | for iter in range(0,N): 57 | intersection_point = sequentialIP.optimize_direction_variable_constraints(constraint_mode, desired_direction, contacts, comWF) 58 | print "intersection points: ", intersection_point -------------------------------------------------------------------------------- /jet_leg/plotting/plotting_tools.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon May 28 12:36:24 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | from jet_leg.maths.computational_geometry import ComputationalGeometry 11 | 12 | class Plotter: 13 | 14 | def plot_polygon(self, points, color = '--b', Label = ''): 15 | if np.size(points,1)==2: 16 | x = np.hstack([points[:,0], points[0,0]]) 17 | y = np.hstack([points[:,1], points[0,1]]) 18 | else: 19 | x = np.hstack([points[0,:], points[0,0]]) 20 | y = np.hstack([points[1,:], points[1,0]]) 21 | 22 | plt.plot(x, y, color, linewidth=10., label = Label) 23 | 24 | def plot_line(self, ax, line_coefficients): 25 | a = line_coefficients[0] 26 | b = line_coefficients[1] 27 | c = line_coefficients[2] 28 | x1 = -2 29 | y1 = -a/b*x1 - c/b 30 | 31 | x2 = 2 32 | y2 = -a/b*x2 - c/b 33 | x = np.array([x1, x2]) 34 | y = np.array([y1, y2]) 35 | ax.plot(x,y) 36 | 37 | def plot_actuation_region(self, ax, line_coefficients): 38 | edges_number = np.size(line_coefficients,1) 39 | for j in range(0, edges_number): 40 | self.plot_line(ax, line_coefficients[:,j]) 41 | 42 | def plot_facet(self,ax,facet): 43 | facet_x = facet[0,:] 44 | facet_y = facet[1,:] 45 | facet_z = facet[2,:] 46 | surf = ax.plot_trisurf(facet_x, facet_y, facet_z, linewidth=0., alpha = 0.3) 47 | z = np.array([facet_z, facet_z]) 48 | surf = ax.plot_wireframe(facet_x, facet_y, z, linewidth=1.) 49 | 50 | def plot_cube(self,ax,vertices): 51 | geom = ComputationalGeometry() 52 | face1, face2, face3, face4, face5, face6 = geom.get_facets(vertices) 53 | self.plot_facet(ax,face1) 54 | self.plot_facet(ax,face2) 55 | self.plot_facet(ax,face3) 56 | self.plot_facet(ax,face4) 57 | self.plot_facet(ax,face5) 58 | self.plot_facet(ax,face6) 59 | 60 | def plot_actuation_polygon(self, ax, vertices, foot_pos, scaling_factor = 2000): 61 | vertex = np.zeros((3,8)) 62 | for j in range(0,8): 63 | vertex[0,j] = float(vertices[0,j])/float(scaling_factor) 64 | vertex[1,j] = vertices[1,j]/float(scaling_factor) 65 | vertex[2,j] = vertices[2,j]/float(scaling_factor) 66 | vertex[:,j] = np.add(vertex[:,j],np.transpose(foot_pos)) 67 | 68 | 69 | 70 | self.plot_cube(ax,vertex) -------------------------------------------------------------------------------- /examples/iterative_projection/one_contact.py: -------------------------------------------------------------------------------- 1 | """ 2 | Created on Wed Apr 18 14:15:45 2018 3 | 4 | @author: Romeo Orsolino 5 | """ 6 | import pylab 7 | import pypoman 8 | import numpy as np 9 | from numpy import array, eye, ones, vstack, zeros 10 | 11 | 12 | def skew(v): 13 | if len(v) == 4: v = v[:3]/v[3] 14 | skv = np.roll(np.roll(np.diag(v.flatten()), 1, 1), -1, 0) 15 | return skv - skv.T 16 | 17 | # number of contacts 18 | nc = 1 19 | # number of decision variables of the problem 20 | # (3 contact forces for each contact + 2 com coordinates x and y) 21 | n = 3 22 | 23 | # I set the com coordinates to be in the bottom of the state array: 24 | # x = [f1_x, f1_y, f1_z, ... , f3_x, f3_y, f3_z, com_x, com_y] 25 | E = zeros((2, n)) 26 | E[0, n-2] = 1. 27 | E[1, n-1] = 1. 28 | f = zeros(2) 29 | proj = (E, f) # y = E * x + f 30 | 31 | g = -9.81 32 | mass = 10 33 | mu = 0.5 34 | grav = array([[0.], [0.], [mass*g]]) 35 | 36 | # postion of the contact. 37 | r1 = array([0.0, 5.0, 0.0]) 38 | 39 | # contact surface normal 40 | n1 = array([[0.0], [0.0], [1.0]]) 41 | 42 | # projection matrix 43 | P = array([[1., 0., 0.], 44 | [0., 1., 0.]]) 45 | Pt = np.transpose(P) 46 | 47 | ## Definition of the equality constraints 48 | m_eq = 6 49 | #grav_skew = skew(grav); 50 | #A2 = vstack([zeros((3,2)), -np.dot(grav_skew,Pt)]) 51 | 52 | a1 = vstack([+eye(3), skew(r1)]) 53 | 54 | #A1 = np.hstack((a1, a2, a3)) 55 | 56 | C = a1#np.hstack((a1,A2)) 57 | 58 | d = vstack([-grav, zeros((3,1))]).reshape((6)) 59 | C = zeros((6,6)) 60 | d = zeros((6,1)).reshape((6)) 61 | #print(C) 62 | #print(d) 63 | 64 | eq = (C, d) # C * x == d 65 | 66 | ## Definition of the inequality constraints 67 | n1_t = np.transpose(n1) 68 | 69 | C = +eye(3) - np.dot(n1, n1_t) 70 | #print(C) 71 | C = C[0:2,0:3] 72 | #print(C) 73 | u = mu*n1 74 | #print(u) 75 | U = vstack([np.transpose(u), 76 | np.transpose(u)]) 77 | #print(U) 78 | c1 = C - U 79 | #print(C) 80 | c2 = C + U 81 | #d1 = np.block([[c1, zeros((3,2))], 82 | # [zeros((2,3)), +eye(2)]]) 83 | #d2 = np.block([[c2, zeros((3,2))], 84 | # [zeros((2,3)), -eye(2)]]) 85 | 86 | A1 = vstack([c1, c2]) 87 | A = np.block([[A1, zeros((4,3))], 88 | [zeros((4,3)), A1]]) 89 | #b1 = vstack([zeros((3,1)), +1000.*ones((2,1))]) 90 | #b2 = vstack([zeros((3,1)), +1000.*ones((2,1))]) 91 | b = zeros((8,1)).reshape((8)) 92 | print(A) 93 | print(b) 94 | ineq = (A, b) # A * x <= b 95 | 96 | vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl') 97 | pylab.ion() 98 | pylab.figure() 99 | print(vertices) 100 | pypoman.plot_polygon(vertices) -------------------------------------------------------------------------------- /examples/iterative_projection/pymanoid_double_support.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of jet-leg . 7 | # 8 | # jet-leg is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # jet-leg is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with jet-leg. If not, see . 20 | 21 | import IPython 22 | 23 | import numpy 24 | import pymanoid 25 | 26 | from pymanoid import Stance 27 | from pymanoid.gui import StaticEquilibriumWrenchDrawer 28 | 29 | from pymanoid_common import ActuationDependentArea 30 | from pymanoid_common import LocalActuationDependentPolygonDrawer 31 | from pymanoid_common import draw_horizontal_polygon 32 | from pymanoid_common import set_torque_limits 33 | 34 | 35 | if __name__ == "__main__": 36 | sim = pymanoid.Simulation(dt=0.03) 37 | robot = pymanoid.robots.JVRC1() 38 | set_torque_limits(robot) 39 | robot.show_com() 40 | 41 | sim.set_viewer() 42 | sim.set_camera_transform(numpy.array([ 43 | [-0.75166831, -0.47792323, 0.45451529, -0.99502754], 44 | [-0.65817994, 0.49930137, -0.563469, 1.14903212], 45 | [0.04235481, -0.72269463, -0.68986849, 2.35493684], 46 | [0., 0., 0., 1.]])) 47 | robot.set_transparency(0.25) 48 | 49 | stance = Stance.from_json('jvrc1_double_support.json') 50 | stance.bind(robot) 51 | robot.ik.solve() 52 | 53 | polygon_height = stance.com.z # [m] 54 | uncons_polygon = stance.compute_static_equilibrium_polygon(method="cdd") 55 | h1 = draw_horizontal_polygon(uncons_polygon, polygon_height, color='g') 56 | 57 | sim.schedule(robot.ik) 58 | sim.start() 59 | 60 | ada = ActuationDependentArea(robot, stance) 61 | ada.sample_working_set(uncons_polygon, "sample", 20) 62 | 63 | PLAY_WITH_LOCAL_POLYGONS = False 64 | if PLAY_WITH_LOCAL_POLYGONS: 65 | local_polygon_drawer = LocalActuationDependentPolygonDrawer( 66 | robot, stance, polygon_height) 67 | wrench_drawer = StaticEquilibriumWrenchDrawer(stance) 68 | sim.schedule_extra(local_polygon_drawer) 69 | sim.schedule_extra(wrench_drawer) 70 | 71 | DRAW_VOLUME = True 72 | if DRAW_VOLUME: 73 | h2 = ada.draw_volume( 74 | min_height=0.5, max_height=1.0, dh=0.05, hull=True) 75 | else: # not DRAW_VOLUME 76 | h2 = ada.draw_at_height(polygon_height) 77 | 78 | if IPython.get_ipython() is None: 79 | IPython.embed() 80 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/urdf/leg/hyq_leg_RH.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/urdf/leg/hyq_leg_LF.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/urdf/leg/hyq_leg_LH.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /examples/iterative_projection/bretl_projection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jun 4 15:45:43 2018 4 | 5 | @author: Romeo Orsolino 6 | 7 | """ 8 | import math 9 | import numpy as np 10 | # for plotting 11 | from plotting_tools import Plotter 12 | from arrow3D import Arrow3D 13 | import matplotlib.pyplot as plt 14 | 15 | def get_edges(vertices): 16 | edges = np.zeros((0,3)) 17 | for j in range(1, points_num): 18 | diff = vertices[j,:] - vertices[j-1,:] 19 | edge = np.hstack([diff, np.dot(diff,vertices[j,:])]) 20 | edges = np.vstack([edges,edge]) 21 | 22 | diff = vertices[0,:] - vertices[-1,:] 23 | edge = np.hstack([diff, np.dot(diff,vertices[0,:])]) 24 | edges = np.vstack([edges,edge]) 25 | return edges 26 | 27 | def find_closest_points(vertex_out, inner_approx): 28 | points_num = np.size(inner_approx,0) 29 | sorted_distance = [None]*points_num 30 | for j in range(0, points_num): 31 | v_in = inner_approx[j,:] 32 | sorted_distance[j] = math.hypot(vertex_out[0] - v_in[0], vertex_out[1] - v_in[1]) 33 | 34 | sorted_indices = np.argsort(sorted_distance) 35 | return sorted_distance, sorted_indices 36 | 37 | def get_largest_area(inner_approx, outer_approx): 38 | points_num = np.size(inner_approx,0) 39 | old_area = 0. 40 | area = 0. 41 | for j in range(0, points_num): 42 | closest_v, indices = find_closest_points(outer_approx[j,:], inner_approx) 43 | p1 = np.hstack([outer_approx[j,:],0.]) 44 | p2 = np.hstack([inner_approx[indices[0],:],0.]) 45 | p3 = np.hstack([inner_approx[indices[1],:],0.]) 46 | cross_prod = np.cross(p2 - p1, p3 - p1) 47 | area = cross_prod[2] 48 | if area > old_area : 49 | edge_to_expand = np.vstack([closest_v[indices[0],:], closest_v[indices[1],:]]) 50 | 51 | ordered_indices = np.argsort(area) 52 | 53 | return largest_area 54 | 55 | r1 = array([10.0, 10.0, 0.0]) 56 | r2 = array([10.0,-10.0, 0.0]) 57 | r3 = array([-10.0,-10.0, 0.0]) 58 | r4 = array([-10.0,10.0, 0.0]) 59 | ''' init outer and inner approximation''' 60 | outer_approx = np.vstack([r1[0:2], r2[0:2], r3[0:2], r4[0:2]]) 61 | inner_approx = np.zeros((0,2)) 62 | points_num = np.size(outer_approx,0) 63 | for j in range(0, points_num-1): 64 | avg = (outer_approx[j,:] + outer_approx[j+1,:])/2. 65 | inner_approx = np.vstack([inner_approx, avg]) 66 | 67 | avg = (outer_approx[points_num-1,:] + outer_approx[0,:])/2. 68 | inner_approx = np.vstack([inner_approx, avg]) 69 | 70 | ''' get the edges from the v-description ''' 71 | outer_edges = get_edges(outer_approx) 72 | inner_edges = get_edges(inner_approx) 73 | 74 | ''' compute max area ''' 75 | find_closest_points(outer_approx[0,:], inner_approx) 76 | get_largest_area(outer_approx, inner_approx) 77 | print 78 | 79 | print outer_edges 80 | print inner_edges 81 | ''' plotting ''' 82 | inner_x = np.hstack([inner_approx[:,0], inner_approx[0,0]]) 83 | inner_y = np.hstack([inner_approx[:,1], inner_approx[0,1]]) 84 | outer_x = np.hstack([outer_approx[:,0], outer_approx[0,0]]) 85 | outer_y = np.hstack([outer_approx[:,1], outer_approx[0,1]]) 86 | 87 | plt.plot(inner_x, inner_y) 88 | plt.plot(outer_x, outer_y) -------------------------------------------------------------------------------- /examples/static_equilibrium_check/single_LP_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 12 10:54:31 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import time 9 | import pylab 10 | import pypoman 11 | import numpy as np 12 | 13 | from context import jet_leg 14 | 15 | from numpy import array 16 | from numpy.linalg import norm 17 | from jet_leg.plotting_tools import Plotter 18 | import random 19 | from jet_leg.math_tools import Math 20 | from jet_leg.computational_dynamics import ComputationalDynamics 21 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 22 | 23 | import matplotlib.pyplot as plt 24 | from jet_leg.arrow3D import Arrow3D 25 | 26 | plt.close('all') 27 | math = Math() 28 | # number of contacts 29 | #nc = 3 30 | # number of generators, i.e. rays used to linearize the friction cone 31 | ng = 4 32 | 33 | # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION 34 | 35 | constraint_mode_IP = ['ONLY_ACTUATION', 36 | 'ONLY_ACTUATION', 37 | 'ONLY_ACTUATION', 38 | 'ONLY_ACTUATION'] 39 | useVariableJacobian = False 40 | 41 | # contact positions 42 | """ contact points """ 43 | 44 | LF_foot = np.array([0.3, 0.2, -0.5]) 45 | RF_foot = np.array([0.3, -0.2, -0.5]) 46 | LH_foot = np.array([-0.3, 0.2, -0.5]) 47 | RH_foot = np.array([-0.3, -0.2, -0.5]) 48 | 49 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 50 | 51 | ''' parameters to be tuned''' 52 | g = 9.81 53 | trunk_mass = 25. 54 | mu = 0.9 55 | 56 | stanceFeet = [1,1,1,1] 57 | randomSwingLeg = random.randint(0,3) 58 | print 'Swing leg', randomSwingLeg 59 | print 'stanceLegs ' ,stanceFeet 60 | 61 | axisZ= array([[0.0], [0.0], [1.0]]) 62 | 63 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 64 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 65 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 66 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 67 | normals = np.vstack([n1, n2, n3, n4]) 68 | 69 | LF_tau_lim = [50.0, 100.0, 100.0] 70 | RF_tau_lim = [50.0, 100.0, 100.0] 71 | LH_tau_lim = [50.0, 100.0, 100.0] 72 | RH_tau_lim = [50.0, 100.0, 100.0] 73 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 74 | 75 | comWF = np.array([0.0,-0.3,0.0]) 76 | # 77 | fig = plt.figure() 78 | nc = np.sum(stanceFeet) 79 | plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15) 80 | 81 | comp_dyn = ComputationalDynamics() 82 | params = IterativeProjectionParameters() 83 | 84 | params.setContactsPosBF(contacts) 85 | params.setCoMPosWF(comWF) 86 | params.setTorqueLims(torque_limits) 87 | params.setActiveContacts(stanceFeet) 88 | params.setConstraintModes(constraint_mode_IP) 89 | params.setContactNormals(normals) 90 | params.setFrictionCoefficient(mu) 91 | params.setNumberOfFrictionConesEdges(ng) 92 | params.setTotalMass(trunk_mass) 93 | 94 | ''' compute iterative projection ''' 95 | feasible_points = np.zeros((0,3)) 96 | unfeasible_points = np.zeros((0,3)) 97 | contact_forces = np.zeros((0,nc*3)) 98 | LP_actuation_polygons, feasible_points, unfeasible_points, contact_forces = comp_dyn.compute_lp(params, feasible_points, unfeasible_points, contact_forces, comWF) 99 | print 'is point feasible?', feasible_points 100 | #print IP_points 101 | 102 | ''' plotting Iterative Projection points ''' 103 | feasiblePointsSize = np.size(feasible_points,0) 104 | for j in range(0, feasiblePointsSize): 105 | plt.scatter(feasible_points[j,0], feasible_points[j,1],c='g',s=50) 106 | lastFeasibleIndex = j 107 | unfeasiblePointsSize = np.size(unfeasible_points,0) 108 | 109 | for j in range(0, unfeasiblePointsSize): 110 | plt.scatter(unfeasible_points[j,0], unfeasible_points[j,1],c='r',s=50) 111 | lastUnfeasibleIndex = j 112 | 113 | 114 | plt.grid() 115 | plt.xlabel("X [m]") 116 | plt.ylabel("Y [m]") 117 | #plt.legend() 118 | plt.show() -------------------------------------------------------------------------------- /jet_leg/maths/computational_geometry.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon May 28 13:05:01 2018 4 | 5 | @author: rorsolino 6 | """ 7 | import numpy as np 8 | 9 | class ComputationalGeometry: 10 | 11 | def get_facets(self, v_rep): 12 | vx = v_rep[0,0:8] 13 | vy = v_rep[1,0:8] 14 | vz = v_rep[2,0:8] 15 | face1 = np.vstack([np.hstack([vx[0:4],vx[0]]), 16 | np.hstack([vy[0:4],vy[0]]), 17 | np.hstack([vz[0:4],vz[0]])]) 18 | face2 = np.vstack([np.hstack([vx[4:8],vx[4]]), 19 | np.hstack([vy[4:8],vy[4]]), 20 | np.hstack([vz[4:8],vz[4]])]) 21 | face3 = np.vstack([np.hstack([vx[0],vx[1],vx[5],vx[4],vx[0]]), 22 | np.hstack([vy[0],vy[1],vy[5],vy[4],vy[0]]), 23 | np.hstack([vz[0],vz[1],vz[5],vz[4],vz[0]])]) 24 | face4 = np.vstack([np.hstack([vx[1],vx[2],vx[6],vx[5],vx[1]]), 25 | np.hstack([vy[1],vy[2],vy[6],vy[5],vy[1]]), 26 | np.hstack([vz[1],vz[2],vz[6],vz[5],vz[1]])]) 27 | face5 = np.vstack([np.hstack([vx[2],vx[3],vx[7],vx[6],vx[2]]), 28 | np.hstack([vy[2],vy[3],vy[7],vy[6],vy[2]]), 29 | np.hstack([vz[2],vz[3],vz[7],vz[6],vz[2]])]) 30 | face6 = np.vstack([np.hstack([vx[0],vx[3],vx[7],vx[4],vx[0]]), 31 | np.hstack([vy[0],vy[3],vy[7],vy[4],vy[0]]), 32 | np.hstack([vz[0],vz[3],vz[7],vz[4],vz[0]])]) 33 | return face1, face2, face3, face4, face5, face6 34 | 35 | def get_halfspace_rep(self, v_rep): 36 | face1, face2, face3, face4, face5, face6 = self.get_facets(v_rep) 37 | edge1 = face1[:,2] - face1[:,1] 38 | edge2 = face1[:,0] - face1[:,1] 39 | normal = np.cross(edge1, edge2) 40 | norm = np.linalg.norm(normal) 41 | normal = normal/norm 42 | known_term = np.dot(normal, face1[:,0]) 43 | h_rep1 = np.hstack([normal, known_term]) 44 | 45 | edge1 = face2[:,2] - face2[:,1] 46 | edge2 = face2[:,0] - face2[:,1] 47 | normal = np.cross(edge1, edge2) 48 | norm = np.linalg.norm(normal) 49 | normal = normal/norm 50 | known_term = np.dot(normal, face2[:,0]) 51 | h_rep2 = np.hstack([normal, known_term]) 52 | 53 | edge1 = face3[:,2] - face3[:,1] 54 | edge2 = face3[:,0] - face3[:,1] 55 | normal = np.cross(edge1, edge2) 56 | norm = np.linalg.norm(normal) 57 | normal = normal/norm 58 | known_term = np.dot(normal, face3[:,0]) 59 | h_rep3 = np.hstack([normal, known_term]) 60 | 61 | edge1 = face4[:,2] - face4[:,1] 62 | edge2 = face4[:,0] - face4[:,1] 63 | normal = np.cross(edge1, edge2) 64 | norm = np.linalg.norm(normal) 65 | normal = normal/norm 66 | known_term = np.dot(normal, face4[:,0]) 67 | h_rep4 = np.hstack([normal, known_term]) 68 | 69 | edge1 = face5[:,2] - face5[:,1] 70 | edge2 = face5[:,0] - face5[:,1] 71 | normal = np.cross(edge1, edge2) 72 | #print edge1, edge2, normal 73 | norm = np.linalg.norm(normal) 74 | #print "norms ", norm, normal 75 | normal = normal/norm 76 | #print normal 77 | known_term = np.dot(normal, face5[:,0]) 78 | h_rep5 = np.hstack([normal, known_term]) 79 | 80 | edge1 = face6[:,2] - face6[:,1] 81 | edge2 = face6[:,0] - face6[:,1] 82 | normal = np.cross(edge1, edge2) 83 | norm = np.linalg.norm(normal) 84 | normal = normal/norm 85 | known_term = np.dot(normal, face6[:,0]) 86 | h_rep6 = np.hstack([normal, known_term]) 87 | 88 | return h_rep1, h_rep2, h_rep3, h_rep4, h_rep5, h_rep6 89 | 90 | def computePolygonArea(self, v_rep): 91 | v_rep = np.transpose(v_rep) 92 | # print 'v rep ', v_rep 93 | # Initialze area 94 | area = 0.0; 95 | num_of_vertices = np.size(v_rep,1) 96 | # print num_of_vertices 97 | #Calculate value of shoelace formula 98 | j = num_of_vertices - 1; 99 | for i in range(0, num_of_vertices): 100 | area += (v_rep[0,j] + v_rep[0,i]) * (v_rep[1,j] - v_rep[1,i]); 101 | j = i; #j is previous vertex to i 102 | 103 | # Return absolute value 104 | return np.abs(area / 2.0) 105 | -------------------------------------------------------------------------------- /jet_leg/robots/hyqreal/hyqreal_kinematics.py: -------------------------------------------------------------------------------- 1 | import pinocchio 2 | from pinocchio.utils import * 3 | from pinocchio.robot_wrapper import RobotWrapper 4 | import matplotlib.pyplot as plt 5 | import os 6 | 7 | 8 | class hyqrealKinematics(): 9 | def __init__(self): 10 | 11 | self.PKG = os.path.dirname(os.path.abspath(__file__)) + '/urdfs/hyqreal/' 12 | self.URDF = self.PKG + 'urdf/hyqreal.urdf' 13 | if self.PKG is None: 14 | self.robot = RobotWrapper.BuildFromURDF(self.URDF) 15 | else: 16 | self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG]) 17 | 18 | self.model = self.robot.model 19 | self.data = self.robot.data 20 | self.LF_foot_jac = None 21 | self.LH_foot_jac = None 22 | self.RF_foot_jac = None 23 | self.RH_foot_jac = None 24 | self.urdf_foot_name_lf = 'lf_foot' 25 | self.urdf_foot_name_lh = 'lh_foot' 26 | self.urdf_foot_name_rf = 'rf_foot' 27 | self.urdf_foot_name_rh = 'rh_foot' 28 | 29 | def getBlockIndex(self, frame_name): 30 | if frame_name == self.urdf_foot_name_lf: 31 | idx = 0 32 | elif frame_name == self.urdf_foot_name_lh: 33 | idx = 3 34 | elif frame_name == self.urdf_foot_name_rf: 35 | idx = 6 36 | elif frame_name == self.urdf_foot_name_rh: 37 | idx = 9 38 | 39 | #print ('index is',idx) 40 | return idx 41 | 42 | def footInverseKinematicsFixedBase(self, foot_pos_des, frame_name): 43 | frame_id = self.model.getFrameId(frame_name) 44 | blockIdx = self.getBlockIndex(frame_name) 45 | anymal_q0 = pinocchio.neutral(self.model) 46 | q = anymal_q0 47 | eps = 0.005 48 | IT_MAX = 200 49 | DT = 1e-1 50 | err = np.zeros((3, 1)) 51 | e = np.zeros((3, 1)) 52 | 53 | i = 0 54 | while True: 55 | pinocchio.forwardKinematics(self.model, self.data, q) 56 | pinocchio.framesForwardKinematics(self.model, self.data, q) 57 | foot_pos = self.data.oMf[frame_id].translation 58 | #err = np.hstack([err, (foot_pos - foot_pos_des)]) 59 | #e = err[:,-1] 60 | #print foot_pos_des[0], foot_pos[[0]], foot_pos[[0]] - foot_pos_des[0] 61 | #e = foot_pos - foot_pos_des 62 | e[0] = foot_pos[[0]] - foot_pos_des[0] 63 | e[1] = foot_pos[[1]] - foot_pos_des[1] 64 | e[2] = foot_pos[[2]] - foot_pos_des[2] 65 | if np.linalg.norm(e) < eps: 66 | #print("IK Convergence achieved!") 67 | break 68 | if i >= IT_MAX: 69 | print("\n Warning: the iterative algorithm has not reached convergence to the desired precision. Error is: ", np.linalg.norm(e)) 70 | raise Exception('FailedConvergence') 71 | pinocchio.computeJointJacobians(self.model, self.data, q) # compute jacobians 72 | J = pinocchio.getFrameJacobian(self.model, self.data, frame_id, pinocchio.LOCAL_WORLD_ALIGNED) 73 | J_lin = J[:3, :] 74 | #print J_lin 75 | v = - np.linalg.pinv(J_lin) * e 76 | q = pinocchio.integrate(self.model, q, v * DT) 77 | i += 1 78 | #print i 79 | 80 | q_leg = q[blockIdx:blockIdx+3] 81 | J_leg = J_lin[:,blockIdx:blockIdx+3] 82 | return q_leg, J_leg, err 83 | 84 | def fixedBaseInverseKinematics(self, feetPosDes): 85 | f_p_des = np.array(feetPosDes[0,:]).T 86 | q_LF, self.LF_foot_jac, err = self.footInverseKinematicsFixedBase(f_p_des, self.urdf_foot_name_lf) 87 | 88 | f_p_des = np.array(feetPosDes[2, :]).T 89 | q_LH, self.LH_foot_jac, err = self.footInverseKinematicsFixedBase(f_p_des, self.urdf_foot_name_lh) 90 | 91 | f_p_des = np.array(feetPosDes[1, :]).T 92 | q_RF, self.RF_foot_jac, err = self.footInverseKinematicsFixedBase(f_p_des, self.urdf_foot_name_rf) 93 | 94 | f_p_des = np.array(feetPosDes[3, :]).T 95 | q_RH, self.RH_foot_jac, err = self.footInverseKinematicsFixedBase(f_p_des, self.urdf_foot_name_rh) 96 | 97 | '''please NOTICE here the alphabetical order of the legs in the vector q: LF -> LH -> RF -> RH ''' 98 | q = np.vstack([q_LF, q_LH, q_RF, q_RH]) 99 | legJacs = np.vstack([self.LF_foot_jac, self.LH_foot_jac, self.RF_foot_jac, self.RH_foot_jac]) 100 | return q, legJacs 101 | 102 | def getLegJacobians(self): 103 | isOutOfWS = False 104 | return self.LF_foot_jac, self.RF_foot_jac, self.LH_foot_jac, self.RH_foot_jac, isOutOfWS 105 | 106 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/urdf/leg/hyq_leg_RF.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | --> 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /jet_leg/optimization/directed_optimization.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 21:57:55 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | import numpy as np 8 | 9 | #from context import jet_leg 10 | 11 | import time 12 | 13 | from pypoman.lp import solve_lp, GLPK_IF_AVAILABLE 14 | from pypoman.bretl import Vertex 15 | from pypoman.bretl import Polygon 16 | 17 | import random 18 | import cvxopt 19 | from cvxopt import matrix, solvers 20 | 21 | from numpy import array, cos, cross, pi, sin 22 | from numpy.random import random 23 | from scipy.linalg import norm 24 | from scipy.linalg import block_diag 25 | 26 | from numpy import array, cross, dot, eye, hstack, vstack, zeros 27 | 28 | import matplotlib.pyplot as plt 29 | from jet_leg.plotting_tools import Plotter 30 | from jet_leg.constraints import Constraints 31 | from jet_leg.hyq_kinematics import HyQKinematics 32 | from jet_leg.math_tools import Math 33 | from jet_leg.computational_dynamics import ComputationalDynamics 34 | from jet_leg.height_map import HeightMap 35 | 36 | from path_sequential_iterative_projection import PathIterativeProjection 37 | 38 | 39 | ''' MAIN ''' 40 | start_t_IPVC = time.time() 41 | math = Math() 42 | compDyn = ComputationalDynamics() 43 | pathIP = PathIterativeProjection() 44 | # number of contacts 45 | nc = 3 46 | # number of generators, i.e. rays used to linearize the friction cone 47 | ng = 4 48 | 49 | # ONLY_ACTUATION or ONLY_FRICTION 50 | constraint_mode = 'ONLY_ACTUATION' 51 | useVariableJacobian = True 52 | trunk_mass = 100 53 | mu = 0.8 54 | # number of decision variables of the problem 55 | n = nc*6 56 | i = 0 57 | comTrajectoriesToStack = np.zeros((0,3)) 58 | terrain = HeightMap() 59 | 60 | comWF = np.array([0.1, 0.1, 0.0]) 61 | optimizedVariablesToStack = np.zeros((0,3)) 62 | iterProj = PathIterativeProjection() 63 | for_iter = 0 64 | for LH_x in np.arange(-0.8,-0.3, 0.1): 65 | for LH_y in np.arange(0.1,0.4, 0.1): 66 | for dir_y in np.arange(-0.2,0.0,0.2): 67 | desired_direction = np.array([-1.0, dir_y, 0.0]) 68 | #print "direction: ", desired_direction 69 | """ contact points """ 70 | LF_foot = np.array([0.4, 0.3, -0.5]) 71 | RF_foot = np.array([0.4, -0.3, -0.5]) 72 | terrainHeight = terrain.get_height(LH_x, LH_y) 73 | LH_foot = np.array([LH_x, LH_y, terrainHeight-0.5]) 74 | #print "Terrain height: ", LH_foot 75 | RH_foot = np.array([-0.3, -0.2, -0.5]) 76 | 77 | contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 78 | contacts = contactsToStack[0:nc, :] 79 | print contacts 80 | 81 | newLimitPoint = pathIP.find_vertex_along_path(constraint_mode, contacts, comWF, desired_direction) 82 | 83 | for_iter += 1 84 | #print "for ",for_iter 85 | comTrajectoriesToStack = np.vstack([comTrajectoriesToStack, newLimitPoint]) 86 | optimizedVariablesToStack = np.vstack([optimizedVariablesToStack, np.array([LH_x, LH_y, dir_y])]) 87 | 88 | print "Final CoM points ", comTrajectoriesToStack 89 | print "Tested combinations: ", optimizedVariablesToStack 90 | 91 | max_motion_indices = np.unravel_index(np.argsort(comTrajectoriesToStack[:,0], axis=None), comTrajectoriesToStack[:,0].shape) 92 | max_motin_index = max_motion_indices[0][0] 93 | print("Directed Iterative Projection: --- %s seconds ---" % (time.time() - start_t_IPVC)) 94 | 95 | 96 | ''' plotting Iterative Projection points ''' 97 | 98 | axisZ= array([[0.0], [0.0], [1.0]]) 99 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 100 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 101 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 102 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 103 | # %% Cell 2 104 | 105 | normals = np.vstack([n1, n2, n3, n4]) 106 | IP_points, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(constraint_mode, contacts, normals, trunk_mass, ng, mu) 107 | 108 | #IP_points_saturated_friction, actuation_polygons = compDyn.iterative_projection_bretl('ONLY_FRICTION', contacts, normals, trunk_mass, ng, mu, saturate_normal_force = True) 109 | 110 | '''Plotting''' 111 | plt.close('all') 112 | plotter = Plotter() 113 | 114 | plt.figure() 115 | plt.grid() 116 | plt.xlabel("X [m]") 117 | plt.ylabel("Y [m]") 118 | 119 | plt.plot(comTrajectoriesToStack[:,0], comTrajectoriesToStack[:,1], 'g^', markersize=20) 120 | plt.plot(comTrajectoriesToStack[max_motin_index,0], comTrajectoriesToStack[max_motin_index,1], 'y^', markersize=25) 121 | h1 = plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15, label='feet') 122 | 123 | plt.plot(optimizedVariablesToStack[:,0], optimizedVariablesToStack[:,1], 'ko', markersize = 15) 124 | plt.plot(optimizedVariablesToStack[max_motin_index,0], optimizedVariablesToStack[max_motin_index,1], 'yo', markersize=25) 125 | plt.xlim(-0.9, 0.5) 126 | plt.ylim(-0.7, 0.7) 127 | plt.legend() 128 | plt.show() 129 | 130 | -------------------------------------------------------------------------------- /jet_leg/robots/anymal/anymal_kinematics.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | import pinocchio 4 | from pinocchio.robot_wrapper import RobotWrapper 5 | from pinocchio.utils import * 6 | 7 | 8 | class anymalKinematics(): 9 | def __init__(self): 10 | self.PKG = os.path.dirname(os.path.abspath(__file__)) 11 | self.URDF = self.PKG + '/urdf/anymal_boxy.urdf' 12 | if self.PKG is None: 13 | self.robot = RobotWrapper.BuildFromURDF(self.URDF) 14 | else: 15 | self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG]) 16 | self.robot = RobotWrapper.BuildFromURDF(self.URDF, [self.PKG]) 17 | self.model = self.robot.model 18 | self.LF_foot_jac = None 19 | self.LH_foot_jac = None 20 | self.RF_foot_jac = None 21 | self.RH_foot_jac = None 22 | self.urdf_foot_name_lf = 'LF_FOOT' 23 | self.urdf_foot_name_lh = 'LH_FOOT' 24 | self.urdf_foot_name_rf = 'RF_FOOT' 25 | self.urdf_foot_name_rh = 'RH_FOOT' 26 | self.ik_success = False 27 | 28 | def getBlockIndex(self, frame_name): 29 | if frame_name == self.urdf_foot_name_lf: 30 | idx = 0 31 | elif frame_name == self.urdf_foot_name_lh: 32 | idx = 3 33 | elif frame_name == self.urdf_foot_name_rf: 34 | idx = 6 35 | elif frame_name == self.urdf_foot_name_rh: 36 | idx = 9 37 | 38 | return idx 39 | 40 | def footInverseKinematicsFixedBase(self, foot_pos_des, frame_name): 41 | frame_id = self.model.getFrameId(frame_name) 42 | blockIdx = self.getBlockIndex(frame_name) 43 | anymal_q0 = pinocchio.neutral(self.model) 44 | data = self.model.createData() 45 | q = anymal_q0 46 | eps = 0.005 47 | IT_MAX = 200 48 | DT = 1e-1 49 | err = np.zeros((3, 1)) 50 | e = np.zeros((3, 1)) 51 | 52 | i = 0 53 | while True: 54 | pinocchio.forwardKinematics(self.model, data, q) 55 | pinocchio.framesForwardKinematics(self.model, data, q) 56 | foot_pos = data.oMf[frame_id].translation 57 | e = foot_pos - foot_pos_des 58 | 59 | pinocchio.computeJointJacobians(self.model, data, q) # compute jacobians 60 | J = pinocchio.getFrameJacobian(self.model, data, frame_id, pinocchio.LOCAL_WORLD_ALIGNED) 61 | J_lin = J[:3, :] 62 | 63 | if np.linalg.norm(e) < eps: 64 | IKsuccess = True 65 | break 66 | if i >= IT_MAX: 67 | print( 68 | "\n Warning: the iterative algorithm has not reached convergence to the desired precision. Error is: ", 69 | np.linalg.norm(e)) 70 | IKsuccess = False 71 | break 72 | v = - np.linalg.pinv(J_lin) @ e 73 | q = pinocchio.integrate(self.model, q, v * DT) 74 | i += 1 75 | 76 | q_leg = q[blockIdx:blockIdx + 3] 77 | J_leg = J_lin[:, blockIdx:blockIdx + 3] 78 | return q_leg, J_leg, err, IKsuccess 79 | 80 | def fixedBaseInverseKinematics(self, feetPosDes): 81 | 82 | self.LF_foot_jac = [] 83 | self.LH_foot_jac = [] 84 | self.RF_foot_jac = [] 85 | self.RH_foot_jac = [] 86 | leg_ik_success = np.zeros((4)) 87 | 88 | f_p_des = np.array(feetPosDes[0, :]).T 89 | q_LF, self.LF_foot_jac, err, leg_ik_success[0] = self.footInverseKinematicsFixedBase(f_p_des, 90 | self.urdf_foot_name_lf) 91 | 92 | f_p_des = np.array(feetPosDes[2, :]).T 93 | q_LH, self.LH_foot_jac, err, leg_ik_success[2] = self.footInverseKinematicsFixedBase(f_p_des, 94 | self.urdf_foot_name_lh) 95 | 96 | f_p_des = np.array(feetPosDes[1, :]).T 97 | q_RF, self.RF_foot_jac, err, leg_ik_success[1] = self.footInverseKinematicsFixedBase(f_p_des, 98 | self.urdf_foot_name_rf) 99 | 100 | f_p_des = np.array(feetPosDes[3, :]).T 101 | q_RH, self.RH_foot_jac, err, leg_ik_success[3] = self.footInverseKinematicsFixedBase(f_p_des, 102 | self.urdf_foot_name_rh) 103 | print("success", leg_ik_success) 104 | self.ik_success = bool(leg_ik_success[0] and leg_ik_success[1] and leg_ik_success[2] and leg_ik_success[3]) 105 | 106 | if self.ik_success is False: 107 | print('Warning, IK failed. Jacobian is singular') 108 | 109 | '''please NOTICE here the alphabetical order of the legs in the vector q: LF -> LH -> RF -> RH ''' 110 | q = np.vstack([q_LF, q_LH, q_RF, q_RH]) 111 | 112 | return q 113 | 114 | def getLegJacobians(self): 115 | isOutOfWS = not self.ik_success 116 | return self.LF_foot_jac, self.RF_foot_jac, self.LH_foot_jac, self.RH_foot_jac, isOutOfWS 117 | -------------------------------------------------------------------------------- /examples/iterative_projection/pymanoid_ladder_climbing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # 4 | # Copyright (C) 2018 Stephane Caron 5 | # 6 | # This file is part of jet-leg . 7 | # 8 | # jet-leg is free software: you can redistribute it and/or modify it under the 9 | # terms of the GNU Lesser General Public License as published by the Free 10 | # Software Foundation, either version 3 of the License, or (at your option) any 11 | # later version. 12 | # 13 | # jet-leg is distributed in the hope that it will be useful, but WITHOUT ANY 14 | # WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR 15 | # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 16 | # details. 17 | # 18 | # You should have received a copy of the GNU Lesser General Public License 19 | # along with jet-leg. If not, see . 20 | 21 | import IPython 22 | 23 | from numpy import array 24 | 25 | import pymanoid 26 | 27 | from pymanoid import Stance 28 | from pymanoid.gui import draw_horizontal_polygon 29 | 30 | from pymanoid_common import ActuationDependentArea 31 | from pymanoid_common import compute_geom_reachable_polygon 32 | from pymanoid_common import set_torque_limits 33 | 34 | 35 | if __name__ == "__main__": 36 | sim = pymanoid.Simulation(dt=0.03) 37 | robot = pymanoid.robots.JVRC1() 38 | set_torque_limits(robot) 39 | robot.tau_max *= 0.7 40 | robot.show_com() 41 | 42 | __shape = (0.01, 0.2) 43 | rungs = [ 44 | pymanoid.Contact(shape=__shape, pos=[0.2, 0., z], rpy=[0., -0.6, 0.]) 45 | for z in [0., 0.3, 0.6, 0.9, 1.2, 1.5, 1.8]] 46 | 47 | q_max = robot.q_max.copy() 48 | q_max[robot.CHEST_P] = 0 49 | q_max[robot.ROT_P] = 0.5 50 | robot.set_dof_limits(robot.q_min, q_max) 51 | 52 | q_fingers = array([+1., -1.5, -1.5, +0.1, -0.5, -0.5]) 53 | robot.set_dof_values( 54 | q_fingers, 55 | [robot.L_LTHUMB, robot.L_LINDEX, robot.L_LLITTLE, robot.L_UTHUMB, 56 | robot.L_UINDEX, robot.L_ULITTLE]) 57 | robot.set_dof_values( 58 | -q_fingers, 59 | [robot.R_LTHUMB, robot.R_LINDEX, robot.R_LLITTLE, robot.R_UTHUMB, 60 | robot.R_UINDEX, robot.R_ULITTLE]) 61 | 62 | sim.set_viewer() 63 | sim.set_camera_transform(array([ 64 | [-0.75318301, -0.33670976, 0.56510344, -1.27825475], 65 | [-0.65389426, 0.28962469, -0.69895625, 1.3535881], 66 | [0.07167748, -0.89595987, -0.43831297, 1.87996328], 67 | [0., 0., 0., 1.]])) 68 | robot.set_transparency(0.25) 69 | 70 | stance = Stance.from_json('jvrc1_ladder_climbing.json') 71 | stance.bind(robot) 72 | 73 | robot.ik.maximize_margins = True 74 | robot.ik.verbosity = 0 75 | robot.ik.solve(impr_stop=1e-3) 76 | 77 | from pymanoid.tasks import AxisAngleContactTask 78 | if False: 79 | del robot.ik.tasks['left_hand_palm'] 80 | del robot.ik.tasks['right_hand_palm'] 81 | lh_task = AxisAngleContactTask( 82 | robot, robot.left_hand, stance.left_hand, weight=1, gain=0.8) 83 | rh_task = AxisAngleContactTask( 84 | robot, robot.right_hand, stance.right_hand, weight=1, gain=0.8) 85 | lh_task.doc_mask = array([1., 1., 1., 1., 0.1, 1.]) 86 | rh_task.doc_mask = array([1., 1., 1., 1., 0.1, 1.]) 87 | robot.ik.add(lh_task) 88 | robot.ik.add(rh_task) 89 | if False: 90 | del robot.ik.tasks['left_foot_base'] 91 | del robot.ik.tasks['right_foot_base'] 92 | lf_task = AxisAngleContactTask( 93 | robot, robot.left_foot, stance.left_foot, weight=1, gain=0.8) 94 | rf_task = AxisAngleContactTask( 95 | robot, robot.right_foot, stance.right_foot, weight=1, gain=0.8) 96 | lf_task.doc_mask = array([1., 1., 1., 1., 0.1, 1.]) 97 | rf_task.doc_mask = array([1., 1., 1., 1., 0.1, 1.]) 98 | robot.ik.add(lf_task) 99 | robot.ik.add(rf_task) 100 | 101 | uncons_polygon = stance.compute_static_equilibrium_polygon(method="bretl") 102 | 103 | CHECK_FULL_GEOM = False 104 | if CHECK_FULL_GEOM: 105 | geom_polygon = compute_geom_reachable_polygon( 106 | robot, stance, xlim=(0.0, 0.2), ylim=(-0.2, 0.2),) 107 | else: 108 | x_min, x_max = -0.2, -0.15 109 | y_min, y_max = -0.15, 0.15 110 | geom_polygon = [ 111 | (x_min, y_min), (x_max, y_min), (x_max, y_max), (x_min, y_max)] 112 | 113 | polygon_height = stance.com.z 114 | h1 = draw_horizontal_polygon(uncons_polygon, polygon_height, color='g') 115 | h2 = draw_horizontal_polygon(geom_polygon, polygon_height, color='m') 116 | 117 | sim.schedule(robot.ik) 118 | # sim.schedule_extra(wrench_drawer) 119 | sim.start() 120 | 121 | ada = ActuationDependentArea(robot, stance) 122 | ada.sample_working_set(geom_polygon, "sample", 5) 123 | 124 | DRAW_VOLUME = True 125 | if DRAW_VOLUME: 126 | h3 = ada.draw_volume( 127 | min_height=0.7, max_height=0.88, dh=0.02, hull=True) 128 | else: # not DRAW_VOLUME 129 | h3 = ada.draw_at_height(polygon_height) 130 | 131 | if IPython.get_ipython() is None: 132 | IPython.embed() 133 | -------------------------------------------------------------------------------- /unit_tests/test_pyipopt.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri Aug 3 07:01:19 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | # -*- coding: utf-8 -*- 9 | """ 10 | Created on Fri Aug 3 06:55:23 2018 11 | 12 | @author: rorsolino 13 | """ 14 | 15 | # -*- coding: utf-8 -*- 16 | """ 17 | cyipot: Python wrapper for the Ipopt optimization package, written in Cython. 18 | 19 | Copyright (C) 2012 Amit Aides 20 | Author: Amit Aides 21 | URL: 22 | License: EPL 1.0 23 | """ 24 | # 25 | # Test the "ipopt" Python interface on the Hock & Schittkowski test problem 26 | # #71. See: Willi Hock and Klaus Schittkowski. (1981) Test Examples for 27 | # Nonlinear Programming Codes. Lecture Notes in Economics and Mathematical 28 | # Systems Vol. 187, Springer-Verlag. 29 | # 30 | # Based on matlab code by Peter Carbonetto. 31 | # 32 | 33 | import numpy as np 34 | import scipy.sparse as sps 35 | import ipopt 36 | 37 | 38 | class hs071(object): 39 | def __init__(self): 40 | pass 41 | 42 | def objective(self, x): 43 | # 44 | # The callback for calculating the objective 45 | # 46 | return x[0] * x[3] * np.sum(x[0:3]) + x[2] 47 | 48 | def gradient(self, x): 49 | # 50 | # The callback for calculating the gradient 51 | # 52 | return np.array([ 53 | x[0] * x[3] + x[3] * np.sum(x[0:3]), 54 | x[0] * x[3], 55 | x[0] * x[3] + 1.0, 56 | x[0] * np.sum(x[0:3]) 57 | ]) 58 | 59 | def constraints(self, x): 60 | # 61 | # The callback for calculating the constraints 62 | # 63 | return np.array((np.prod(x), np.dot(x, x))) 64 | 65 | def jacobian(self, x): 66 | # 67 | # The callback for calculating the Jacobian 68 | # 69 | #return np.concatenate((np.prod(x) / x, 2*x)) 70 | return False 71 | 72 | def hessianstructure(self): 73 | # 74 | # The structure of the Hessian 75 | # Note: 76 | # The default hessian structure is of a lower triangular matrix. Therefore 77 | # this function is redundant. I include it as an example for structure 78 | # callback. 79 | # 80 | global hs 81 | 82 | hs = sps.coo_matrix(np.tril(np.ones((4, 4)))) 83 | return (hs.col, hs.row) 84 | 85 | def hessian(self, x, lagrange, obj_factor): 86 | # 87 | # The callback for calculating the Hessian 88 | # 89 | H = obj_factor*np.array(( 90 | (2*x[3], 0, 0, 0), 91 | (x[3], 0, 0, 0), 92 | (x[3], 0, 0, 0), 93 | (2*x[0]+x[1]+x[2], x[0], x[0], 0))) 94 | 95 | H += lagrange[0]*np.array(( 96 | (0, 0, 0, 0), 97 | (x[2]*x[3], 0, 0, 0), 98 | (x[1]*x[3], x[0]*x[3], 0, 0), 99 | (x[1]*x[2], x[0]*x[2], x[0]*x[1], 0))) 100 | 101 | H += lagrange[1]*2*np.eye(4) 102 | 103 | # 104 | # Note: 105 | # 106 | # 107 | return H[hs.row, hs.col] 108 | 109 | def intermediate( 110 | self, 111 | alg_mod, 112 | iter_count, 113 | obj_value, 114 | inf_pr, 115 | inf_du, 116 | mu, 117 | d_norm, 118 | regularization_size, 119 | alpha_du, 120 | alpha_pr, 121 | ls_trials 122 | ): 123 | 124 | # 125 | # Example for the use of the intermediate callback. 126 | # 127 | print "Objective value at iteration #%d is - %g" % (iter_count, obj_value) 128 | 129 | 130 | def main(): 131 | # 132 | # Define the problem 133 | # 134 | x0 = [1.0, 5.0, 5.0, 1.0] 135 | 136 | lb = [1.0, 1.0, 1.0, 1.0] 137 | ub = [5.0, 5.0, 5.0, 5.0] 138 | 139 | cl = [25.0, 40.0] 140 | cu = [2.0e19, 40.0] 141 | 142 | nlp = ipopt.problem( 143 | n=len(x0), 144 | m=len(cl), 145 | problem_obj=hs071(), 146 | lb=lb, 147 | ub=ub, 148 | cl=cl, 149 | cu=cu 150 | ) 151 | 152 | # 153 | # Set solver options 154 | # 155 | #nlp.addOption('derivative_test', 'second-order') 156 | nlp.addOption('mu_strategy', 'adaptive') 157 | nlp.addOption('jacobian_approximation', 'finite-difference-values') 158 | nlp.addOption('hessian_approximation', 'limited-memory') 159 | nlp.addOption('tol', 1e-7) 160 | 161 | # 162 | # Scale the problem (Just for demonstration purposes) 163 | # 164 | nlp.setProblemScaling( 165 | obj_scaling=2, 166 | x_scaling=[1, 1, 1, 1] 167 | ) 168 | nlp.addOption('nlp_scaling_method', 'user-scaling') 169 | 170 | # 171 | # Solve the problem 172 | # 173 | x, info = nlp.solve(x0) 174 | 175 | print "Solution of the primal variables: x=%s\n" % repr(x) 176 | 177 | print "Solution of the dual variables: lambda=%s\n" % repr(info['mult_g']) 178 | 179 | print "Objective=%s\n" % repr(info['obj_val']) 180 | 181 | 182 | if __name__ == '__main__': 183 | main() 184 | -------------------------------------------------------------------------------- /examples/foothold_planning/foothold_planning_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Oct 25 12:06:27 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import numpy as np 9 | from context import jet_leg 10 | import time 11 | from numpy import array 12 | 13 | import matplotlib.pyplot as plt 14 | from jet_leg.plotting_tools import Plotter 15 | from jet_leg.math_tools import Math 16 | from jet_leg.computational_dynamics import ComputationalDynamics 17 | from jet_leg.height_map import HeightMap 18 | 19 | from jet_leg.path_sequential_iterative_projection import PathIterativeProjection 20 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 21 | 22 | 23 | ''' MAIN ''' 24 | 25 | plt.close('all') 26 | start_t_IPVC = time.time() 27 | 28 | math = Math() 29 | compDyn = ComputationalDynamics() 30 | pathIP = PathIterativeProjection() 31 | # number of contacts 32 | number_of_contacts = 3 33 | 34 | # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION 35 | constraint_mode = ['ONLY_FRICTION', 36 | 'ONLY_FRICTION', 37 | 'ONLY_FRICTION', 38 | 'ONLY_FRICTION'] 39 | useVariableJacobian = True 40 | trunk_mass = 100 41 | mu = 0.8 42 | 43 | comTrajectoriesToStack = np.zeros((0,3)) 44 | terrain = HeightMap() 45 | comWF = np.array([0.1, 0.1, 0.0]) 46 | optimizedVariablesToStack = np.zeros((0,3)) 47 | iterProj = PathIterativeProjection() 48 | 49 | LF_tau_lim = [50.0, 100.0, 100.0] 50 | RF_tau_lim = [50.0, 100.0, 100.0] 51 | LH_tau_lim = [50.0, 100.0, 100.0] 52 | RH_tau_lim = [50.0, 100.0, 100.0] 53 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 54 | 55 | axisZ= array([[0.0], [0.0], [1.0]]) 56 | math = Math() 57 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 58 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 59 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 60 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 61 | # %% Cell 2 62 | normals = np.vstack([n1, n2, n3, n4]) 63 | ng = 4 64 | tolerance = 0.005 65 | 66 | params = IterativeProjectionParameters() 67 | params.setCoMPosWF(comWF) 68 | params.setTorqueLims(torque_limits) 69 | params.setConstraintModes(constraint_mode) 70 | params.setContactNormals(normals) 71 | params.setFrictionCoefficient(mu) 72 | params.setNumberOfFrictionConesEdges(ng) 73 | params.setTotalMass(trunk_mass) 74 | 75 | 76 | for LH_x in np.arange(-0.7,-0.2, 0.1): 77 | for LH_y in np.arange(-0.1,0.3, 0.1): 78 | angle = np.random.normal(np.pi, 0.1) 79 | desired_direction = np.array([np.cos(angle), np.sin(angle), 0.0]) 80 | 81 | """ contact points """ 82 | LF_foot = np.array([0.4, 0.3, -0.5]) 83 | RF_foot = np.array([0.4, -0.3, -0.5]) 84 | terrainHeight = terrain.get_height(LH_x, LH_y) 85 | LH_foot = np.array([LH_x, LH_y, terrainHeight-0.5]) 86 | RH_foot = np.array([-0.3, -0.2, -0.5]) 87 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 88 | 89 | stanceLegs = [1,1,1,1] 90 | stanceIndex = [] 91 | swingIndex = [] 92 | print 'stance', stanceLegs 93 | for iter in range(0, 4): 94 | if stanceLegs[iter] == 1: 95 | # print 'new poly', stanceIndex, iter 96 | stanceIndex = np.hstack([stanceIndex, iter]) 97 | else: 98 | swingIndex = iter 99 | 100 | print 'new search angle: ', angle 101 | desired_direction = np.array([np.cos(angle), np.sin(angle), 0.0]) 102 | 103 | params.setContactsPosWF(contacts) 104 | params.setActiveContacts(stanceLegs) 105 | newLimitPoint, stackedErrors, stackedPolygons = pathIP.find_vertex_along_path(params, desired_direction,tolerance) 106 | comTrajectoriesToStack = np.vstack([comTrajectoriesToStack, newLimitPoint]) 107 | optimizedVariablesToStack = np.vstack([optimizedVariablesToStack, np.array([LH_x, LH_y, angle])]) 108 | 109 | 110 | print 'Errors convergence: ', stackedErrors 111 | 112 | print("Actuation Region estimation time: --- %s seconds ---" % (time.time() - start_t_IPVC)) 113 | 114 | max_motion_indices = np.unravel_index(np.argsort(comTrajectoriesToStack[:,0], axis=None), comTrajectoriesToStack[:,0].shape) 115 | max_motin_index = max_motion_indices[0][0] 116 | 117 | '''Plotting''' 118 | plt.figure() 119 | plt.grid() 120 | plt.xlabel("X [m]") 121 | plt.ylabel("Y [m]") 122 | 123 | plt.plot(comTrajectoriesToStack[:,0], comTrajectoriesToStack[:,1], 'g^', markersize=20, label= 'Actuation region vertices') 124 | plt.plot(comWF[0], comWF[1], 'ro', markersize=20, label= 'Initial CoM position used in the SIP alg.') 125 | segment = np.vstack([comWF,newLimitPoint]) 126 | plt.plot(contacts[0:number_of_contacts,0],contacts[0:number_of_contacts,1],'ko',markersize=15, label='Stance feet') 127 | plt.plot(comTrajectoriesToStack[max_motin_index,0], comTrajectoriesToStack[max_motin_index,1], 'y^', markersize=25, label= 'furthest vertex along the given search direction') 128 | plt.plot(optimizedVariablesToStack[:,0], optimizedVariablesToStack[:,1], 'bo', markersize = 15, label = 'tested stance feet') 129 | #plt.plot(optimizedVariablesToStack[max_motin_index,0], optimizedVariablesToStack[max_motin_index,1], 'yo', markersize=20, label='selected foothold') 130 | 131 | plt.xlim(-0.9, 0.5) 132 | plt.ylim(-0.7, 0.7) 133 | plt.legend() 134 | plt.show() 135 | 136 | -------------------------------------------------------------------------------- /examples/feasible_region/four_generators_four_contacts.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Apr 18 14:15:45 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import pylab 9 | pylab.close("all") 10 | import pypoman 11 | import numpy as np 12 | from numpy import array, eye, ones, vstack, zeros 13 | 14 | 15 | def skew(v): 16 | if len(v) == 4: v = v[:3]/v[3] 17 | skv = np.roll(np.roll(np.diag(v.flatten()), 1, 1), -1, 0) 18 | return skv - skv.T 19 | 20 | def getGraspMatrix(r): 21 | G = np.block([[np.eye(3), zeros((3,3))], 22 | [skew(r), np.eye(3)]]) 23 | return G 24 | 25 | # number of contacts 26 | nc = 4 27 | # number of decision variables of the problem 28 | # (3 contact forces for each contact + 2 com coordinates x and y) 29 | n = nc*3 30 | 31 | use_grasp_matrix = True 32 | 33 | # postions of the 4 contacts 34 | r1 = array([10.0, 10.0, 0.0]) 35 | r2 = array([10.0, -10.0, 0.0]) 36 | r3 = array([-10.0, -10.0, 0.0]) 37 | r4 = array([-10.0, 10.0, 0.0]) 38 | 39 | std_dev = np.sqrt(r1[0]*r1[0]+r1[1]*r1[1])/1000 40 | print "" 41 | print "standard dev:" 42 | print std_dev 43 | noise = np.random.normal(0,std_dev,16) 44 | # I set the com coordinates to be in the bottom of the state array: 45 | # x = [f1_x, f1_y, f1_z, f2_x, f2_y, f2_z, ... , f4_x, f4_y, f4_z, com_x, com_y] 46 | 47 | if use_grasp_matrix == 'True': 48 | G1 = getGraspMatrix(r1) 49 | G2 = getGraspMatrix(r2) 50 | G3 = getGraspMatrix(r3) 51 | G4 = getGraspMatrix(r4) 52 | Ex = np.hstack((G1[4,0:3],G2[4,0:3],G3[4,0:3],G3[4,0:3])) 53 | Ey = np.hstack((G1[3,0:3],G2[3,0:3],G3[3,0:3],G3[3,0:3])) 54 | E = vstack((Ex, Ey)) 55 | else: 56 | E = zeros((2, nc*3)) 57 | # tau_0x 58 | E[0, 1] = -r1[2]+noise[0] 59 | E[0, 2] = +r1[1]+noise[1] 60 | 61 | E[0, 4] = -r2[2]+noise[2] 62 | E[0, 5] = +r2[1]+noise[3] 63 | 64 | E[0, 7] = -r3[2]+noise[4] 65 | E[0, 8] = +r3[1]+noise[5] 66 | 67 | E[0, 10] = -r4[2]+noise[6] 68 | E[0, 11] = +r4[1]+noise[7] 69 | 70 | #tau_0y 71 | E[1, 0] = +r1[2]+noise[8] 72 | E[1, 2] = -r1[0]+noise[9] 73 | 74 | E[1, 3] = +r2[2]+noise[10] 75 | E[1, 5] = -r2[0]+noise[11] 76 | 77 | E[1, 6] = +r3[2]+noise[12] 78 | E[1, 8] = -r3[0]+noise[13] 79 | 80 | E[1, 9] = +r4[2]+noise[14] 81 | E[1, 11] = -r4[0]+noise[15] 82 | 83 | f = zeros(2) 84 | proj = (E, f) # y = E * x + f 85 | 86 | g = -9.81 87 | mass = 10. 88 | mu = 1. 89 | grav = array([[0.], [0.], [mass*g]]) 90 | 91 | # contact surface normals 92 | n1 = array([[0.0], [0.0], [1.0]]) 93 | n2 = array([[0.0], [0.0], [1.0]]) 94 | n3 = array([[0.0], [0.0], [1.0]]) 95 | n4 = array([[0.0], [0.0], [1.0]]) 96 | 97 | # projection matrix 98 | P = array([[1., 0., 0.], 99 | [0., 1., 0.]]) 100 | Pt = np.transpose(P) 101 | 102 | ## Definition of the equality constraints 103 | m_eq = 6 104 | A2 = vstack([zeros((3,2)), -np.dot(skew(grav),Pt)]) 105 | 106 | a1 = vstack([+eye(3), skew(r1)]) 107 | a2 = vstack([+eye(3), skew(r2)]) 108 | a3 = vstack([+eye(3), skew(r3)]) 109 | a4 = vstack([+eye(3), skew(r4)]) 110 | A1 = np.hstack((a1, a2, a3, a4)) 111 | 112 | #A = np.hstack((A1,A2)) 113 | A = A1 114 | t = vstack([-grav, zeros((3,1))]).reshape((6)) 115 | print A, t 116 | eq = (A, t) # A * x == t 117 | 118 | ## Definition of the inequality constraints 119 | n_generators = 4 120 | m_ineq = (n_generators+1)*nc 121 | u1 = np.hstack((np.dot(mu,n1),np.dot(mu,n1),np.dot(1.0,n1))) 122 | u2 = np.hstack((np.dot(mu,n2),np.dot(mu,n2),np.dot(1.0,n2))) 123 | u3 = np.hstack((np.dot(mu,n3),np.dot(mu,n3),np.dot(1.0,n3))) 124 | u4 = np.hstack((np.dot(mu,n4),np.dot(mu,n4),np.dot(1.0,n4))) 125 | U = np.block([[np.transpose(u1), zeros((3,3)), zeros((3,3)), zeros((3,3))], 126 | [zeros((3,3)), np.transpose(u2), zeros((3,3)), zeros((3,3))], 127 | [zeros((3,3)), zeros((3,3)), np.transpose(u3), zeros((3,3))], 128 | [zeros((3,3)), zeros((3,3)), zeros((3,3)), np.transpose(u4)]]) 129 | #print(u1) 130 | ### Setting up the linear inequality constraints 131 | ## Linearized friction cone: 132 | b1 = eye(3)-np.dot(n1,np.transpose(n1)) 133 | b2 = eye(3)-np.dot(n2,np.transpose(n2)) 134 | b3 = eye(3)-np.dot(n3,np.transpose(n3)) 135 | b4 = eye(3)-np.dot(n4,np.transpose(n4)) 136 | #print(b1) 137 | B = np.block([[b1, zeros((3,3)), zeros((3,3)), zeros((3,3))], 138 | [zeros((3,3)), b2, zeros((3,3)), zeros((3,3))], 139 | [zeros((3,3)), zeros((3,3)), b3, zeros((3,3))], 140 | [zeros((3,3)), zeros((3,3)), zeros((3,3)), b4]]) 141 | # fx <= mu*fz, fy <= mu*fz and fz > 0 142 | C1 = - B - U 143 | # fx >= -mu*fz and fy >= -mu*fz 144 | C2 = B - U 145 | c2a = C2[0:2,0:12] 146 | c2b = C2[3:5,0:12] 147 | c2c = C2[6:8,0:12] 148 | c2d = C2[9:11,0:12] 149 | C3 = vstack([c2a, c2b, c2c, c2d]) 150 | C = vstack([C1, C3]) 151 | print(C) 152 | 153 | b = zeros((m_ineq,1)).reshape(m_ineq) 154 | print(b) 155 | ineq = (C, b) # C * x <= b 156 | 157 | vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl') 158 | pylab.ion() 159 | pylab.figure() 160 | print(vertices) 161 | pypoman.plot_polygon(vertices) 162 | 163 | # Project Tau_0 into CoM coordinates as in Eq. 53 164 | # p_g = (n/mg) x tau_0 + z_g*n 165 | n = array([0., 0., 1./(mass*g)]) 166 | points = [] 167 | for j in range(0,len(vertices)): 168 | vx = vertices[j][0] 169 | vy = vertices[j][1] 170 | tau = array([vx, vy, 0.]) 171 | p = np.cross(n,tau) 172 | points.append([p[0],p[1]]) 173 | #print points 174 | 175 | print points 176 | pypoman.plot_polygon(points) -------------------------------------------------------------------------------- /examples/feasible_region/eight_generators_four_contacts.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Apr 18 14:15:45 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import pylab 9 | pylab.close("all") 10 | import pypoman 11 | import numpy as np 12 | from numpy import array, eye, ones, vstack, zeros 13 | 14 | 15 | def skew(v): 16 | if len(v) == 4: v = v[:3]/v[3] 17 | skv = np.roll(np.roll(np.diag(v.flatten()), 1, 1), -1, 0) 18 | return skv - skv.T 19 | 20 | # number of contacts 21 | nc = 4 22 | # number of decision variables of the problem 23 | # (3 contact forces for each contact + 2 com coordinates x and y) 24 | n = nc*3 25 | 26 | g = -9.81 27 | mass = 10. 28 | mu = 0.6 29 | grav = array([[0.], [0.], [mass*g]]) 30 | 31 | # postions of the 4 contacts (I am assuming them all to be point-contacts as we normally do for HyQ) 32 | r1 = array([1.0, 1.0, 0.]) 33 | r2 = array([1.0, -1.0, 0.]) 34 | r3 = array([-1.0, 1.0, 0.]) 35 | r4 = array([-1.0, -1.0, 0.]) 36 | 37 | std_dev = np.sqrt(r1[0]*r1[0]+r1[1]*r1[1])/1000 38 | print "" 39 | print "standard dev:" 40 | print std_dev 41 | noise = np.random.normal(0,std_dev,16) 42 | # I set the com coordinates to be in the bottom of the state array: 43 | # x = [f1_x, f1_y, f1_z, f2_x, f2_y, f2_z, ... , f4_x, f4_y, f4_z, com_x, com_y] 44 | E = zeros((2, n)) 45 | # tau_0x 46 | E[0, 1] = -r1[2]+noise[0] 47 | E[0, 2] = +r1[1]+noise[1] 48 | 49 | E[0, 4] = -r2[2]+noise[2] 50 | E[0, 5] = +r2[1]+noise[3] 51 | 52 | E[0, 7] = -r3[2]+noise[4] 53 | E[0, 8] = +r3[1]+noise[5] 54 | 55 | E[0, 10] = -r4[2]+noise[6] 56 | E[0, 11] = +r4[1]+noise[7] 57 | 58 | #tau_0y 59 | E[1, 0] = +r1[2]+noise[8] 60 | E[1, 2] = -r1[0]+noise[9] 61 | 62 | E[1, 3] = +r2[2]+noise[10] 63 | E[1, 5] = -r2[0]+noise[11] 64 | 65 | E[1, 6] = +r3[2]+noise[12] 66 | E[1, 8] = -r3[0]+noise[13] 67 | 68 | E[1, 9] = +r4[2]+noise[14] 69 | E[1, 11] = -r4[0]+noise[15] 70 | 71 | f = zeros(2) 72 | proj = (E, f) # y = E * x + f 73 | 74 | # contact surface normals 75 | n1 = array([[0.0], [0.0], [1.0]]) 76 | n2 = array([[0.0], [0.0], [1.0]]) 77 | n3 = array([[0.0], [0.0], [1.0]]) 78 | n4 = array([[0.0], [0.0], [1.0]]) 79 | 80 | # projection matrix 81 | P = array([[1., 0., 0.], 82 | [0., 1., 0.]]) 83 | Pt = np.transpose(P) 84 | 85 | ## Definition of the equality constraints 86 | m_eq = 6 87 | A2 = vstack([zeros((3,2)), -np.dot(skew(grav),Pt)]) 88 | 89 | a1 = vstack([+eye(3), skew(r1)]) 90 | a2 = vstack([+eye(3), skew(r2)]) 91 | a3 = vstack([+eye(3), skew(r3)]) 92 | a4 = vstack([+eye(3), skew(r4)]) 93 | A1 = np.hstack((a1, a2, a3, a4)) 94 | 95 | #A = np.hstack((A1,A2)) 96 | A = A1 97 | t = vstack([-grav, zeros((3,1))]).reshape((6)) 98 | eq = (A, t) # A * x == t 99 | 100 | ## Start defining the inequality constraints 101 | n_generators = 8 102 | m_ineq = 28 #missing constraints here. m_ineq should actually be (n_generators+1)*nc = 36 103 | 104 | u1 = np.hstack((np.dot(mu,n1),np.dot(mu,n1),np.dot(1.0,n1))) 105 | u2 = np.hstack((np.dot(mu,n2),np.dot(mu,n2),np.dot(1.0,n2))) 106 | u3 = np.hstack((np.dot(mu,n3),np.dot(mu,n3),np.dot(1.0,n3))) 107 | u4 = np.hstack((np.dot(mu,n4),np.dot(mu,n4),np.dot(1.0,n4))) 108 | U = np.block([[np.transpose(u1), zeros((3,3)), zeros((3,3)), zeros((3,3))], 109 | [zeros((3,3)), np.transpose(u2), zeros((3,3)), zeros((3,3))], 110 | [zeros((3,3)), zeros((3,3)), np.transpose(u3), zeros((3,3))], 111 | [zeros((3,3)), zeros((3,3)), zeros((3,3)), np.transpose(u4)]]) 112 | ### Setting up the linear constraints 113 | ## Linearized friction cone: 114 | b1 = eye(3)-np.dot(n1,np.transpose(n1)) 115 | b2 = eye(3)-np.dot(n2,np.transpose(n2)) 116 | b3 = eye(3)-np.dot(n3,np.transpose(n3)) 117 | b4 = eye(3)-np.dot(n4,np.transpose(n4)) 118 | #print(b1) 119 | B = np.block([[b1, zeros((3,3)), zeros((3,3)), zeros((3,3))], 120 | [zeros((3,3)), b2, zeros((3,3)), zeros((3,3))], 121 | [zeros((3,3)), zeros((3,3)), b3, zeros((3,3))], 122 | [zeros((3,3)), zeros((3,3)), zeros((3,3)), b4]]) 123 | # fx <= mu*fz, fy <= mu*fz and fz > 0 124 | C1 = - B - U 125 | # fx >= -mu*fz and fy >= -mu*fz 126 | C_tmp = B - U 127 | c2a = C_tmp[0:2,0:12] 128 | c2b = C_tmp[3:5,0:12] 129 | c2c = C_tmp[6:8,0:12] 130 | c2d = C_tmp[9:11,0:12] 131 | C2 = vstack([c2a, c2b, c2c, c2d]) 132 | # 0.75*fx + 0.75*fy <= mu*fz 133 | C3 = array([[0.75, 0.75, -mu*1., 0., 0., 0., 0., 0., 0., 0., 0., 0.], 134 | [0., 0., 0., 0.75, 0.75, -mu*1., 0., 0., 0., 0., 0., 0.], 135 | [0., 0., 0., 0., 0., 0., 0.75, 0.75, -mu*1., 0., 0., 0.], 136 | [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.75, 0.75, -mu*1.]]) 137 | # 0.75*fx + 0.75*fy >= -mu*fz 138 | C4 = array([[-0.75, -0.75, -mu*1., 0., 0., 0., 0., 0., 0., 0., 0., 0.], 139 | [0., 0., 0., -0.75, -0.75, -mu*1., 0., 0., 0., 0., 0., 0.], 140 | [0., 0., 0., 0., 0., 0., -0.75, -0.75, -mu*1., 0., 0., 0.], 141 | [0., 0., 0., 0., 0., 0., 0., 0., 0., -0.75, -0.75, -mu*1.]]) 142 | # concatenate all the inequality constraints above 143 | C = vstack([C1, C2, C3, C4]) 144 | 145 | print(C) 146 | 147 | # vector of known coefficients 148 | b = zeros((m_ineq,1)).reshape(m_ineq) 149 | print(b) 150 | ineq = (C, b) # C * x <= b 151 | 152 | # Solve the projection problem 153 | vertices = pypoman.project_polytope(proj, ineq, eq, method='bretl') 154 | pylab.ion() 155 | pylab.figure() 156 | print(vertices) 157 | pypoman.plot_polygon(vertices) 158 | # Project Tau_0 into CoM coordinates as in Eq. 53 159 | # p_g = (n/mg) x tau_0 + z_g*n 160 | n = array([0., 0., 1./(mass*g)]) 161 | points = [] 162 | for j in range(0,len(vertices)): 163 | vx = vertices[j][0] 164 | vy = vertices[j][1] 165 | tau = array([vx, vy, 0.]) 166 | p = np.cross(n,tau) 167 | points.append([p[0],p[1]]) 168 | #print points 169 | 170 | print points 171 | pypoman.plot_polygon(points) -------------------------------------------------------------------------------- /examples/iterative_projection/sequentialIterativeProjection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 4 16:18:52 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | import numpy as np 9 | from context import jet_leg 10 | import time 11 | 12 | import cvxopt 13 | from cvxopt import matrix, solvers 14 | 15 | from numpy import array, cos, cross, pi, sin 16 | from numpy.random import random 17 | from scipy.linalg import norm 18 | from scipy.linalg import block_diag 19 | 20 | from numpy import array, cross, dot, eye, hstack, vstack, zeros, matrix 21 | 22 | import matplotlib.pyplot as plt 23 | from jet_leg.plotting_tools import Plotter 24 | from jet_leg.constraints import Constraints 25 | from jet_leg.hyq_kinematics import HyQKinematics 26 | from jet_leg.math_tools import Math 27 | from jet_leg.computational_dynamics import ComputationalDynamics 28 | from jet_leg.vertex_based_projection import VertexBasedProjection 29 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 30 | 31 | ''' MAIN ''' 32 | start_t_IPVC = time.time() 33 | math = Math() 34 | compDyn = ComputationalDynamics() 35 | # number of generators, i.e. rays used to linearize the friction cone 36 | ng = 4 37 | 38 | # ONLY_ACTUATION or ONLY_FRICTION or FRICTION_AND_ACTUATION 39 | constraint_mode_IP = ['ONLY_ACTUATION', 40 | 'ONLY_ACTUATION', 41 | 'ONLY_ACTUATION', 42 | 'ONLY_ACTUATION'] 43 | useVariableJacobian = True 44 | g = 9.81 45 | trunk_mass = 85. 46 | mu = 0.9 47 | 48 | 49 | """ contact points """ 50 | LF_foot = np.array([0.3, 0.2, -0.5]) 51 | RF_foot = np.array([0.3, -0.2, -0.5]) 52 | LH_foot = np.array([-0.3, 0.2, -0.5]) 53 | RH_foot = np.array([-0.3, -0.2, -0.5]) 54 | 55 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 56 | stanceFeet = [1,1,1,1] 57 | nc = np.sum(stanceFeet) 58 | comWF = np.array([0.0, 0.0, 0.0]) 59 | 60 | ''' plotting Iterative Projection points ''' 61 | 62 | axisZ= array([[0.0], [0.0], [1.0]]) 63 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 64 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 65 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 66 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 67 | # %% Cell 2 68 | 69 | normals = np.vstack([n1, n2, n3, n4]) 70 | 71 | LF_tau_lim = [50.0, 100.0, 100.0] 72 | RF_tau_lim = [50.0, 100.0, 100.0] 73 | LH_tau_lim = [50.0, 100.0, 100.0] 74 | RH_tau_lim = [50.0, 100.0, 100.0] 75 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 76 | #comWF = np.array([0.0,0.0,0.0]) 77 | params = IterativeProjectionParameters() 78 | params.setContactsPos(contacts) 79 | params.setCoMPos(comWF) 80 | params.setTorqueLims(torque_limits) 81 | params.setActiveContacts(stanceFeet) 82 | params.setConstraintModes(constraint_mode_IP) 83 | params.setContactNormals(normals) 84 | params.setFrictionCoefficient(mu) 85 | params.setNumberOfFrictionConesEdges(ng) 86 | params.setTrunkMass(trunk_mass) 87 | 88 | feasible, unfeasible, contact_forces = compDyn.LP_projection(params, useVariableJacobian, 0.2, 0.2) 89 | print feasible 90 | print unfeasible 91 | #desired_direction = [-1.0, -1.0] 92 | 93 | IP_points1, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 94 | 95 | comWF = np.array([-0.205, 0.1, 0.0]) 96 | params.setCoMPos(comWF) 97 | IP_points2, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 98 | 99 | comWF = np.array([-0.131, 0.1, 0.0]) 100 | params.setCoMPos(comWF) 101 | IP_points3, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 102 | 103 | comWF = np.array([-0.17, 0.1, 0.0]) 104 | params.setCoMPos(comWF) 105 | IP_points4, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 106 | 107 | comWF = np.array([-0.18, 0.1, 0.0]) 108 | params.setCoMPos(comWF) 109 | IP_points5, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 110 | 111 | comWF = np.array([-0.19, 0.1, 0.0]) 112 | params.setCoMPos(comWF) 113 | IP_points6, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(params) 114 | #IP_points_saturated_friction, actuation_polygons = compDyn.iterative_projection_bretl('ONLY_FRICTION', contacts, normals, trunk_mass, ng, mu, saturate_normal_force = True) 115 | 116 | '''Plotting''' 117 | plt.close('all') 118 | plotter = Plotter() 119 | 120 | plt.figure() 121 | plt.grid() 122 | plt.xlabel("X [m]") 123 | plt.ylabel("Y [m]") 124 | contacts = params.getContactsPos() 125 | print nc, contacts 126 | h1 = plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15, label='feet') 127 | 128 | plotter.plot_polygon(np.transpose(IP_points1), color = 'b') 129 | plotter.plot_polygon(np.transpose(IP_points2), color = 'g') 130 | plotter.plot_polygon(np.transpose(IP_points3), color = 'r') 131 | plotter.plot_polygon(np.transpose(IP_points4), color = 'y') 132 | plotter.plot_polygon(np.transpose(IP_points5), color = 'k') 133 | plotter.plot_polygon(np.transpose(IP_points6), color = 'b') 134 | #plotter.plot_polygon(np.transpose(IP_points_saturated_friction), color = '--r') 135 | 136 | feasiblePointsSize = np.size(feasible,0) 137 | for j in range(0, feasiblePointsSize): 138 | if (feasible[j,2]<0.01)&(feasible[j,2]>-0.01): 139 | plt.scatter(feasible[j,0], feasible[j,1],c='g',s=50) 140 | lastFeasibleIndex = j 141 | unfeasiblePointsSize = np.size(unfeasible,0) 142 | print unfeasiblePointsSize 143 | for j in range(0, unfeasiblePointsSize): 144 | if (unfeasible[j,2]<0.01)&(unfeasible[j,2]>-0.01): 145 | plt.scatter(unfeasible[j,0], unfeasible[j,1],c='r',s=50) 146 | lastUnfeasibleIndex = j 147 | #h2 = plt.scatter(feasible[lastFeasibleIndex,0], feasible[lastFeasibleIndex,1],c='g',s=50, label='LP feasible') 148 | #h3 = plt.scatter(unfeasible[lastUnfeasibleIndex,0], unfeasible[lastUnfeasibleIndex,1],c='r',s=50, label='LP unfeasible') 149 | 150 | # 151 | #print "final vertices: ", vx 152 | #print "number of vertices: ", np.size(vx, 0) 153 | plt.legend() 154 | plt.show() 155 | 156 | -------------------------------------------------------------------------------- /examples/force_polytopes/maximum_contact_force.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 12 10:54:31 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | from numpy import array 11 | from jet_leg.plotting.plotting_tools import Plotter 12 | import random 13 | from jet_leg.maths.math_tools import Math 14 | from jet_leg.dynamics.computational_dynamics import ComputationalDynamics 15 | from jet_leg.maths.iterative_projection_parameters import IterativeProjectionParameters 16 | 17 | import matplotlib.pyplot as plt 18 | from jet_leg.plotting.arrow3D import Arrow3D 19 | 20 | plt.close('all') 21 | math = Math() 22 | 23 | ''' number of generators, i.e. rays/edges used to linearize the friction cone ''' 24 | ng = 4 25 | 26 | ''' 27 | possible constraints for each foot: 28 | ONLY_ACTUATION = only joint-torque limits are enforces 29 | ONLY_FRICTION = only friction cone constraints are enforced 30 | FRICTION_AND_ACTUATION = both friction cone constraints and joint-torque limits 31 | ''' 32 | constraint_mode_IP = ['FRICTION_AND_ACTUATION', 33 | 'FRICTION_AND_ACTUATION', 34 | 'FRICTION_AND_ACTUATION', 35 | 'FRICTION_AND_ACTUATION'] 36 | 37 | # number of decision variables of the problem 38 | # n = nc*6 39 | comWF = np.array([0.0, -0.0, 0.0]) 40 | 41 | ''' Set the robot's name (either 'hyq', 'hyqreal' or 'anymal')''' 42 | robot_name = 'anymal' 43 | comp_dyn = ComputationalDynamics(robot_name) 44 | 45 | stackedForcePolytopesLF = np.zeros((3,8)) 46 | stackedFootPosLF = [] 47 | for lf_x in np.arange(-0.2, 0.2, 0.1): 48 | 49 | """ contact points in the World Frame""" 50 | LF_foot = np.array([lf_x, 0.2, -0.4]) 51 | RF_foot = np.array([0.3, -0.2, -0.4]) 52 | LH_foot = np.array([-0.3, 0.2, -0.4]) 53 | RH_foot = np.array([-0.3, -0.2, -0.4]) 54 | print LF_foot 55 | contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 56 | 57 | # contacts = contactsToStack[0:nc+1, :] 58 | # print contacts 59 | 60 | ''' parameters to be tuned''' 61 | g = 9.81 62 | trunk_mass = 45. 63 | mu = 0.5 64 | 65 | ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' 66 | stanceFeet = [1, 1, 1, 1] 67 | 68 | randomSwingLeg = random.randint(0, 3) 69 | tripleStance = False # if you want you can define a swing leg using this variable 70 | if tripleStance: 71 | print 'Swing leg', randomSwingLeg 72 | stanceFeet[randomSwingLeg] = 0 73 | print 'stanceLegs ', stanceFeet 74 | 75 | ''' now I define the normals to the surface of the contact points. By default they are all vertical now''' 76 | axisZ = array([[0.0], [0.0], [1.0]]) 77 | 78 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LF 79 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RF 80 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LH 81 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RH 82 | normals = np.vstack([n1, n2, n3, n4]) 83 | 84 | ''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg) 85 | HAA = Hip Abduction Adduction 86 | HFE = Hip Flextion Extension 87 | KFE = Knee Flextion Extension 88 | ''' 89 | LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 90 | RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 91 | LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 92 | RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 93 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 94 | 95 | ''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.''' 96 | extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm 97 | 98 | 99 | '''You now need to fill the 'params' object with all the relevant 100 | informations needed for the computation of the IP''' 101 | params = IterativeProjectionParameters() 102 | params.setContactsPosWF(contacts) 103 | params.setCoMPosWF(comWF) 104 | params.setTorqueLims(torque_limits) 105 | params.setActiveContacts(stanceFeet) 106 | params.setConstraintModes(constraint_mode_IP) 107 | params.setContactNormals(normals) 108 | params.setFrictionCoefficient(mu) 109 | params.setNumberOfFrictionConesEdges(ng) 110 | params.setTotalMass(trunk_mass) 111 | params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment 112 | 113 | # print "Inequalities", comp_dyn.ineq 114 | # print "actuation polygons" 115 | # print actuation_polygons 116 | 117 | '''I now check whether the given CoM configuration is stable or not''' 118 | C, d, isIKoutOfWorkSpace, forcePolytopes = comp_dyn.constr.getInequalities(params) 119 | 120 | if isIKoutOfWorkSpace: 121 | break 122 | else: 123 | forcePolytopeLF = forcePolytopes[0] 124 | print("force polytope LS", forcePolytopeLF) 125 | stackedForcePolytopesLF = np.hstack([stackedForcePolytopesLF, forcePolytopeLF]) 126 | stackedFootPosLF = np.hstack([stackedFootPosLF, lf_x]) 127 | 128 | ''' 2D figure ''' 129 | number_of_samples = np.shape(stackedForcePolytopesLF)[1]/8 -1 130 | print number_of_samples 131 | plt.figure() 132 | for i in np.arange(0,8): 133 | vx = [] 134 | vy = [] 135 | vz = [] 136 | for j in np.arange(0,number_of_samples): 137 | tmpVX = stackedForcePolytopesLF[0] 138 | tmpVY = stackedForcePolytopesLF[1] 139 | tmpVZ = stackedForcePolytopesLF[2] 140 | print np.shape(tmpVX) 141 | print j 142 | vx = np.hstack([vx, tmpVX[8 + j * 8 + i]]) 143 | vy = np.hstack([vy, tmpVY[8 + j * 8 + i]]) 144 | vz = np.hstack([vz, tmpVZ[8 + j * 8 + i]]) 145 | 146 | print vx 147 | print vy 148 | plt.plot(stackedFootPosLF, vz, '-o', markersize=15, label='CoM') 149 | 150 | plt.grid() 151 | plt.xlabel("X [m]") 152 | plt.ylabel("Y [m]") 153 | plt.legend() 154 | plt.show() 155 | 156 | -------------------------------------------------------------------------------- /examples/iterative_projection/path_sequential_IP_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Oct 25 10:58:03 2018 4 | 5 | @author: rorsolino 6 | """ 7 | import numpy as np 8 | from context import jet_leg 9 | import time 10 | from numpy import array 11 | 12 | import matplotlib.pyplot as plt 13 | import matplotlib.colors as colors 14 | import matplotlib.cm as cmx 15 | 16 | from jet_leg.plotting.plotting_tools import Plotter 17 | from jet_leg.maths.math_tools import Math 18 | from jet_leg.dynamics.computational_dynamics import ComputationalDynamics 19 | from jet_leg.map.height_map import HeightMap 20 | 21 | from jet_leg.optimization.path_sequential_iterative_projection import PathIterativeProjection 22 | from jet_leg.maths.iterative_projection_parameters import IterativeProjectionParameters 23 | 24 | ''' MAIN ''' 25 | start_t_IPVC = time.time() 26 | 27 | math = Math() 28 | compDyn = ComputationalDynamics() 29 | pathIP = PathIterativeProjection() 30 | # number of contacts 31 | number_of_contacts = 4 32 | 33 | # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION 34 | constraint_mode = ['FRICTION_AND_ACTUATION', 35 | 'FRICTION_AND_ACTUATION', 36 | 'FRICTION_AND_ACTUATION', 37 | 'FRICTION_AND_ACTUATION'] 38 | useVariableJacobian = True 39 | total_mass = 100 40 | mu = 0.8 41 | 42 | terrain = HeightMap() 43 | comWF = np.array([0.1, 0.1, 0.0]) 44 | #iterProj = PathIterativeProjection() 45 | 46 | #print "direction: ", desired_direction 47 | """ contact points """ 48 | LF_foot = np.array([0.4, 0.3, -0.5]) 49 | RF_foot = np.array([0.4, -0.3, -0.5]) 50 | terrainHeight = terrain.get_height(-.4, 0.3) 51 | LH_foot = np.array([-.4, 0.3, terrainHeight-0.5]) 52 | RH_foot = np.array([-0.3, -0.2, -0.5]) 53 | 54 | angle = np.random.normal(np.pi, 0.2) 55 | desired_direction = np.array([np.cos(angle), np.sin(angle), 0.0]) 56 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 57 | 58 | stanceLegs = [1,1,1,1] 59 | stanceIndex = [] 60 | swingIndex = [] 61 | print 'stance', stanceLegs 62 | for iter in range(0, 4): 63 | if stanceLegs[iter] == 1: 64 | # print 'new poly', stanceIndex, iter 65 | stanceIndex = np.hstack([stanceIndex, iter]) 66 | else: 67 | swingIndex = iter 68 | 69 | print stanceIndex, swingIndex 70 | 71 | LF_tau_lim = [50.0, 100.0, 100.0] 72 | RF_tau_lim = [50.0, 100.0, 100.0] 73 | LH_tau_lim = [50.0, 100.0, 100.0] 74 | RH_tau_lim = [50.0, 100.0, 100.0] 75 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 76 | comWF = np.array([0.0,0.0,0.0]) 77 | 78 | axisZ= array([[0.0], [0.0], [1.0]]) 79 | math = Math() 80 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 81 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 82 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 83 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 84 | # %% Cell 2 85 | normals = np.vstack([n1, n2, n3, n4]) 86 | ng = 4 87 | tolerance = 0.01 88 | max_iter = 10 89 | 90 | params = IterativeProjectionParameters() 91 | params.setContactsPosWF(contacts) 92 | params.setCoMPosWF(comWF) 93 | params.setTorqueLims(torque_limits) 94 | params.setActiveContacts(stanceLegs) 95 | params.setConstraintModes(constraint_mode) 96 | params.setContactNormals(normals) 97 | params.setFrictionCoefficient(mu) 98 | params.setNumberOfFrictionConesEdges(ng) 99 | params.setTotalMass(total_mass) 100 | CoMlist, stackedErrors, stacked_polygons = pathIP.find_vertex_along_path(params, desired_direction, tolerance, max_iter) 101 | newLimitPoint = CoMlist[-1] 102 | print 'Errors convergence: ', stackedErrors 103 | print 'first', stacked_polygons[0] 104 | first = stacked_polygons.pop(0) 105 | print 'first', first 106 | first = stacked_polygons.pop(0) 107 | print 'second', first 108 | print("Path Sequential Iterative Projection: --- %s seconds ---" % (time.time() - start_t_IPVC)) 109 | 110 | 111 | '''Plotting Fig 1''' 112 | plt.close('all') 113 | plotter = Plotter() 114 | 115 | print stackedErrors 116 | #dx = stackedErrors[:,0] 117 | #dy = stackedErrors[:,1] 118 | #d = np.linalg.norm(stackedErrors, axis=1) 119 | 120 | plt.figure() 121 | plt.grid() 122 | plt.ylabel("Absolute error [m]") 123 | plt.xlabel("Iterations number") 124 | #plt.plot(np.abs(stackedErrors[:,0]), 'b', linewidth=5, label = 'X direction') 125 | #plt.plot(np.abs(stackedErrors[:,1]), 'r', linewidth=2, label = 'Y direction') 126 | plt.plot(np.abs(d), 'g', linewidth=2, label = 'd') 127 | plt.legend() 128 | 129 | 130 | '''Plotting Fig 2''' 131 | plt.figure() 132 | plt.grid() 133 | plt.xlabel("X [m]") 134 | plt.ylabel("Y [m]") 135 | 136 | plt.plot(newLimitPoint[0], newLimitPoint[1], 'g^', markersize=20, label= 'Actuation region vertex') 137 | plt.plot(comWF[0], comWF[1], 'ro', markersize=20, label= 'Initial CoM position used in the SIP alg.') 138 | segment = np.vstack([comWF,newLimitPoint]) 139 | plt.plot(segment[:,0], segment[:,1], 'b-', linewidth=2, label= 'Search direction') 140 | plt.plot(contacts[0:number_of_contacts,0],contacts[0:number_of_contacts,1],'ko',markersize=15, label='Stance feet') 141 | 142 | ''' instantaneous plot actuation regions''' 143 | lower_lim = -10 144 | upper_lim = 10 145 | scale = np.linspace(lower_lim, upper_lim, 10) 146 | jet = cm = plt.get_cmap('seismic') 147 | cNorm = colors.Normalize(vmin=lower_lim, vmax=upper_lim) 148 | scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=jet) 149 | index = 0 150 | err = np.hstack(stackedErrors) 151 | 152 | 153 | for polygon in stacked_polygons: 154 | point = np.vstack([polygon]) 155 | x = np.hstack([point[:,0], point[0,0]]) 156 | y = np.hstack([point[:,1], point[0,1]]) 157 | colorVal = scalarMap.to_rgba(scale[index]) 158 | # print index 159 | plt.plot(x,y, color = colorVal, linewidth=5., label = 'error: '+str(round(d[index], 2))) 160 | plt.plot(CoMlist[index,0], CoMlist[index,1], color = colorVal, marker='o', markersize=15) 161 | index+=1 162 | 163 | 164 | plt.xlim(-0.9, 0.5) 165 | plt.ylim(-0.7, 0.7) 166 | plt.legend() 167 | plt.show() 168 | 169 | -------------------------------------------------------------------------------- /examples/feasible_region/actuation_region_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Thu Oct 25 11:44:22 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | import numpy as np 9 | from context import jet_leg 10 | import time 11 | from numpy import array 12 | 13 | import matplotlib.pyplot as plt 14 | from jet_leg.plotting_tools import Plotter 15 | from jet_leg.math_tools import Math 16 | from jet_leg.computational_dynamics import ComputationalDynamics 17 | from jet_leg.height_map import HeightMap 18 | 19 | from jet_leg.path_sequential_iterative_projection import PathIterativeProjection 20 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 21 | 22 | 23 | ''' MAIN ''' 24 | 25 | plt.close('all') 26 | start_t_IPVC = time.time() 27 | 28 | math = Math() 29 | compDyn = ComputationalDynamics() 30 | pathIP = PathIterativeProjection() 31 | 32 | # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION 33 | constraint_mode = ['FRICTION_AND_ACTUATION', 34 | 'FRICTION_AND_ACTUATION', 35 | 'FRICTION_AND_ACTUATION', 36 | 'FRICTION_AND_ACTUATION'] 37 | useVariableJacobian = True 38 | total_mass = 100 39 | mu = 0.8 40 | 41 | comTrajectoriesToStack = np.zeros((0,3)) 42 | terrain = HeightMap() 43 | comWF = np.array([0.1, 0.1, 0.0]) 44 | optimizedVariablesToStack = np.zeros((0,3)) 45 | iterProj = PathIterativeProjection() 46 | 47 | axisZ= array([[0.0], [0.0], [1.0]]) 48 | math = Math() 49 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 50 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 51 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 52 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 53 | # %% Cell 2 54 | normals = np.vstack([n1, n2, n3, n4]) 55 | ng = 4 56 | #print "direction: ", desired_direction 57 | """ contact points """ 58 | #LF_foot = np.array([0.4, 0.3, -0.5]) 59 | #RF_foot = np.array([0.4, -0.3, -0.5]) 60 | #terrainHeight = terrain.get_height(-.4, 0.3) 61 | #LH_foot = np.array([-.4, 0.3, terrainHeight-0.5]) 62 | #RH_foot = np.array([-0.3, -0.2, -0.5]) 63 | 64 | LF_foot = np.array([0.3, 0.3, -0.5]) 65 | RF_foot = np.array([0.3, -0.2, -0.5]) 66 | LH_foot = np.array([-0.3, 0.3, -0.5]) 67 | RH_foot = np.array([-0.3, -0.2, -0.5]) 68 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 69 | stanceLegs = [1,1,1,1] 70 | stanceIndex = [] 71 | swingIndex = [] 72 | startingPointX = 0.0 73 | startingPointY = 0.0 74 | print 'stance', stanceLegs 75 | for iter in range(0, 4): 76 | if stanceLegs[iter] == 1: 77 | # print 'new poly', stanceIndex, iter 78 | # print 'a',contacts[iter, 0] 79 | startingPointX += contacts[iter, 0] 80 | startingPointY += contacts[iter, 1] 81 | print startingPointX, startingPointY 82 | stanceIndex = np.hstack([stanceIndex, iter]) 83 | else: 84 | swingIndex = iter 85 | 86 | # number of contacts 87 | number_of_contacts = np.sum(stanceLegs) 88 | startingPoint = np.array([startingPointX/float(number_of_contacts), startingPointY/float(number_of_contacts), 0.0]) 89 | print startingPoint 90 | 91 | 92 | 93 | LF_tau_lim = [50.0, 100.0, 100.0] 94 | RF_tau_lim = [50.0, 100.0, 100.0] 95 | LH_tau_lim = [50.0, 100.0, 100.0] 96 | RH_tau_lim = [50.0, 100.0, 100.0] 97 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 98 | 99 | tolerance = 0.005 100 | 101 | params = IterativeProjectionParameters() 102 | params.setContactsPosWF(contacts) 103 | params.setCoMPosWF(startingPoint) 104 | params.setTorqueLims(torque_limits) 105 | params.setActiveContacts(stanceLegs) 106 | params.setConstraintModes(constraint_mode) 107 | params.setContactNormals(normals) 108 | params.setFrictionCoefficient(mu) 109 | params.setNumberOfFrictionConesEdges(ng) 110 | params.setTotalMass(total_mass) 111 | 112 | for angle in np.arange(0.0, 0.5, 0.25): 113 | print '===============> new search angle: ', angle 114 | startingPoint = np.array([startingPointX/float(number_of_contacts) + np.random.normal(0.0, 0.05), 115 | startingPointY/float(number_of_contacts) + np.random.normal(0.0, 0.05), 0.0]) 116 | params.setCoMPosWF(startingPoint) 117 | desired_direction = np.array([np.cos(angle), np.sin(angle), 0.0]) 118 | newLimitPoint, stackedErrors, stackedPolygons = pathIP.find_vertex_along_path(params, desired_direction, tolerance) 119 | comTrajectoriesToStack = np.vstack([comTrajectoriesToStack, newLimitPoint]) 120 | 121 | 122 | print 'Errors convergence: ', stackedErrors 123 | 124 | print("Actuation Region estimation time: --- %s seconds ---" % (time.time() - start_t_IPVC)) 125 | 126 | '''Plotting''' 127 | plt.figure() 128 | plt.grid() 129 | plt.xlabel("X [m]") 130 | plt.ylabel("Y [m]") 131 | 132 | plt.plot(comTrajectoriesToStack[:,0], comTrajectoriesToStack[:,1], 'g--', linewidth=2) 133 | plt.plot(comTrajectoriesToStack[:,0], comTrajectoriesToStack[:,1], 'g^', markersize=20, label= 'Actuation region vertices') 134 | plt.plot(startingPoint[0], startingPoint[1], 'ro', markersize=20, label= 'Initial CoM position used in the SIP alg.') 135 | segment = np.vstack([comWF,newLimitPoint]) 136 | print int(stanceIndex[0]) 137 | print contacts[int(stanceIndex[0])][0] 138 | if number_of_contacts == 4: 139 | contactsX = [contacts[int(stanceIndex[0])][0], contacts[int(stanceIndex[1])][0], contacts[int(stanceIndex[2])][0], contacts[int(stanceIndex[3])][0]] 140 | contactsY = [contacts[int(stanceIndex[0])][1], contacts[int(stanceIndex[1])][1], contacts[int(stanceIndex[2])][1], contacts[int(stanceIndex[3])][1]] 141 | else: 142 | contactsX = [contacts[int(stanceIndex[0])][0], contacts[int(stanceIndex[1])][0], contacts[int(stanceIndex[2])][0]] 143 | contactsY = [contacts[int(stanceIndex[0])][1], contacts[int(stanceIndex[1])][1], contacts[int(stanceIndex[2])][1]] 144 | print contactsX, contactsY 145 | plt.plot(contactsX,contactsY,'ko',markersize=15, label='Stance feet') 146 | 147 | plt.xlim(-0.8, 0.6) 148 | plt.ylim(-0.7, 0.7) 149 | plt.legend() 150 | plt.show() 151 | 152 | -------------------------------------------------------------------------------- /unit_tests/test_bilinear_constraints.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ====================== 3 | 3D surface (color map) 4 | ====================== 5 | 6 | Demonstrates plotting a 3D surface colored with the coolwarm color map. 7 | The surface is made opaque by using antialiased=False. 8 | 9 | Also demonstrates using the LinearLocator and custom formatting for the 10 | z axis tick labels. 11 | ''' 12 | import context 13 | from mpl_toolkits.mplot3d import Axes3D 14 | import matplotlib.pyplot as plt 15 | from matplotlib import cm 16 | import matplotlib as mpl 17 | from matplotlib.ticker import LinearLocator, FormatStrFormatter 18 | import numpy as np 19 | import random as rnd 20 | from jet_leg.bilinear_constraints import BilinearConstraints 21 | 22 | 23 | '''MAIN ''' 24 | makePlots = False 25 | bilinearConstraint = BilinearConstraints() 26 | x_val = 1.5#rnd.random() 27 | X, F, p_hat, p_hat_relaxation, q_hat = bilinearConstraint.make_convex(x_val) 28 | 29 | 30 | 31 | ''' Plot the results ''' 32 | if makePlots: 33 | plt.close("all") 34 | fig = plt.figure(1) 35 | 36 | ax = fig.gca(projection='3d') 37 | Tau = bilinearConstraint.get_torque() 38 | surf = ax.plot_surface(X, F, Tau, color="r", alpha=0.4) 39 | #surf = ax.plot_surface(X, F, p_hat, color="r", alpha=0.4) 40 | #surf = ax.plot_surface(X, F, p_hat_relaxation, color="b", alpha=0.3) 41 | #surf = ax.plot_surface(X, Y, Conv_minus, color="y", alpha=0.3) 42 | #surf = ax.plot_surface(X, Y, Tau_approx, color="g", alpha=0.3) 43 | # Customize the z axis. 44 | ax.set_zlim(-10.01, 10.01) 45 | ax.zaxis.set_major_locator(LinearLocator(10)) 46 | ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f')) 47 | 48 | ax.set_xlim(-10.01, 10.01) 49 | ax.set_ylim(-10.01, 10.01) 50 | fake2Dline = mpl.lines.Line2D([0],[0], linestyle="none", c='r', marker = 's', alpha=0.4) 51 | ax.legend([fake2Dline], ['bilinear constraint'], numpoints = 1, loc="upper left", markerscale=5.) 52 | ax.set_xlabel('position [m]', labelpad=20) 53 | ax.set_ylabel('force [N]', labelpad=20) 54 | ax.set_zlabel('torque [Nm]', labelpad=20) 55 | mpl.rcParams.update({'font.size': 22}) 56 | #mpl.rcParams['xtick.labelsize'] =15 57 | #plt.tick_params(labelsize=1) 58 | #ax.set_xticklabels(ax1_x, fontsize=15) 59 | fig.savefig('../figs/bilinear_constraints/bilinear_constraint.pdf') 60 | fig.savefig('../figs/bilinear_constraints/bilinear_constraint.png') 61 | 62 | fig = plt.figure(2) 63 | ax = fig.gca(projection='3d') 64 | Tau = bilinearConstraint.get_torque() 65 | #surf = ax.plot_surface(X, F, Tau, color="b", alpha=0.7) 66 | surf = ax.plot_surface(X, F, 0.25*p_hat, color="r", alpha=0.6) 67 | surf = ax.plot_surface(X, F, -0.25*q_hat, color="r", alpha=0.6) 68 | #surf = ax.plot_surface(X, F, p_hat_relaxation, color="b", alpha=0.3) 69 | #surf = ax.plot_surface(X, Y, Conv_minus, color="y", alpha=0.3) 70 | #surf = ax.plot_surface(X, Y, Tau_approx, color="g", alpha=0.3) 71 | # Customize the z axis. 72 | ax.set_zlim(-10.01, 10.01) 73 | ax.zaxis.set_major_locator(LinearLocator(10)) 74 | ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f')) 75 | 76 | ax.set_xlim(-10.01, 10.01) 77 | ax.set_ylim(-10.01, 10.01) 78 | fake2Dline = mpl.lines.Line2D([0],[0], linestyle="none", c='r', marker = 's', alpha=0.4) 79 | ax.legend([fake2Dline], ['quadratic terms'], numpoints = 1, loc="upper left", markerscale=5.) 80 | ax.set_xlabel('position [m]', labelpad=20) 81 | ax.set_ylabel('force [N]', labelpad=20) 82 | ax.set_zlabel('torque [Nm]', labelpad=20) 83 | mpl.rcParams.update({'font.size': 22}) 84 | #mpl.rcParams['xtick.labelsize'] =15 85 | #plt.tick_params(labelsize=1) 86 | #ax.set_xticklabels(ax1_x, fontsize=15) 87 | fig.savefig('../figs/bilinear_constraints/quadratic_contraints.pdf') 88 | fig.savefig('../figs/bilinear_constraints/quadratic_contraints.png') 89 | 90 | fig = plt.figure(3) 91 | ax = fig.gca(projection='3d') 92 | #surf = ax.plot_surface(X, Y, Tau, color="r", alpha=0.4) 93 | surf = ax.plot_surface(X, F, p_hat, color="r", alpha=0.4) 94 | surf = ax.plot_surface(X, F, p_hat_relaxation, color="b", alpha=0.3) 95 | #surf = ax.plot_surface(X, Y, Conv_minus, color="y", alpha=0.3) 96 | #surf = ax.plot_surface(X, Y, Tau_approx, color="g", alpha=0.3) 97 | # Customize the z axis. 98 | ax.set_zlim(-10.01, 10.01) 99 | ax.zaxis.set_major_locator(LinearLocator(10)) 100 | ax.zaxis.set_major_formatter(FormatStrFormatter('%.02f')) 101 | 102 | ax.set_xlim(-10.01, 10.01) 103 | ax.set_ylim(-10.01, 10.01) 104 | fake2Dline1 = mpl.lines.Line2D([0],[0], linestyle="none", c='r', marker = 's', alpha=0.4) 105 | fake2Dline2 = mpl.lines.Line2D([0],[0], linestyle="none", c='b', marker = 's', alpha=0.3) 106 | ax.legend([fake2Dline1, fake2Dline2], ['quadratic soft constraint', 'linear constraint'], numpoints = 1, loc="upper left", markerscale=5.) 107 | ax.set_xlabel('position [m]', labelpad=20) 108 | ax.set_ylabel('force [N]', labelpad=20) 109 | ax.set_zlabel('torque [Nm]', labelpad=20) 110 | mpl.rcParams.update({'font.size': 15}) 111 | #mpl.rcParams['xtick.labelsize'] =15 112 | #plt.tick_params(labelsize=1) 113 | #ax.set_xticklabels(ax1_x, fontsize=15) 114 | fig.savefig('../figs/bilinear_constraints/linear_constraints.pdf') 115 | fig.savefig('../figs/bilinear_constraints/linear_constraints.png') 116 | 117 | 118 | fig = plt.figure(4) 119 | plt.plot(X[len(X)/2.0,:],np.diagonal(p_hat),'r',linewidth=3, label = 'quadratic soft constraint') 120 | plt.plot(X[len(X)/2.0,:],np.diagonal(p_hat_relaxation),'b',linewidth=3, label = 'linear approximation') 121 | plt.plot(x_val,0.0,'ro') 122 | plt.xlim(-20, 20) 123 | plt.ylim(-5, 35) 124 | plt.grid() 125 | plt.legend() 126 | plt.xlabel('position [m]') 127 | plt.ylabel('torque [Nm]') 128 | mpl.rcParams.update({'font.size': 15}) 129 | fig.savefig('../figs/bilinear_constraints/linear_constraints_2D.pdf') 130 | fig.savefig('../figs/bilinear_constraints/linear_constraints_2D.png') -------------------------------------------------------------------------------- /examples/force_polytopes/compute_force_polytopes.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 12 10:54:31 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | from numpy import array 11 | from jet_leg.plotting.plotting_tools import Plotter 12 | import random 13 | from jet_leg.maths.math_tools import Math 14 | from jet_leg.dynamics.computational_dynamics import ComputationalDynamics 15 | from jet_leg.maths.iterative_projection_parameters import IterativeProjectionParameters 16 | 17 | import matplotlib.pyplot as plt 18 | from jet_leg.plotting.arrow3D import Arrow3D 19 | 20 | plt.close('all') 21 | math = Math() 22 | 23 | ''' number of generators, i.e. rays/edges used to linearize the friction cone ''' 24 | ng = 4 25 | 26 | ''' 27 | possible constraints for each foot: 28 | ONLY_ACTUATION = only joint-torque limits are enforces 29 | ONLY_FRICTION = only friction cone constraints are enforced 30 | FRICTION_AND_ACTUATION = both friction cone constraints and joint-torque limits 31 | ''' 32 | constraint_mode_IP = ['FRICTION_AND_ACTUATION', 33 | 'FRICTION_AND_ACTUATION', 34 | 'FRICTION_AND_ACTUATION', 35 | 'FRICTION_AND_ACTUATION'] 36 | 37 | # number of decision variables of the problem 38 | # n = nc*6 39 | comWF = np.array([0.0, -0.0, 0.0]) 40 | 41 | """ contact points in the World Frame""" 42 | LF_foot = np.array([0.4, 0.3, -0.5]) 43 | RF_foot = np.array([0.3, -0.2, -0.5]) 44 | LH_foot = np.array([-0.3, 0.2, -0.5]) 45 | RH_foot = np.array([-0.3, -0.2, -0.5]) 46 | 47 | contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 48 | 49 | # contacts = contactsToStack[0:nc+1, :] 50 | # print contacts 51 | 52 | ''' parameters to be tuned''' 53 | g = 9.81 54 | trunk_mass = 85. 55 | mu = 0.5 56 | 57 | ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' 58 | stanceFeet = [1, 1, 1, 1] 59 | 60 | randomSwingLeg = random.randint(0, 3) 61 | tripleStance = False # if you want you can define a swing leg using this variable 62 | if tripleStance: 63 | print 'Swing leg', randomSwingLeg 64 | stanceFeet[randomSwingLeg] = 0 65 | print 'stanceLegs ', stanceFeet 66 | 67 | ''' now I define the normals to the surface of the contact points. By default they are all vertical now''' 68 | axisZ = array([[0.0], [0.0], [1.0]]) 69 | 70 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LF 71 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RF 72 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LH 73 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RH 74 | normals = np.vstack([n1, n2, n3, n4]) 75 | 76 | ''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg) 77 | HAA = Hip Abduction Adduction 78 | HFE = Hip Flextion Extension 79 | KFE = Knee Flextion Extension 80 | ''' 81 | LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 82 | RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 83 | LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 84 | RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 85 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 86 | 87 | ''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.''' 88 | extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm 89 | 90 | comp_dyn = ComputationalDynamics('anymal') 91 | 92 | '''You now need to fill the 'params' object with all the relevant 93 | informations needed for the computation of the IP''' 94 | params = IterativeProjectionParameters() 95 | params.setContactsPosWF(contacts) 96 | params.setCoMPosWF(comWF) 97 | params.setTorqueLims(torque_limits) 98 | params.setActiveContacts(stanceFeet) 99 | params.setConstraintModes(constraint_mode_IP) 100 | params.setContactNormals(normals) 101 | params.setFrictionCoefficient(mu) 102 | params.setNumberOfFrictionConesEdges(ng) 103 | params.setTotalMass(trunk_mass) 104 | params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment 105 | 106 | # print "Inequalities", comp_dyn.ineq 107 | # print "actuation polygons" 108 | # print actuation_polygons 109 | 110 | '''I now check whether the given CoM configuration is stable or not''' 111 | C, d, isIKoutOfWorkSpace, forcePolytopes = comp_dyn.constr.getInequalities(params) 112 | 113 | '''Plotting the contact points in the 3D figure''' 114 | fig = plt.figure() 115 | ax = fig.add_subplot(111, projection='3d') 116 | ax.set_xlabel('X axis') 117 | ax.set_ylabel('Y axis') 118 | ax.set_zlabel('Z axis') 119 | 120 | nc = np.sum(stanceFeet) 121 | stanceID = params.getStanceIndex(stanceFeet) 122 | force_scaling_factor = 1500 123 | # plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15) 124 | fz_tot = 0.0 125 | for j in range(0, 126 | nc): # this will only show the contact positions and normals of the feet that are defined to be in stance 127 | idx = int(stanceID[j]) 128 | ax.scatter(contacts[idx, 0], contacts[idx, 1], contacts[idx, 2], c='b', s=100) 129 | 130 | ''' draw 3D arrows corresponding to contact normals''' 131 | a = Arrow3D([contacts[idx, 0], contacts[idx, 0] + normals[idx, 0] / 10], 132 | [contacts[idx, 1], contacts[idx, 1] + normals[idx, 1] / 10], 133 | [contacts[idx, 2], contacts[idx, 2] + normals[idx, 2] / 10], mutation_scale=20, lw=3, arrowstyle="-|>", 134 | color="r") 135 | 136 | ''' The black spheres represent the projection of the contact points on the same plane of the feasible region''' 137 | ax.scatter(contacts[idx, 0], contacts[idx, 1], 0.0, c='k', s=100) 138 | ax.add_artist(a) 139 | 140 | print 'sum of vertical forces is', fz_tot 141 | 142 | ''' plotting Iterative Projection points ''' 143 | plotter = Plotter() 144 | for j in range(0, nc): # this will only show the force polytopes of the feet that are defined to be in stance 145 | idx = int(stanceID[j]) 146 | if (constraint_mode_IP[idx] == 'ONLY_ACTUATION') or (constraint_mode_IP[idx] == 'FRICTION_AND_ACTUATION'): 147 | plotter.plot_actuation_polygon(ax, forcePolytopes[idx], contacts[idx, :], force_scaling_factor) 148 | 149 | plt.grid() 150 | plt.xlabel("X [m]") 151 | plt.ylabel("Y [m]") 152 | plt.legend() 153 | plt.show() -------------------------------------------------------------------------------- /jet_leg/maths/math_tools.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 5 09:43:27 2018 4 | 5 | @author: romeo orsolino 6 | """ 7 | import numpy as np 8 | 9 | class Math: 10 | def normalize(self, n): 11 | norm1 = np.linalg.norm(n) 12 | n = np.true_divide(n, norm1) 13 | return n 14 | 15 | def skew(self, v): 16 | if len(v) == 4: v = v[:3]/v[3] 17 | skv = np.roll(np.roll(np.diag(v.flatten()), 1, 1), -1, 0) 18 | return skv - skv.T 19 | 20 | def rotation_matrix_from_normal(self, n): 21 | n = n.reshape((3,)) 22 | e_x = np.array([1., 0., 0.]) 23 | t = e_x - np.dot(e_x, n) * n 24 | t = t / np.linalg.norm(t) 25 | b = np.cross(n, t) 26 | return np.vstack([t, b, n]).T 27 | 28 | def getGraspMatrix(self, r): 29 | math = Math() 30 | G = block([[np.eye(3), np.zeros((3, 3))], 31 | [math.skew(r), np.eye(3)]]) 32 | return G 33 | 34 | 35 | def rpyToRot(self, roll, pitch, yaw): 36 | 37 | 38 | 39 | Rx = np.array([ [ 1 , 0 , 0], 40 | [0 , np.cos(roll) , np.sin(roll)], 41 | [0 , -np.sin(roll), np.cos(roll)]]); 42 | 43 | 44 | Ry = np.array([[np.cos(pitch) , 0 , -np.sin(pitch)], 45 | [ 0 , 1 , 0], 46 | [np.sin(pitch) , 0 , np.cos(pitch)]]); 47 | 48 | 49 | Rz = np.array([[ np.cos(yaw) , np.sin(yaw) , 0], 50 | [-np.sin(yaw) , np.cos(yaw) , 0], 51 | [0 , 0 , 1]]); 52 | 53 | 54 | R = Rx.dot(Ry.dot(Rz)); 55 | return R 56 | 57 | def line(self, p1, p2): 58 | A = (p1[1] - p2[1]) 59 | B = (p2[0] - p1[0]) 60 | C = (p1[0]*p2[1] - p2[0]*p1[1]) 61 | return A, B, -C 62 | 63 | def two_lines_intersection(self, L1, L2): 64 | D = L1[0] * L2[1] - L1[1] * L2[0] 65 | Dx = L1[2] * L2[1] - L1[1] * L2[2] 66 | Dy = L1[0] * L2[2] - L1[2] * L2[0] 67 | if D != 0: 68 | x = Dx / D 69 | y = Dy / D 70 | return x,y 71 | else: 72 | return False 73 | 74 | def is_point_inside_segment(self, first_input_point, second_input_point, point_to_check): 75 | epsilon = 0.001 76 | 77 | if (np.abs(first_input_point[0] - second_input_point[0]) < 1e-02): 78 | alpha = (point_to_check[1] - second_input_point[1]) / (first_input_point[1] - second_input_point[1]) 79 | else: 80 | alpha = (point_to_check[0] - second_input_point[0]) / (first_input_point[0] - second_input_point[0]) 81 | 82 | if(alpha>=-epsilon)&(alpha<=1.0+epsilon): 83 | new_point = point_to_check 84 | else: 85 | new_point = False 86 | 87 | return new_point, alpha 88 | 89 | def find_point_to_line_signed_distance(self, segment_point1, segment_point2, point_to_check): 90 | # this function returns a positive distance if the point is on the right side of the segment. This will return 91 | # a positive distance for a polygon queried in clockwise order and with a point_to_check which lies inside the polygon itself 92 | num = (segment_point2[0] - segment_point1[0])*(segment_point1[1] - point_to_check[1]) - (segment_point1[0] - point_to_check[0])*(segment_point2[1] - segment_point1[1]) 93 | denum_sq = (segment_point2[0] - segment_point1[0])*(segment_point2[0] - segment_point1[0]) + (segment_point2[1] - segment_point1[1])*(segment_point2[1] - segment_point1[1]) 94 | dist = num/np.sqrt(denum_sq) 95 | return dist 96 | 97 | def find_residual_radius(self, polygon, point_to_check): 98 | # this function returns a positive distance if the point is on the right side of the segment. This will return 99 | # a positive distance for a polygon queried in clockwise order and with a point_to_check which lies inside the polygon itself 100 | numberOfVertices = np.size(polygon,0) 101 | residual_radius = 1000000.0 102 | for i in range(0,numberOfVertices-1): 103 | s1 = polygon[i,:] 104 | s2 = polygon[i+1,:] 105 | d_temp = self.find_point_to_line_signed_distance(s1, s2, point_to_check) 106 | if d_temp < 0.0: 107 | print('Warning! found negative distance. Polygon might not be in clockwise order...') 108 | elif d_temp < residual_radius: 109 | residual_radius = d_temp 110 | 111 | # we dont need to compute for the last edge cause we added an extra point to close the polytop (last point equal to the first) 112 | 113 | return residual_radius 114 | 115 | def find_polygon_segment_intersection(self, vertices_input, desired_direction, starting_point): 116 | desired_direction = desired_direction/np.linalg.norm(desired_direction)*10.0 117 | 118 | desired_com_line = self.line(starting_point, starting_point+desired_direction) 119 | tmp_vertices = np.vstack([vertices_input, vertices_input[0]]) 120 | intersection_points = np.zeros((0,2)) 121 | points_along_direction = np.zeros((0,2)) 122 | point_to_com_distance = np.zeros((0,1)) 123 | 124 | for i in range(0,len(vertices_input)): 125 | v1 = tmp_vertices[i,:] 126 | v2 = tmp_vertices[i+1,:] 127 | actuation_region_edge = self.line(v1, v2) 128 | new_point = self.two_lines_intersection(desired_com_line, actuation_region_edge) 129 | 130 | if new_point: 131 | intersection_points = np.vstack([intersection_points, new_point]) 132 | new_point, alpha = self.is_point_inside_segment(starting_point, starting_point+desired_direction, new_point) 133 | if new_point: 134 | points_along_direction = np.vstack([points_along_direction, new_point]) 135 | d = np.sqrt((new_point[0] - starting_point[0])*(new_point[0] - starting_point[0]) + (new_point[1] - starting_point[1])*(new_point[1] - starting_point[1])) 136 | point_to_com_distance = np.vstack([point_to_com_distance, d]) 137 | 138 | else: 139 | print("lines are parallel!") 140 | #while new_point is False: 141 | # desired_com_line = self.line(starting_point, starting_point+desired_direction) 142 | # new_point = self.two_lines_intersection(desired_com_line, actuation_region_edge) 143 | # intersection_points = np.vstack([intersection_points, new_point]) 144 | # print new_point 145 | 146 | idx = np.argmin(point_to_com_distance) 147 | final_point = points_along_direction[idx,:] 148 | return final_point, intersection_points 149 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | hyqgreen planning 2 | hyqgreen planning 3 | 4 | 5 | # Feasible Region: an Actuation-Aware Extension of the Support Region 6 | This python library contains the code used for the motion planning formulation proposed in this [preprint](https://arxiv.org/abs/1903.07999#). In here you can also find the code used to generate the figures and plots of the manuscript. 7 | 8 | hyqgreen planning hyqgreen planning 9 | 10 | Jet-leg performs common operations over [convex 11 | polyhedra](https://en.wikipedia.org/wiki/Convex_polyhedron) in higher dimensions in order to assess the problem of stability and motion feasibility of legged robots. 12 | 13 | ## What you can do with Jet-leg: 14 | - compute the Support region of legged robots as in [Bretl. et al. 2008](https://ieeexplore.ieee.org/abstract/document/4598894); 15 | - compute the Feasible region of legged robots as in [Orsolino. et al. 2019](https://arxiv.org/abs/1903.07999#); 16 | - compute force polytopes of legged robots given their URDF; 17 | - compare different leg designs and understand their consequences on the robot's balancing capabilities; 18 | - test various formulations of linear, convex or nonlinear trajectory optimization problems; 19 | 20 | ## Quick installation 21 | You can directly install the pre-built library usign pip. First, create a virtual environment: 22 | ``` 23 | python -m venv my_project 24 | cd source/bin/activate 25 | ``` 26 | Then, install the required dependencies: 27 | ``` 28 | sudo apt-get install -y cython libglpk-dev python3-tk 29 | pip install pytest numpy scipy matplotlib pycddlib pypoman pin 30 | ``` 31 | and finally run: 32 | ``` 33 | pip install jet-leg 34 | ``` 35 | To make sure that the installation worked as expected, you can make sure that the following command runs without errors: 36 | ``` 37 | python3 -c "import jet_leg" 38 | ``` 39 | Have a look to the [this](https://github.com/orsoromeo/jet-leg/tree/master/examples) folder for a few examples on how to use the library. 40 | 41 | ## Build from source 42 | For active development of the library a source installation is recommended. For this, you will need to clone the repo first. Then, you can build the package using the provided Docker environment. Do so by running the following command: 43 | ``` 44 | cd jet-leg 45 | ./build.sh 46 | ``` 47 | This will create a docker image with all the required dependencies. You can then attach the image by running: 48 | ``` 49 | ./run.sh 50 | ``` 51 | To make sure that the installation worked successfully, you can run, for example: 52 | ``` 53 | python3 examples/iterative_projection/single_iterative_projection_example.py 54 | ``` 55 | The example above should generate two figures representing the feasible region of the Anymal robot in a default configuration in 2D and 3D. 56 | 57 | ## Python dependencies: 58 | - Numpy 59 | - Scipy 60 | - Pycddlib 61 | - Matplotlib 62 | - [Pypoman](https://github.com/stephane-caron/pypoman) for the manipulation of polyhedrical object 63 | - [Pinocchio](https://github.com/stack-of-tasks/pinocchio) 64 | 65 | 73 | 74 | ## Optional dependencies: 75 | 76 | - [Ipopt](https://projects.coin-or.org/Ipopt) and its Python interface [Pypi](https://pypi.org/project/ipopt/) for the solution of large-scale nonlinear optimization problems 77 | - [ffmpeg](https://www.ffmpeg.org/) for the generation of Matplotlib animations 78 | - [unittest](https://docs.python.org/3/library/unittest.html) for testing of dependencies installation and for development 79 | 80 | 81 | 91 | 92 | 99 | 100 | ## Troubleshooting 101 | 102 | - Jet-leg has been tested using Python 3.8.17 and Docker; 103 | - If CVXOPT is not found even after trying the pip-installation, we then suggest to try install the version 1.1.4 of CVXOPT using Synaptic or to clone and install it manually after building. Note: delete every previous installation of cvxopt that is in the system using locate cvxopt (after sudo updatedb) 104 | 105 | ## See also 106 | 107 | - The [pypoman](https://github.com/stephane-caron/pypoman) and [pymanoid](https://github.com/stephane-caron/pymanoid) libraries developed by Stéphane Caron 108 | - Komei Fukuda's [Frequently Asked Questions in Polyhedral Computation](http://www.cs.mcgill.ca/~fukuda/soft/polyfaq/polyfaq.html) 109 | - The 110 | [Polyhedron](http://doc.sagemath.org/html/en/reference/discrete_geometry/sage/geometry/polyhedron/constructor.html) class in [Sage](http://www.sagemath.org/) 111 | - The [StabiliPy](https://github.com/haudren/stabilipy) package provides a more 112 | general recursive projection method 113 | - [Pinocchio](https://github.com/stack-of-tasks/pinocchio) is used to compute the Jacobian matrix and to solve the Inverse Kinematics (IK) problem. 114 | -------------------------------------------------------------------------------- /unit_tests/actuation_region_main.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 12 10:54:31 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import time 9 | import pylab 10 | import pypoman 11 | import numpy as np 12 | 13 | from context import jet_leg 14 | 15 | from numpy import array, cross, dot, eye, hstack, vstack, zeros, matrix 16 | from numpy.linalg import norm 17 | from jet_leg.plotting_tools import Plotter 18 | 19 | from jet_leg.math_tools import Math 20 | from jet_leg.computational_dynamics import ComputationalDynamics 21 | from jet_leg.vertex_based_projection import VertexBasedProjection 22 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 23 | import random 24 | import matplotlib.pyplot as plt 25 | from jet_leg.arrow3D import Arrow3D 26 | 27 | plt.close('all') 28 | math = Math() 29 | 30 | ''' number of generators, i.e. rays/edges used to linearize the friction cone ''' 31 | ng = 4 32 | 33 | ''' 34 | possible constraints for each foot: 35 | ONLY_ACTUATION = only joint-torque limits are enforces 36 | ONLY_FRICTION = only friction cone constraints are enforced 37 | FRICTION_AND_ACTUATION = both friction cone constraints and joint-torque limits 38 | ''' 39 | constraint_mode_IP = ['FRICTION_AND_ACTUATION', 40 | 'FRICTION_AND_ACTUATION', 41 | 'FRICTION_AND_ACTUATION', 42 | 'FRICTION_AND_ACTUATION'] 43 | 44 | # number of decision variables of the problem 45 | #n = nc*6 46 | comWF = np.array([0.0, 0.0, 0.0]) 47 | 48 | """ contact points in the World Frame""" 49 | LF_foot = np.array([0.3, 0.2, -0.5]) 50 | RF_foot = np.array([0.3, -0.2, -0.5]) 51 | LH_foot = np.array([-0.3, 0.2, -0.5]) 52 | RH_foot = np.array([-0.3, -0.2, -0.5]) 53 | 54 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 55 | 56 | #contacts = contactsToStack[0:nc+1, :] 57 | #print contacts 58 | 59 | ''' parameters to be tuned''' 60 | g = 9.81 61 | trunk_mass = 85. 62 | mu = 0.5 63 | 64 | ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' 65 | stanceFeet = [1,1,1,1] 66 | 67 | randomSwingLeg = random.randint(0,3) 68 | tripleStance = True # if you want you can define a swing leg using this variable 69 | if tripleStance: 70 | print 'Swing leg', randomSwingLeg 71 | stanceFeet[randomSwingLeg] = 0 72 | print 'stanceLegs ' ,stanceFeet 73 | 74 | ''' now I define the normals to the surface of the contact points. By default they are all vertical now''' 75 | axisZ= array([[0.0], [0.0], [1.0]]) 76 | 77 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # LF 78 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # RF 79 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # LH 80 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # RH 81 | normals = np.vstack([n1, n2, n3, n4]) 82 | 83 | ''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg) 84 | HAA = Hip Abduction Adduction 85 | HFE = Hip Flextion Extension 86 | KFE = Knee Flextion Extension 87 | ''' 88 | LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 89 | RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 90 | LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 91 | RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE 92 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 93 | 94 | ''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.''' 95 | extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm 96 | 97 | comp_dyn = ComputationalDynamics() 98 | 99 | '''You now need to fill the 'params' object with all the relevant 100 | informations needed for the computation of the IP''' 101 | params = IterativeProjectionParameters() 102 | params.setContactsPosWF(contacts) 103 | params.setCoMPosWF(comWF) 104 | params.setTorqueLims(torque_limits) 105 | params.setActiveContacts(stanceFeet) 106 | params.setConstraintModes(constraint_mode_IP) 107 | params.setContactNormals(normals) 108 | params.setFrictionCoefficient(mu) 109 | params.setNumberOfFrictionConesEdges(ng) 110 | params.setTotalMass(trunk_mass) 111 | params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment 112 | 113 | ''' compute iterative projection 114 | Outputs of "iterative_projection_bretl" are: 115 | IP_points = resulting 2D vertices 116 | actuation_polygons = these are the vertices of the 3D force polytopes (one per leg) 117 | computation_time = how long it took to compute the iterative projection 118 | ''' 119 | IP_points, force_polytopes, IP_computation_time = comp_dyn.iterative_projection_bretl(params) 120 | 121 | ''' plotting Iterative Projection points ''' 122 | 123 | feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params) 124 | #print contact_forces 125 | #for i in range(0, np.size(contact_forces,0)): 126 | # for j in range(0,nc): 127 | # a = Arrow3D([contacts[j,0], contacts[j,0]+contact_forces[i,j*3]/200], [contacts[j,1], contacts[j,1]+contact_forces[i,j*3+1]/200],[contacts[j,2], contacts[j,2]+contact_forces[i,j*3+2]/200], mutation_scale=20, lw=3, arrowstyle="-|>", color="g") 128 | # ax.add_artist(a) 129 | 130 | #a1 = Arrow3D([0.0, 0.0],[ 0.0,0.0],[ 0.0, -trunk_mass*g/scaling_factor], mutation_scale=20, lw=3, arrowstyle="-|>", color="b") 131 | #ax.add_artist(a1) 132 | 133 | ''' plotting LP test points ''' 134 | #if np.size(feasible,0) != 0: 135 | # ax.scatter(feasible[:,0], feasible[:,1], feasible[:,2],c='g',s=50) 136 | #if np.size(unfeasible,0) != 0: 137 | # ax.scatter(unfeasible[:,0], unfeasible[:,1], unfeasible[:,2],c='r',s=50) 138 | 139 | ''' Vertex-based projection ''' 140 | vertexBasedProj = VertexBasedProjection() 141 | vertices2d, simplices = vertexBasedProj.project(params) 142 | 143 | #for simplex in simplices: 144 | # plt.plot(vertices2d[simplex, 0], vertices2d[simplex, 1], 'y-', linewidth=5.) 145 | 146 | plt.show() 147 | 148 | ''' Add 2D figure ''' 149 | #plt.figure() 150 | #h1 = plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15, label='feet') 151 | #if np.size(feasible,0) != 0: 152 | # h2 = plt.scatter(feasible[:,0], feasible[:,1],c='g',s=50, label='LP feasible') 153 | #if np.size(unfeasible,0) != 0: 154 | # h3 = plt.scatter(unfeasible[:,0], unfeasible[:,1],c='r',s=50, label='LP unfeasible') 155 | 156 | feasiblePointsSize = np.size(feasible,0) 157 | for j in range(0, feasiblePointsSize): 158 | if (feasible[j,2]<0.01)&(feasible[j,2]>-0.01): 159 | plt.scatter(feasible[j,0], feasible[j,1],c='g',s=50) 160 | lastFeasibleIndex = j 161 | unfeasiblePointsSize = np.size(unfeasible,0) 162 | 163 | for j in range(0, unfeasiblePointsSize): 164 | if (unfeasible[j,2]<0.01)&(unfeasible[j,2]>-0.01): 165 | plt.scatter(unfeasible[j,0], unfeasible[j,1],c='r',s=50) 166 | lastUnfeasibleIndex = j 167 | h2 = plt.scatter(feasible[lastFeasibleIndex,0], feasible[lastFeasibleIndex,1],c='g',s=50, label='LP feasible') 168 | h3 = plt.scatter(unfeasible[lastUnfeasibleIndex,0], unfeasible[lastUnfeasibleIndex,1],c='r',s=50, label='LP unfeasible') 169 | 170 | 171 | h4 = plotter.plot_polygon(np.transpose(IP_points), '--b','Iterative Projection') 172 | 173 | #i = 0 174 | #for simplex in simplices: 175 | # if (i==0): 176 | # h5 = plt.plot(vertices2d[simplex, 0], vertices2d[simplex, 1], 'y-', linewidth=5., label = 'Vertex-based projection') 177 | # else: 178 | # plt.plot(vertices2d[simplex, 0], vertices2d[simplex, 1], 'y-', linewidth=5.) 179 | # i+=1 180 | 181 | plt.grid() 182 | plt.xlabel("X [m]") 183 | plt.ylabel("Y [m]") 184 | plt.legend() 185 | #plt.legend((h1,h2,h3,h4,h5),('feet','Iterative Projection', 'LP feasible', 'LP unfeasible','Vertex-based projection')) 186 | plt.show() -------------------------------------------------------------------------------- /unit_tests/test_intersection.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Aug 15 19:22:30 2018 4 | 5 | @author: romeoorsolino 6 | """ 7 | 8 | from context import jet_leg 9 | from jet_leg.math_tools import Math 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import unittest 13 | 14 | class TestStringMethods(unittest.TestCase): 15 | 16 | epsilon = 10e-06 17 | 18 | def test_square_line_intersection_predefined_values(self): 19 | math = Math() 20 | #starting_point = np.array([np.random.randint(0,100)/100.0,np.random.randint(0,100)/100.0,0.0]) 21 | 22 | starting_point = np.array([0.0,0.0,0.0]) 23 | #print "starting point is: ",starting_point 24 | #angle = np.random.randint(0,600)/600.0 25 | search_direction = np.array([1.0,1.0,0.0]) 26 | vertices = np.array([[1.0,1.0,0.0], 27 | [1.0,-1.0,0.0], 28 | [-1.0,-1.0,0.0], 29 | [-1.0,1.0,0.0]]) 30 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 31 | expected_intersection_point = np.array([1.0, 1.0]) 32 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 33 | 34 | search_direction = np.array([-1.0,1.0,0.0]) 35 | vertices = np.array([[1.0,1.0,0.0], 36 | [1.0,-1.0,0.0], 37 | [-1.0,-1.0,0.0], 38 | [-1.0,1.0,0.0]]) 39 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 40 | expected_intersection_point = np.array([-1.0, 1.0]) 41 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 42 | 43 | search_direction = np.array([1.0,-1.0,0.0]) 44 | vertices = np.array([[1.0,1.0,0.0], 45 | [1.0,-1.0,0.0], 46 | [-1.0,-1.0,0.0], 47 | [-1.0,1.0,0.0]]) 48 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 49 | expected_intersection_point = np.array([1.0, -1.0]) 50 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 51 | 52 | search_direction = np.array([-1.0,-1.0,0.0]) 53 | vertices = np.array([[1.0,1.0,0.0], 54 | [1.0,-1.0,0.0], 55 | [-1.0,-1.0,0.0], 56 | [-1.0,1.0,0.0]]) 57 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 58 | expected_intersection_point = np.array([-1.0, -1.0]) 59 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 60 | 61 | search_direction = np.array([1.0,0.0,0.0]) 62 | vertices = np.array([[1.0,1.0,0.0], 63 | [1.0,-1.0,0.0], 64 | [-1.0,-1.0,0.0], 65 | [-1.0,1.0,0.0]]) 66 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 67 | expected_intersection_point = np.array([1.0, 0.0]) 68 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 69 | 70 | search_direction = np.array([0.0,1.0,0.0]) 71 | vertices = np.array([[1.0,1.0,0.0], 72 | [1.0,-1.0,0.0], 73 | [-1.0,-1.0,0.0], 74 | [-1.0,1.0,0.0]]) 75 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 76 | expected_intersection_point = np.array([0.0, 1.0]) 77 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 78 | 79 | search_direction = np.array([0.0,-1.0,0.0]) 80 | vertices = np.array([[1.0,1.0,0.0], 81 | [1.0,-1.0,0.0], 82 | [-1.0,-1.0,0.0], 83 | [-1.0,1.0,0.0]]) 84 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 85 | expected_intersection_point = np.array([0.0, -1.0]) 86 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 87 | 88 | search_direction = np.array([-1.0,0.0,0.0]) 89 | vertices = np.array([[1.0,1.0,0.0], 90 | [1.0,-1.0,0.0], 91 | [-1.0,-1.0,0.0], 92 | [-1.0,1.0,0.0]]) 93 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 94 | expected_intersection_point = np.array([-1.0, 0.0]) 95 | self.assertTrue((new_p - expected_intersection_point < self.epsilon).all()) 96 | return new_p, all_points 97 | 98 | def test_square_line_intersection_random_values(self): 99 | math = Math() 100 | 101 | for iter in range(0,10): 102 | starting_point = np.array([np.random.randint(-100,100)/100.0,np.random.randint(-100,100)/100.0,0.0]) 103 | 104 | #starting_point = np.array([0.0,0.0,0.0]) 105 | #print "starting point is: ",starting_point 106 | #angle = np.random.randint(0,600)/600.0 107 | search_direction = np.array([1.0,1.0,0.0]) 108 | vertices = np.array([[1.0,1.0,0.0], 109 | [1.0,-1.0,0.0], 110 | [-1.0,-1.0,0.0], 111 | [-1.0,1.0,0.0]]) 112 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 113 | expected_intersection_point = np.array([1.0, 1.0]) 114 | self.assertTrue((new_p[1] >= -new_p[0]).all()) 115 | 116 | 117 | for iter in range(0,10): 118 | starting_point = np.array([np.random.randint(-100,100)/100.0,np.random.randint(-100,100)/100.0,0.0]) 119 | 120 | #starting_point = np.array([0.0,0.0,0.0]) 121 | #print "starting point is: ",starting_point 122 | #angle = np.random.randint(0,600)/600.0 123 | search_direction = np.array([-1.0,-1.0,0.0]) 124 | vertices = np.array([[1.0,1.0,0.0], 125 | [1.0,-1.0,0.0], 126 | [-1.0,-1.0,0.0], 127 | [-1.0,1.0,0.0]]) 128 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 129 | expected_intersection_point = np.array([1.0, 1.0]) 130 | self.assertTrue((new_p[1] <= -new_p[0]).all()) 131 | 132 | for iter in range(0,10): 133 | starting_point = np.array([np.random.randint(-100,100)/100.0,np.random.randint(-100,100)/100.0,0.0]) 134 | 135 | #starting_point = np.array([0.0,0.0,0.0]) 136 | #print "starting point is: ",starting_point 137 | #angle = np.random.randint(0,600)/600.0 138 | search_direction = np.array([1.0,0.0,0.0]) 139 | vertices = np.array([[1.0,1.0,0.0], 140 | [1.0,-1.0,0.0], 141 | [-1.0,-1.0,0.0], 142 | [-1.0,1.0,0.0]]) 143 | new_p, all_points = math.find_polygon_segment_intersection(vertices, search_direction, starting_point) 144 | expected_intersection_point = np.array([1.0, 1.0]) 145 | self.assertTrue((new_p[0] > 0).all()) 146 | 147 | '''main''' 148 | #point, all_points = test_square_line_intersection() 149 | #print 'all points : ',all_points, point 150 | #plt.grid() 151 | #plt.plot(all_points[:,0], all_points[:,1], 'bo') 152 | #plt.plot(point[0], point[1], 'r^', markersize = 20) -------------------------------------------------------------------------------- /examples/static_equilibrium_check/check_stability_lp_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jun 12 10:54:31 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | import numpy as np 9 | 10 | from numpy import array 11 | from jet_leg.plotting.plotting_tools import Plotter 12 | import random 13 | from jet_leg.maths.math_tools import Math 14 | from jet_leg.dynamics.computational_dynamics import ComputationalDynamics 15 | from jet_leg.maths.iterative_projection_parameters import IterativeProjectionParameters 16 | 17 | import matplotlib.pyplot as plt 18 | from jet_leg.plotting.arrow3D import Arrow3D 19 | 20 | plt.close('all') 21 | math = Math() 22 | 23 | ''' Set the robot's name (either 'hyq', 'hyqreal' or 'anymal')''' 24 | robot_name = 'anymal' 25 | 26 | ''' number of generators, i.e. rays/edges used to linearize the friction cone ''' 27 | ng = 4 28 | 29 | ''' 30 | possible constraints for each foot: 31 | ONLY_ACTUATION = only joint-torque limits are enforces 32 | ONLY_FRICTION = only friction cone constraints are enforced 33 | FRICTION_AND_ACTUATION = both friction cone constraints and joint-torque limits 34 | ''' 35 | constraint_mode_IP = ['FRICTION_AND_ACTUATION', 36 | 'FRICTION_AND_ACTUATION', 37 | 'FRICTION_AND_ACTUATION', 38 | 'FRICTION_AND_ACTUATION'] 39 | 40 | # number of decision variables of the problem 41 | # n = nc*6 42 | comWF = np.array([0., 0., 0.0]) 43 | 44 | """ contact points in the World Frame""" 45 | LF_foot = np.array([0.3, 0.2, -0.4]) 46 | RF_foot = np.array([0.3, -0.2, -0.4]) 47 | LH_foot = np.array([-0.3, 0.2, -0.4]) 48 | RH_foot = np.array([-0.3, -0.2, -0.4]) 49 | 50 | contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 51 | 52 | # contacts = contactsToStack[0:nc+1, :] 53 | # print contacts 54 | 55 | ''' parameters to be tuned''' 56 | trunk_mass = 45. 57 | mu = 0.5 58 | 59 | ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' 60 | stanceFeet = [1, 1, 1, 1] 61 | 62 | randomSwingLeg = random.randint(0, 3) 63 | tripleStance = False # if you want you can define a swing leg using this variable 64 | if tripleStance: 65 | print 'Swing leg', randomSwingLeg 66 | stanceFeet[randomSwingLeg] = 0 67 | print 'stanceLegs ', stanceFeet 68 | 69 | ''' now I define the normals to the surface of the contact points. By default they are all vertical now''' 70 | axisZ = array([[0.0], [0.0], [1.0]]) 71 | 72 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LF 73 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RF 74 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # LH 75 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # RH 76 | normals = np.vstack([n1, n2, n3, n4]) 77 | 78 | ''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg) 79 | HAA = Hip Abduction Adduction 80 | HFE = Hip Flextion Extension 81 | KFE = Knee Flextion Extension 82 | ''' 83 | LF_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE 84 | RF_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE 85 | LH_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE 86 | RH_tau_lim = [40.0, 40.0, 40.0] # HAA, HFE, KFE 87 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 88 | 89 | ''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.''' 90 | extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm 91 | 92 | comp_dyn = ComputationalDynamics(robot_name) 93 | 94 | '''You now need to fill the 'params' object with all the relevant 95 | informations needed for the computation of the IP''' 96 | params = IterativeProjectionParameters() 97 | 98 | params.setContactsPosWF(contacts) 99 | params.setCoMPosWF(comWF) 100 | params.setTorqueLims(torque_limits) 101 | params.setActiveContacts(stanceFeet) 102 | params.setConstraintModes(constraint_mode_IP) 103 | params.setContactNormals(normals) 104 | params.setFrictionCoefficient(mu) 105 | params.setNumberOfFrictionConesEdges(ng) 106 | params.setTotalMass(trunk_mass) 107 | params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment 108 | 109 | # print "Inequalities", comp_dyn.ineq 110 | # print "actuation polygons" 111 | # print actuation_polygons 112 | 113 | '''I now check whether the given CoM configuration is stable or not''' 114 | isConfigurationStable, contactForces, forcePolytopes = comp_dyn.check_equilibrium(params) 115 | print isConfigurationStable 116 | print 'contact forces', contactForces 117 | 118 | '''Plotting the contact points in the 3D figure''' 119 | fig = plt.figure() 120 | ax = fig.add_subplot(111, projection='3d') 121 | ax.set_xlabel('X axis') 122 | ax.set_ylabel('Y axis') 123 | ax.set_zlabel('Z axis') 124 | 125 | nc = np.sum(stanceFeet) 126 | stanceID = params.getStanceIndex(stanceFeet) 127 | force_scaling_factor = 1500 128 | # plt.plot(contacts[0:nc,0],contacts[0:nc,1],'ko',markersize=15) 129 | fz_tot = 0.0 130 | for j in range(0, 131 | nc): # this will only show the contact positions and normals of the feet that are defined to be in stance 132 | idx = int(stanceID[j]) 133 | ax.scatter(contacts[idx, 0], contacts[idx, 1], contacts[idx, 2], c='b', s=100) 134 | '''CoM will be plotted in green if it is stable (i.e., if it is inside the feasible region''' 135 | if isConfigurationStable: 136 | ax.scatter(comWF[0], comWF[1], comWF[2], c='g', s=100) 137 | grf = contactForces[j * 3:j * 3 + 3] 138 | fz_tot += grf[2] 139 | 140 | ''' draw the set contact forces that respects the constraints''' 141 | b = Arrow3D([contacts[idx, 0], contacts[idx, 0] + grf[0] / force_scaling_factor], 142 | [contacts[idx, 1], contacts[idx, 1] + grf[1] / force_scaling_factor], 143 | [contacts[idx, 2], contacts[idx, 2] + grf[2] / force_scaling_factor], mutation_scale=20, lw=3, 144 | arrowstyle="-|>", 145 | color="b") 146 | ax.add_artist(b) 147 | else: 148 | ax.scatter(comWF[0], comWF[1], comWF[2], c='r', s=100) 149 | 150 | ''' draw 3D arrows corresponding to contact normals''' 151 | a = Arrow3D([contacts[idx, 0], contacts[idx, 0] + normals[idx, 0] / 10], 152 | [contacts[idx, 1], contacts[idx, 1] + normals[idx, 1] / 10], 153 | [contacts[idx, 2], contacts[idx, 2] + normals[idx, 2] / 10], mutation_scale=20, lw=3, arrowstyle="-|>", 154 | color="r") 155 | 156 | ''' The black spheres represent the projection of the contact points on the same plane of the feasible region''' 157 | ax.scatter(contacts[idx, 0], contacts[idx, 1], 0.0, c='k', s=100) 158 | ax.add_artist(a) 159 | 160 | print 'sum of vertical forces is', fz_tot 161 | 162 | ''' plotting Iterative Projection points ''' 163 | plotter = Plotter() 164 | for j in range(0, nc): # this will only show the force polytopes of the feet that are defined to be in stance 165 | idx = int(stanceID[j]) 166 | if (constraint_mode_IP[idx] == 'ONLY_ACTUATION') or (constraint_mode_IP[idx] == 'FRICTION_AND_ACTUATION'): 167 | plotter.plot_actuation_polygon(ax, forcePolytopes[idx], contacts[idx, :], force_scaling_factor) 168 | 169 | ''' 2D figure ''' 170 | plt.figure() 171 | for j in range(0, 172 | nc): # this will only show the contact positions and normals of the feet that are defined to be in stance 173 | idx = int(stanceID[j]) 174 | ''' The black spheres represent the projection of the contact points on the same plane of the feasible region''' 175 | h1 = plt.plot(contacts[idx, 0], contacts[idx, 1], 'ko', markersize=15, label='stance feet') 176 | 177 | 178 | '''CoM will be plotted in green if it is stable (i.e., if it is inside the feasible region''' 179 | if isConfigurationStable: 180 | plt.plot(comWF[0], comWF[1], 'go', markersize=15, label='CoM') 181 | else: 182 | plt.plot(comWF[0], comWF[1], 'ro', markersize=15, label='CoM') 183 | 184 | plt.grid() 185 | plt.xlabel("X [m]") 186 | plt.ylabel("Y [m]") 187 | plt.legend() 188 | plt.show() -------------------------------------------------------------------------------- /examples/foothold_planning/single_foothold_optimization_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Dec 11 19:42:07 2018 4 | 5 | @author: rorsolino 6 | """ 7 | 8 | # -*- coding: utf-8 -*- 9 | """ 10 | Created on Thu Oct 18 11:05:06 2018 11 | 12 | @author: rorsolino 13 | """ 14 | 15 | # -*- coding: utf-8 -*- 16 | """ 17 | Created on Tue Jun 12 10:54:31 2018 18 | 19 | @author: Romeo Orsolino 20 | """ 21 | 22 | import time 23 | import pylab 24 | import pypoman 25 | import numpy as np 26 | from copy import deepcopy 27 | 28 | from context import jet_leg 29 | import matplotlib.colors as colors 30 | import matplotlib.cm as cmx 31 | 32 | from numpy import array 33 | from numpy.linalg import norm 34 | from jet_leg.plotting_tools import Plotter 35 | import random 36 | from jet_leg.math_tools import Math 37 | from jet_leg.computational_dynamics import ComputationalDynamics 38 | from jet_leg.iterative_projection_parameters import IterativeProjectionParameters 39 | from jet_leg.foothold_planning_interface import FootholdPlanningInterface 40 | from jet_leg.foothold_planning import FootHoldPlanning 41 | 42 | import matplotlib.pyplot as plt 43 | from jet_leg.arrow3D import Arrow3D 44 | 45 | plt.close('all') 46 | math = Math() 47 | # number of contacts 48 | #nc = 3 49 | # number of generators, i.e. rays used to linearize the friction cone 50 | ng = 4 51 | 52 | # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION 53 | 54 | constraint_mode_IP = ['FRICTION_AND_ACTUATION', 55 | 'FRICTION_AND_ACTUATION', 56 | 'FRICTION_AND_ACTUATION', 57 | 'FRICTION_AND_ACTUATION'] 58 | useVariableJacobian = False 59 | # number of decision variables of the problem 60 | #n = nc*6 61 | # contact positions 62 | """ contact points """ 63 | 64 | LF_foot = np.array([0.3, 0.2, -0.0]) 65 | RF_foot = np.array([0.3, -0.2, -0.0]) 66 | LH_foot = np.array([-0.3, 0.2, -0.0]) 67 | RH_foot = np.array([-0.3, -0.2, -0.0]) 68 | 69 | feetPos = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 70 | contacts =deepcopy(feetPos) 71 | 72 | #contacts = contactsToStack[0:nc+1, :] 73 | #print contacts 74 | 75 | ''' parameters to be tuned''' 76 | g = 9.81 77 | trunk_mass = 95. 78 | mu = 0.9 79 | 80 | stanceFeet = [1,1,1,0] 81 | randomSwingLeg = random.randint(0,3) 82 | print 'Swing leg', randomSwingLeg 83 | print 'stanceLegs ' ,stanceFeet 84 | 85 | axisZ= array([[0.0], [0.0], [1.0]]) 86 | 87 | n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 88 | n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 89 | n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 90 | n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) 91 | normals = np.vstack([n1, n2, n3, n4]) 92 | 93 | LF_tau_lim = [50.0, 100.0, 100.0] 94 | RF_tau_lim = [50.0, 100.0, 100.0] 95 | LH_tau_lim = [50.0, 100.0, 100.0] 96 | RH_tau_lim = [50.0, 100.0, 100.0] 97 | torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) 98 | 99 | 100 | 101 | nc = np.sum(stanceFeet) 102 | 103 | comp_dyn = ComputationalDynamics() 104 | params = IterativeProjectionParameters() 105 | params.setContactsPosWF(contacts) 106 | params.setTorqueLims(torque_limits) 107 | params.setActiveContacts(stanceFeet) 108 | params.setConstraintModes(constraint_mode_IP) 109 | params.setContactNormals(normals) 110 | params.setFrictionCoefficient(mu) 111 | params.setNumberOfFrictionConesEdges(ng) 112 | params.setTotalMass(trunk_mass) 113 | 114 | #inputs for foothold planning 115 | foothold_params = FootholdPlanningInterface() 116 | foothold_params.com_position_to_validateW = [0.2, 0.05, 0.55] 117 | 118 | 119 | maxCorrection = 0.2 120 | #square 121 | #predictedLF_foot = np.add(LF_foot,np.array([0.1,0.3,0.0])) 122 | #footOption0 = [0., 0., 0.] + predictedLF_foot 123 | #footOption1 = [0.,res, 0.] + predictedLF_foot 124 | #footOption2 = [res, 0., 0.] + predictedLF_foot 125 | #footOption3 = [res, res, 0.] + predictedLF_foot 126 | #footOption4 = [-res, res, 0.] + predictedLF_foot 127 | #footOption5 = [res, -res, 0.] + predictedLF_foot 128 | #footOption6 = [-res, -res, 0.] + predictedLF_foot 129 | #footOption7 = [0., -res, 0.] + predictedLF_foot 130 | #footOption8 = [-res, 0., 0.] + predictedLF_foot 131 | 132 | #y 133 | #predictedLF_foot = np.add(LF_foot,np.array([0.1,0.3,0.0])) 134 | #footOption4 = [0., 0., 0.] + predictedLF_foot 135 | #footOption3 = [0, -bound*1/4, 0.] + predictedLF_foot 136 | #footOption2 = [0, -bound*2/4, 0.] + predictedLF_foot 137 | #footOption1 = [0., -bound*3/4, 0.] + predictedLF_foot 138 | #footOption0 = [0, -bound*4/4., 0.] + predictedLF_foot 139 | #footOption5 = [0.,bound*1/4, 0.] + predictedLF_foot 140 | #footOption6 = [0, bound*2/4, 0.] + predictedLF_foot 141 | #footOption7 = [0, bound*3/4, 0.] + predictedLF_foot 142 | #footOption8 = [0, bound*4/4, 0.] + predictedLF_foot 143 | 144 | #x 145 | predictedLF_foot = np.add(LF_foot,np.array([0.1,0.15,0.0])) 146 | footOption4 = [0., 0., 0.] + predictedLF_foot 147 | footOption3 = [ -maxCorrection*1/4, 0., 0.] + predictedLF_foot 148 | footOption2 = [ -maxCorrection*2/4, 0., 0.] + predictedLF_foot 149 | footOption1 = [ -maxCorrection*3/4, 0., 0.] + predictedLF_foot 150 | footOption0 = [ -maxCorrection*4/4., 0., 0.] + predictedLF_foot 151 | footOption5 = [ maxCorrection*1/4, 0., 0.] + predictedLF_foot 152 | footOption6 = [ maxCorrection*2/4, 0.,0.] + predictedLF_foot 153 | footOption7 = [ maxCorrection*3/4, 0.,0.] + predictedLF_foot 154 | footOption8 = [ maxCorrection*4/4, 0., 0.] + predictedLF_foot 155 | 156 | foothold_params.minRadius = 0.2 157 | 158 | 159 | foothold_params.footOptions = np.array([footOption0, 160 | footOption1, 161 | footOption2, 162 | footOption3, 163 | footOption4, 164 | footOption5, 165 | footOption6, 166 | footOption7, 167 | footOption8]) 168 | 169 | ''' compute iterative projection ''' 170 | print 'contacts', feetPos 171 | footHoldPlanning = FootHoldPlanning() 172 | #chosen_foothold, actuationRegions = footHoldPlanning.selectMaximumFeasibleArea(footPlanningParams, params) 173 | 174 | foothold_params.option_index, stackedResidualRadius, actuationRegions, mapFootHoldIdxToPolygonIdx = footHoldPlanning.selectMinumumRequiredFeasibleAreaResidualRadius( foothold_params, params) 175 | print 'residual radius ', stackedResidualRadius 176 | print 'feet options', foothold_params.footOptions 177 | print 'final index', foothold_params.option_index 178 | print 'list of indices', mapFootHoldIdxToPolygonIdx 179 | #IP_points, actuation_polygons, computation_time = comp_dyn.iterative_projection_bretl(params) 180 | #print 'foot option ',chosen_foothold 181 | 182 | lowel_lim = -10 183 | upper_lim = 10 184 | 185 | scale = np.linspace(lowel_lim, upper_lim, np.size(actuationRegions)) 186 | jet = cm = plt.get_cmap('seismic') 187 | cNorm = colors.Normalize(vmin=-30, vmax=35) 188 | scalarMap = cmx.ScalarMappable(norm=cNorm, cmap=jet) 189 | idx = 0 190 | #print 'contacts', feetPos 191 | h0 = plt.plot(foothold_params.footOptions[foothold_params.option_index][0],foothold_params.footOptions[foothold_params.option_index][1],'^', color = 'r', markersize=20) 192 | h1 = plt.plot(feetPos[0:nc,0],feetPos[0:nc,1],'ks',markersize=15, label='feet') 193 | 194 | for polygon in actuationRegions: 195 | colorVal = scalarMap.to_rgba(scale[idx]) 196 | x = polygon[:,0] 197 | y = polygon[:,1] 198 | h = plt.plot(x,y, color = colorVal, linewidth=5.) 199 | # print foothold_params.footOptions[idx] 200 | h2 = plt.plot(foothold_params.footOptions[mapFootHoldIdxToPolygonIdx[idx]][0],foothold_params.footOptions[mapFootHoldIdxToPolygonIdx[idx]][1],'o', color = colorVal, markersize=15) 201 | idx += 1 202 | h3 = plt.plot(params.getCoMPosWF()[0],params.getCoMPosWF()[1],'or',markersize=25) 203 | 204 | 205 | plt.grid() 206 | plt.xlabel("X [m]") 207 | plt.ylabel("Y [m]") 208 | plt.axis('equal') 209 | plt.legend() 210 | plt.show()# -*- coding: utf-8 -*- 211 | -------------------------------------------------------------------------------- /unit_tests/test_hyq_kinematics.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Mon Jul 2 06:10:18 2018 4 | 5 | @author: Romeo Orsolino 6 | """ 7 | 8 | from context import jet_leg 9 | 10 | from jet_leg.robot.hyq_kinematics import HyQKinematics 11 | 12 | import numpy as np 13 | import random 14 | import time 15 | 16 | import unittest 17 | 18 | class TestStringMethods(): 19 | 20 | assertPrecision = 3 21 | 22 | def test_leg_ikpy(self): 23 | epsilon = 1e-03 24 | LF_foot = np.array([0.3735, 0.33, -0.5]) 25 | RF_foot = np.array([0.3735, -0.33, -0.5]) 26 | LH_foot = np.array([-0.3735, 0.33, -0.5]) 27 | RH_foot = np.array([-0.3735, -0.33, -0.5]) 28 | starting_contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) 29 | hyqKin = HyQKinematics() 30 | foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]) 31 | 32 | for legID in range(0,4): 33 | q_leg_ikpy = hyqKin.leg_inverse_kin_ikpy(legID, starting_contacts) 34 | hyqKin.init_jacobians() 35 | hyqKin.init_homogeneous() 36 | 37 | q = hyqKin.inverse_kin(starting_contacts, foot_vel) 38 | q_leg_hardcoded = q[3*legID +0: 3*legID + 3] 39 | self.assertTrue(q_leg_ikpy[0] - q_leg_hardcoded[0] < epsilon) 40 | self.assertTrue(q_leg_ikpy[1] - q_leg_hardcoded[1] < epsilon) 41 | self.assertTrue(q_leg_ikpy[2] - q_leg_hardcoded[2] < epsilon) 42 | 43 | def test_kinematic_loop_default_configuration(self): 44 | 45 | random.seed(9001) 46 | hyqKin = HyQKinematics() 47 | hyqKin.init_jacobians() 48 | hyqKin.init_homogeneous() 49 | foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]]) 50 | LF_foot = np.array([0.3735, 0.207, -0.6]) 51 | RF_foot = np.array([0.3735, -0.207, -0.6]) 52 | LH_foot = np.array([-0.3735, 0.207, -0.6]) 53 | RH_foot = np.array([-0.3735, -0.207, -0.6]) 54 | starting_contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 55 | 56 | q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = hyqKin.inverse_kin(np.transpose(starting_contacts[:,0]), 57 | np.transpose(foot_vel[:,0]), 58 | np.transpose(starting_contacts[:,1]), 59 | np.transpose(foot_vel[:,1]), 60 | np.transpose(starting_contacts[:,2]), 61 | np.transpose(foot_vel[:,2])) 62 | 63 | hyqKin.update_homogeneous(q) 64 | hyqKin.update_jacobians(q) 65 | new_contacts = hyqKin.forward_kin(q) 66 | 67 | self.assertTrue((LF_foot - new_contacts[0,:] < self.epsilon).all()) 68 | self.assertTrue((RF_foot - new_contacts[1,:] < self.epsilon).all()) 69 | self.assertTrue((LH_foot - new_contacts[2,:] < self.epsilon).all()) 70 | self.assertTrue((RH_foot - new_contacts[3,:] < self.epsilon).all()) 71 | 72 | def test_position_is_out_of_workspace(self): 73 | 74 | random.seed(9001) 75 | for j in range(0,10): 76 | hyqKin = HyQKinematics() 77 | 78 | hyqKin.init_jacobians() 79 | hyqKin.init_homogeneous() 80 | 81 | foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]]) 82 | 83 | foot_z = random.uniform(-1,-0.8) 84 | LF_foot = np.array([0.3, 0.2, foot_z]) 85 | RF_foot = np.array([0.3, -0.2, foot_z]) 86 | LH_foot = np.array([-0.3, 0.2, foot_z]) 87 | RH_foot = np.array([-0.3, -0.2, foot_z]) 88 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 89 | 90 | q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = hyqKin.inverse_kin(np.transpose(contacts[:,0]), 91 | np.transpose(foot_vel[:,0]), 92 | np.transpose(contacts[:,1]), 93 | np.transpose(foot_vel[:,1]), 94 | np.transpose(contacts[:,2]), 95 | np.transpose(foot_vel[:,2])) 96 | self.assertTrue(isOutOfWorkspace) 97 | 98 | def test_inverse_kinematics(self): 99 | 100 | hyqKin = HyQKinematics() 101 | 102 | hyqKin.init_jacobians() 103 | hyqKin.init_homogeneous() 104 | 105 | foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]]) 106 | 107 | LF_foot = np.array([0.3, 0.2, -0.5]) 108 | RF_foot = np.array([0.3, -0.2, -0.58]) 109 | LH_foot = np.array([-0.3, 0.2, -0.58]) 110 | RH_foot = np.array([-0.3, -0.2, -0.58]) 111 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 112 | #print contacts 113 | 114 | start_time = time.time() 115 | q = hyqKin.inverse_kin(contacts, foot_vel) 116 | print('total time', time.time()- start_time) 117 | print q 118 | hyqKin.update_homogeneous(q) 119 | hyqKin.update_jacobians(q) 120 | new_contacts = hyqKin.forward_kin(q) 121 | print new_contacts 122 | 123 | #self.assertTrue((LF_foot - new_contacts[0,:] < self.epsilon).all()) 124 | #self.assertTrue((RF_foot - new_contacts[1,:] < self.epsilon).all()) 125 | #self.assertTrue((LH_foot - new_contacts[2,:] < self.epsilon).all()) 126 | #self.assertTrue((RH_foot - new_contacts[3,:] < self.epsilon).all()) 127 | 128 | new_q = hyqKin.inverse_kin(new_contacts, foot_vel) 129 | 130 | print new_q 131 | 132 | #self.assertTrue( (q - new_q < self.epsilon).all()) 133 | 134 | 135 | 136 | def test_forward_kinematics(self): 137 | 138 | hyqKin = HyQKinematics() 139 | 140 | hyqKin.init_jacobians() 141 | hyqKin.init_homogeneous() 142 | 143 | foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]]) 144 | 145 | LF_foot = np.array([0.3, 0.2, -0.58]) 146 | RF_foot = np.array([0.3, -0.2, -0.58]) 147 | LH_foot = np.array([-0.3, 0.2, -0.58]) 148 | RH_foot = np.array([-0.3, -0.2, -0.58]) 149 | contacts = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) 150 | 151 | #print contacts 152 | 153 | q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = hyqKin.inverse_kin(np.transpose(contacts[:,0]), 154 | np.transpose(foot_vel[:,0]), 155 | np.transpose(contacts[:,1]), 156 | np.transpose(foot_vel[:,1]), 157 | np.transpose(contacts[:,2]), 158 | np.transpose(foot_vel[:,2])) 159 | 160 | #print q 161 | hyqKin.update_homogeneous(q) 162 | hyqKin.update_jacobians(q) 163 | new_contacts = hyqKin.forward_kin(q) 164 | #print new_contacts 165 | 166 | self.assertTrue((LF_foot - new_contacts[0,:] < self.epsilon).all()) 167 | self.assertTrue((RF_foot - new_contacts[1,:] < self.epsilon).all()) 168 | self.assertTrue((LH_foot - new_contacts[2,:] < self.epsilon).all()) 169 | self.assertTrue((RH_foot - new_contacts[3,:] < self.epsilon).all()) 170 | 171 | #self.assertEqual(LF_foot.tolist(), new_contacts[0,:].tolist()) 172 | #self.assertEqual(RF_foot.tolist(), new_contacts[1,:].tolist()) 173 | #self.assertEqual(LH_foot.tolist(), new_contacts[2,:].tolist()) 174 | #self.assertEqual(RH_foot.tolist(), new_contacts[3,:].tolist()) 175 | 176 | 177 | '''main''' 178 | test = TestStringMethods() 179 | test.test_inverse_kinematics() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /jet_leg/robots/hyq/urdf/hyq.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | --------------------------------------------------------------------------------