├── pages ├── __init__.py ├── page_landing.py ├── page_patterns.py ├── helpers.py ├── page_inverse.py ├── shared.py └── page_kinematics.py ├── tests ├── __init__.py ├── test_misc.py ├── README.md ├── test_kinematics.py ├── test_patterns.py ├── helpers.py ├── ik_cases │ ├── case1.py │ ├── case2.py │ └── case3.py ├── test_ik.py ├── pattern_cases │ ├── case1.py │ └── case2.py └── kinematics_cases │ ├── case1.py │ └── case2.py ├── hexapod ├── __init__.py ├── templates │ ├── pose_template.py │ └── figure_template.py ├── ik_solver │ ├── shared.py │ ├── recompute_hexapod.py │ ├── helpers.py │ ├── README.md │ ├── ik_solver.py │ └── ik_solver2.py ├── const.py ├── ground_contact_solver │ ├── shared.py │ ├── ground_contact_solver2.py │ └── ground_contact_solver.py ├── plotter.py ├── linkage.py ├── points.py └── models.py ├── widgets ├── __init__.py ├── pose_control │ ├── generic_input_ui.py │ ├── generic_daq_slider_ui.py │ ├── components.py │ ├── generic_slider_ui.py │ ├── kinematics_section_maker.py │ └── joint_widget_maker.py ├── section_maker.py ├── leg_patterns_ui.py ├── dimensions_ui.py └── ik_ui.py ├── Procfile ├── .gitignore ├── assets └── favicon.ico ├── app.py ├── .flake8 ├── checker ├── texts.py ├── requirements.txt ├── .travis.yml ├── .codeclimate.yml ├── LICENSE ├── settings.py ├── index.py ├── style_settings.py ├── README.md ├── external_css ├── dark.css └── light.css └── .pylintrc /pages/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /hexapod/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /widgets/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Procfile: -------------------------------------------------------------------------------- 1 | web: gunicorn index:server 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | **/__pycache__ 3 | .vscode/settings.json 4 | venv 5 | .env 6 | *.pyc -------------------------------------------------------------------------------- /assets/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mithi/hexapod-robot-simulator/HEAD/assets/favicon.ico -------------------------------------------------------------------------------- /app.py: -------------------------------------------------------------------------------- 1 | import os 2 | import dash 3 | from texts import APP_TITLE 4 | from style_settings import EXTERNAL_STYLESHEETS 5 | 6 | app = dash.Dash(__name__, external_stylesheets=EXTERNAL_STYLESHEETS) 7 | app.title = APP_TITLE 8 | server = app.server 9 | server.secret_key = os.environ.get("secret_key", "secret") 10 | app.config.suppress_callback_exceptions = True 11 | -------------------------------------------------------------------------------- /pages/page_landing.py: -------------------------------------------------------------------------------- 1 | import dash_html_components as html 2 | from texts import URL_IMG_LANDING 3 | 4 | img = html.Img(src=URL_IMG_LANDING, style={"width": "100%", "height": "auto"},) 5 | 6 | layout = html.Div( 7 | [ 8 | html.Div(img, style={"width": "20%", "height": "auto"}), 9 | html.Div(html.Br(), style={"width": "auto", "height": "auto"}), 10 | ], 11 | style={"display": "flex", "flex-direction": "row"}, 12 | ) 13 | -------------------------------------------------------------------------------- /tests/test_misc.py: -------------------------------------------------------------------------------- 1 | from settings import ( 2 | DEBUG_MODE, 3 | ASSERTION_ENABLED, 4 | PRINT_IK_LOCAL_LEG, 5 | PRINT_IK, 6 | PRINT_MODEL_ON_UPDATE, 7 | RECOMPUTE_HEXAPOD, 8 | ) 9 | 10 | 11 | def test_deploy_minimum(): 12 | assert not DEBUG_MODE 13 | assert not ASSERTION_ENABLED 14 | assert not PRINT_IK_LOCAL_LEG 15 | assert not PRINT_IK 16 | assert not PRINT_MODEL_ON_UPDATE 17 | assert RECOMPUTE_HEXAPOD 18 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | ignore = E203, E266, E501, W503 3 | # See https://github.com/psf/black/blob/master/README.md#line-length for more details 4 | max-line-length = 90 5 | max-complexity = 18 6 | select = B,C,E,F,W,T4,B9 7 | # We need to configure the mypy.ini because the flake8-mypy's default 8 | # options don't properly override it, so if we don't specify it we get 9 | # half of the config from mypy.ini and half from flake8-mypy. 10 | mypy_config = mypy.ini -------------------------------------------------------------------------------- /checker: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | black . 3 | python -m pytest 4 | python -m py_compile ./*/*.py ./*.py ./*/*/*.py 5 | flake8 ./*/*.py ./*.py ./*/*/*.py 6 | pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code 7 | -------------------------------------------------------------------------------- /widgets/pose_control/generic_input_ui.py: -------------------------------------------------------------------------------- 1 | from widgets.pose_control.joint_widget_maker import ( 2 | make_all_joint_widgets, 3 | make_number_widget, 4 | ) 5 | from widgets.pose_control.kinematics_section_maker import make_section 6 | 7 | # ................................ 8 | # COMPONENTS 9 | # ................................ 10 | 11 | widgets = make_all_joint_widgets(joint_input_function=make_number_widget) 12 | KINEMATICS_WIDGETS_SECTION = make_section(widgets, add_joint_names=True) 13 | -------------------------------------------------------------------------------- /widgets/pose_control/generic_daq_slider_ui.py: -------------------------------------------------------------------------------- 1 | from widgets.pose_control.joint_widget_maker import ( 2 | make_all_joint_widgets, 3 | make_daq_slider, 4 | ) 5 | from widgets.pose_control.kinematics_section_maker import make_section 6 | 7 | # ................................ 8 | # COMPONENTS 9 | # ................................ 10 | 11 | widgets = make_all_joint_widgets(joint_input_function=make_daq_slider) 12 | KINEMATICS_WIDGETS_SECTION = make_section( 13 | widgets, style_to_use={"padding": "0 0 0 3em"} 14 | ) 15 | -------------------------------------------------------------------------------- /tests/README.md: -------------------------------------------------------------------------------- 1 | 2 | # Inverse Kinematics Edge Cases 3 | 4 | - Coxia point shoved on ground 5 | - Body contact shoved on ground 6 | - Can't reach target ground point 7 | - Femur length is too long 8 | - Tibia length is too long 9 | - The ground is blocking the path 10 | - Legs too short 11 | - Too many legs off the floor 12 | - All left legs off the ground 13 | - All right legs off the ground 14 | - Angle required is beyond range of motion 15 | - Alpha 16 | - Beta 17 | - Gamma 18 | 19 | # `VirtualHexapod.Update` Edge Cases 20 | 21 | - Unstable. Center of gravity is outside the hexapod's support polygon -------------------------------------------------------------------------------- /texts.py: -------------------------------------------------------------------------------- 1 | APP_TITLE = "Mithi's Hexapod Robot Simulator" 2 | 3 | URL_KOFI = "https://ko-fi.com/minimithi" 4 | URL_REPO = "https://github.com/mithi/hexapod-robot-simulator" 5 | URL_IMG_LANDING = "https://mithi.github.io/robotics-blog/v2-hexapod-1.gif" 6 | 7 | KINEMATICS_PAGE_PATH = "/kinematics" 8 | IK_PAGE_PATH = "/inverse-kinematics" 9 | PATTERNS_PAGE_PATH = "/leg-patterns" 10 | ROOT_PATH = "/" 11 | 12 | DIMENSIONS_WIDGETS_HEADER = "robot dimensions".upper() 13 | PATTERNS_WIDGETS_HEADER = "leg patterns".upper() 14 | IK_WIDGETS_HEADER = "inverse kinematics".upper() 15 | KINEMATICS_WIDGETS_HEADER = "kinematics".upper() 16 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | astroid==2.4.2 2 | Brotli==1.0.9 3 | click==7.1.2 4 | dash==1.18.1 5 | dash-core-components==1.14.1 6 | dash-daq==0.5.0 7 | dash-html-components==1.1.1 8 | dash-renderer==1.8.3 9 | dash-table==4.11.1 10 | flake8==3.8.4 11 | Flask==1.1.2 12 | Flask-Compress==1.8.0 13 | future==0.18.3 14 | gunicorn==20.0.4 15 | isort==5.7.0 16 | itsdangerous==1.1.0 17 | Jinja2==2.11.3 18 | lazy-object-proxy==1.4.3 19 | MarkupSafe==1.1.1 20 | mccabe==0.6.1 21 | numpy==1.22.0 22 | plotly==4.14.1 23 | pycodestyle==2.6.0 24 | pyflakes==2.2.0 25 | pylint==2.6.0 26 | retrying==1.3.3 27 | six==1.15.0 28 | toml==0.10.2 29 | Werkzeug==2.2.3 30 | wrapt==1.12.1 31 | -------------------------------------------------------------------------------- /tests/test_kinematics.py: -------------------------------------------------------------------------------- 1 | from hexapod.models import VirtualHexapod 2 | from tests.kinematics_cases import case1, case2 3 | from tests.helpers import assert_hexapod_points_equal 4 | 5 | CASES = [case1, case2] 6 | 7 | 8 | def assert_kinematics(case, assume_ground_targets): 9 | hexapod = VirtualHexapod(case.given_dimensions) 10 | hexapod.update(case.given_poses, assume_ground_targets) 11 | assert_hexapod_points_equal( 12 | hexapod, case.correct_body_points, case.correct_leg_points, case.description 13 | ) 14 | 15 | 16 | def test_sample_kinematics(): 17 | for case in CASES: 18 | assert_kinematics(case, True) 19 | assert_kinematics(case, False) 20 | -------------------------------------------------------------------------------- /hexapod/templates/pose_template.py: -------------------------------------------------------------------------------- 1 | # pose = { 2 | # LEG_ID: { 3 | # 'name': LEG_NAME, 4 | # 'id': LEG_ID 5 | # 'coxia': ALPHA, 6 | # 'femur': BETA, 7 | # 'tibia': GAMMA} 8 | # } 9 | # ... 10 | # } 11 | 12 | HEXAPOD_POSE = { 13 | 0: {"coxia": 0, "femur": 0, "tibia": 0, "name": "right-middle", "id": 0}, 14 | 1: {"coxia": 0, "femur": 0, "tibia": 0, "name": "right-front", "id": 1}, 15 | 2: {"coxia": 0, "femur": 0, "tibia": 0, "name": "left-front", "id": 2}, 16 | 3: {"coxia": 0, "femur": 0, "tibia": 0, "name": "left-middle", "id": 3}, 17 | 4: {"coxia": 0, "femur": 0, "tibia": 0, "name": "left-back", "id": 4}, 18 | 5: {"coxia": 0, "femur": 0, "tibia": 0, "name": "right-back", "id": 5}, 19 | } 20 | -------------------------------------------------------------------------------- /tests/test_patterns.py: -------------------------------------------------------------------------------- 1 | from pages.helpers import make_pose 2 | from hexapod.models import VirtualHexapod 3 | from tests.pattern_cases import case1, case2 4 | from tests.helpers import assert_hexapod_points_equal 5 | 6 | 7 | def test_sample_patterns(): 8 | for case in [case1, case2]: 9 | for assume_ground_targets in [True, False]: 10 | poses = make_pose(case.alpha, case.beta, case.gamma) 11 | hexapod = VirtualHexapod(case.given_dimensions) 12 | hexapod.update(poses, assume_ground_targets) 13 | assert_hexapod_points_equal( 14 | hexapod, 15 | case.correct_body_points, 16 | case.correct_leg_points, 17 | case.description, 18 | ) 19 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Disable sudo to speed up the build 2 | sudo: false 3 | 4 | language: python 5 | python: 6 | - "3.8.1" 7 | cache: pip 8 | install: 9 | - pip install -r requirements.txt 10 | - pip install codecov 11 | script: 12 | - python -m py_compile ./*/*.py ./*.py ./*/*/*.py 13 | - flake8 ./*/*.py ./*.py ./*/*/*.py 14 | - pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code 15 | #- python -m pytest 16 | - coverage run -m pytest 17 | - coverage report -m 18 | 19 | # Push the results back to codecov 20 | after_success: 21 | - codecov 22 | -------------------------------------------------------------------------------- /hexapod/ik_solver/shared.py: -------------------------------------------------------------------------------- 1 | # Functions used for solving inverse kinematics 2 | from hexapod.points import is_counter_clockwise, angle_between, rotz 3 | 4 | 5 | def update_hexapod_points(hexapod, leg_id, points): 6 | leg = hexapod.legs[leg_id] 7 | for point, leg_point in zip(points, leg.all_points): 8 | point.name = leg_point.name 9 | leg.all_points = points 10 | 11 | 12 | def find_twist_frame(hexapod, unit_coxia_vector): 13 | twist = angle_between(unit_coxia_vector, hexapod.x_axis) 14 | is_ccw = is_counter_clockwise(unit_coxia_vector, hexapod.x_axis, hexapod.z_axis) 15 | if is_ccw: 16 | twist = -twist 17 | 18 | twist_frame = rotz(twist) 19 | return twist, twist_frame 20 | 21 | 22 | def compute_twist_wrt_to_world(alpha, coxia_axis): 23 | alpha = (alpha - coxia_axis) % 360 24 | if alpha > 180: 25 | return alpha - 360 26 | return alpha 27 | -------------------------------------------------------------------------------- /widgets/pose_control/components.py: -------------------------------------------------------------------------------- 1 | import dash_core_components as dcc 2 | import dash_html_components as html 3 | from dash.dependencies import Input 4 | from texts import KINEMATICS_WIDGETS_HEADER 5 | from hexapod.const import NAMES_LEG, NAMES_JOINT 6 | 7 | 8 | def make_joint_callback_inputs_of_one_leg(leg_name): 9 | return [ 10 | Input(f"widget-{leg_name}-{joint_name}", "value") for joint_name in NAMES_JOINT 11 | ] 12 | 13 | 14 | def make_all_joint_callback_inputs(): 15 | callback_inputs = [] 16 | for leg_name in NAMES_LEG: 17 | callback_inputs += make_joint_callback_inputs_of_one_leg(leg_name) 18 | return callback_inputs 19 | 20 | 21 | # ................................ 22 | # COMPONENTS 23 | # ................................ 24 | 25 | HEADER = html.Label(dcc.Markdown(f"**{KINEMATICS_WIDGETS_HEADER}**")) 26 | KINEMATICS_CALLBACK_INPUTS = make_all_joint_callback_inputs() 27 | -------------------------------------------------------------------------------- /.codeclimate.yml: -------------------------------------------------------------------------------- 1 | version: "2" # required to adjust maintainability checks 2 | exclude_patterns: 3 | - "hexapod/plotter.py" 4 | - "hexapod/templates/figure_template.py" 5 | - "tests/*_cases/*.py" 6 | 7 | checks: 8 | argument-count: 9 | config: 10 | threshold: 6 # 4 11 | complex-logic: 12 | config: 13 | threshold: 4 14 | file-lines: 15 | config: 16 | threshold: 250 17 | method-complexity: 18 | config: 19 | threshold: 6 20 | method-count: 21 | config: 22 | threshold: 20 23 | method-lines: 24 | config: 25 | threshold: 25 26 | nested-control-flow: 27 | config: 28 | threshold: 4 29 | return-statements: 30 | config: 31 | threshold: 4 32 | similar-code: 33 | config: 34 | threshold: # language-specific defaults. an override will affect all languages. 35 | identical-code: 36 | config: 37 | threshold: # language-specific defaults. an override will affect all languages. -------------------------------------------------------------------------------- /hexapod/const.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from hexapod.plotter import HexapodPlotter 3 | from hexapod.models import VirtualHexapod, Hexagon, Linkage 4 | from hexapod.templates.figure_template import HEXAPOD_FIGURE 5 | from hexapod.templates.pose_template import HEXAPOD_POSE 6 | 7 | NAMES_LEG = Hexagon.VERTEX_NAMES 8 | NAMES_JOINT = Linkage.POINT_NAMES 9 | 10 | BASE_DIMENSIONS = { 11 | "front": 100, 12 | "side": 100, 13 | "middle": 100, 14 | "coxia": 100, 15 | "femur": 100, 16 | "tibia": 100, 17 | } 18 | 19 | 20 | BASE_IK_PARAMS = { 21 | "hip_stance": 0, 22 | "leg_stance": 0, 23 | "percent_x": 0, 24 | "percent_y": 0, 25 | "percent_z": 0, 26 | "rot_x": 0, 27 | "rot_y": 0, 28 | "rot_z": 0, 29 | } 30 | 31 | BASE_POSE = deepcopy(HEXAPOD_POSE) 32 | 33 | BASE_HEXAPOD = VirtualHexapod(BASE_DIMENSIONS) 34 | BASE_PLOTTER = HexapodPlotter() 35 | 36 | HEXAPOD = deepcopy(BASE_HEXAPOD) 37 | HEXAPOD.update(HEXAPOD_POSE) 38 | BASE_FIGURE = deepcopy(HEXAPOD_FIGURE) 39 | BASE_PLOTTER.update(BASE_FIGURE, HEXAPOD) 40 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Mithi Sevilla 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /widgets/pose_control/generic_slider_ui.py: -------------------------------------------------------------------------------- 1 | import dash_core_components as dcc 2 | import dash_html_components as html 3 | from hexapod.const import NAMES_LEG 4 | from widgets.section_maker import make_section_type4 5 | from widgets.pose_control.joint_widget_maker import ( 6 | make_all_joint_widgets, 7 | make_slider, 8 | ) 9 | from widgets.pose_control.components import HEADER 10 | 11 | 12 | def make_leg_sections(jwidgets): 13 | widget_sections = [] 14 | header_section = make_section_type4( 15 | "", html.H5("coxia"), html.H5("femur"), html.H5("tibia") 16 | ) 17 | widget_sections.append(header_section) 18 | 19 | for leg in NAMES_LEG: 20 | header = html.Label(dcc.Markdown("**`{}`**".format(leg))) 21 | coxia = jwidgets[leg]["coxia"] 22 | femur = jwidgets[leg]["femur"] 23 | tibia = jwidgets[leg]["tibia"] 24 | section = make_section_type4(header, coxia, femur, tibia) 25 | widget_sections.append(section) 26 | 27 | return html.Div(widget_sections) 28 | 29 | 30 | # ................................ 31 | # COMPONENTS 32 | # ................................ 33 | 34 | widgets = make_all_joint_widgets(joint_input_function=make_slider) 35 | sections = make_leg_sections(widgets) 36 | KINEMATICS_WIDGETS_SECTION = html.Div([HEADER, sections]) 37 | -------------------------------------------------------------------------------- /widgets/pose_control/kinematics_section_maker.py: -------------------------------------------------------------------------------- 1 | import dash_core_components as dcc 2 | import dash_html_components as html 3 | from widgets.section_maker import make_section_type3, make_section_type2 4 | from widgets.pose_control.components import HEADER 5 | 6 | 7 | def make_section(joint_widgets, add_joint_names=False, style_to_use=None): 8 | names = [ 9 | "left-front", 10 | "right-front", 11 | "left-middle", 12 | "right-middle", 13 | "left-back", 14 | "right-back", 15 | ] 16 | 17 | lf, rf, lm, rm, lb, rb = [ 18 | make_leg_section(name, joint_widgets, add_joint_names) for name in names 19 | ] 20 | 21 | widget_sections = html.Div( 22 | [ 23 | make_section_type2(lf, rf), 24 | make_section_type2(lm, rm), 25 | make_section_type2(lb, rb), 26 | ], 27 | style=style_to_use or {}, 28 | ) 29 | 30 | return html.Div([HEADER, widget_sections]) 31 | 32 | 33 | def code(name): 34 | return dcc.Markdown(f"`{name}`") 35 | 36 | 37 | def make_leg_section(name, joint_widgets, add_joint_names=False): 38 | header = html.Label(dcc.Markdown(f"( `{name.upper()}` )")) 39 | coxia = joint_widgets[name]["coxia"] 40 | femur = joint_widgets[name]["femur"] 41 | tibia = joint_widgets[name]["tibia"] 42 | 43 | if add_joint_names: 44 | section = make_section_type3( 45 | coxia, femur, tibia, code("coxia"), code("femur"), code("tibia") 46 | ) 47 | else: 48 | section = make_section_type3(coxia, femur, tibia) 49 | 50 | return html.Div([header, section]) 51 | -------------------------------------------------------------------------------- /widgets/section_maker.py: -------------------------------------------------------------------------------- 1 | # Used to make html divisions 2 | import dash_html_components as html 3 | 4 | 5 | def make_section_type3(div1, div2, div3, name1="", name2="", name3=""): 6 | return html.Div( 7 | [ 8 | html.Div([div1, name1], style={"width": "33%"}), 9 | html.Div([div2, name2], style={"width": "33%"}), 10 | html.Div([div3, name3], style={"width": "33%"}), 11 | ], 12 | style={"display": "flex"}, 13 | ) 14 | 15 | 16 | def make_section_type4(div1, div2, div3, div4): 17 | return html.Div( 18 | [ 19 | html.Div(div1, style={"width": "16%"}), 20 | html.Div(div2, style={"width": "28%"}), 21 | html.Div(div3, style={"width": "28%"}), 22 | html.Div(div4, style={"width": "28%"}), 23 | ], 24 | style={"display": "flex"}, 25 | ) 26 | 27 | 28 | def make_section_type2(div1, div2): 29 | return html.Div( 30 | [ 31 | html.Div(div1, style={"width": "50%"}), 32 | html.Div(div2, style={"width": "50%"}), 33 | ], 34 | style={"display": "flex"}, 35 | ) 36 | 37 | 38 | def make_section_type6(div1, div2, div3, div4, div5, div6): 39 | return html.Div( 40 | [ 41 | html.Div(div1, style={"width": "17%"}), 42 | html.Div(div2, style={"width": "17%"}), 43 | html.Div(div3, style={"width": "17%"}), 44 | html.Div(div4, style={"width": "17%"}), 45 | html.Div(div5, style={"width": "16%"}), 46 | html.Div(div6, style={"width": "16%"}), 47 | ], 48 | style={"display": "flex"}, 49 | ) 50 | -------------------------------------------------------------------------------- /tests/helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def assert_poses_equal(result_poses, correct_poses, description): 5 | for k, v in result_poses.items(): 6 | msg = f"Unequal Poses\n{correct_poses[k]}\n{v}\n(case: {description})" 7 | assert correct_poses[k]["name"] == v["name"] 8 | assert correct_poses[k]["id"] == v["id"] 9 | assert np.isclose(correct_poses[k]["coxia"], v["coxia"]), msg 10 | assert np.isclose(correct_poses[k]["femur"], v["femur"]), msg 11 | assert np.isclose(correct_poses[k]["tibia"], v["tibia"]), msg 12 | 13 | 14 | def assert_hexapod_points_equal( 15 | hexapod, correct_body_points, correct_leg_points, description 16 | ): 17 | def msg(a, b): 18 | return f"Unequal Vectors\nexpected: {a}\n....found: {b}\n(case: {description})" 19 | 20 | for point_a, point_b in zip(correct_body_points, hexapod.body.all_points): 21 | assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) 22 | 23 | for leg_set, leg in zip(correct_leg_points, hexapod.legs): 24 | for point_a, point_b in zip(leg_set, leg.all_points): 25 | assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) 26 | 27 | 28 | def assert_two_hexapods_equal(hexapod1, hexapod2, description): 29 | def msg(a, b): 30 | return f"Unequal Vectors\n1: {a}\n2:{b}\n(case: {description})" 31 | 32 | for point_a, point_b in zip(hexapod1.body.all_points, hexapod2.body.all_points): 33 | assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) 34 | 35 | for leg_a, leg_b in zip(hexapod1.legs, hexapod2.legs): 36 | for point_a, point_b in zip(leg_a.all_points, leg_b.all_points): 37 | assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) 38 | -------------------------------------------------------------------------------- /settings.py: -------------------------------------------------------------------------------- 1 | # *************************** 2 | # Settings 3 | # *************************** 4 | 5 | # The range of each leg joint in degrees 6 | ALPHA_MAX_ANGLE = 90 7 | BETA_MAX_ANGLE = 180 8 | GAMMA_MAX_ANGLE = 180 9 | BODY_MAX_ANGLE = 40 10 | 11 | # LEG STANCE 12 | # would define the starting leg position used to compute 13 | # the target ground contact for inverse kinematics poses 14 | # femur/ beta = -leg_stance 15 | # tibia/ gamma = leg_stance 16 | LEG_STANCE_MAX_ANGLE = 90 17 | 18 | # HIP STANCE 19 | # would defined the starting hip position used to compute 20 | # the target ground contact for inverse kinematics poses 21 | # coxia/alpha angle of 22 | # right_front = -hip_stance 23 | # left_front = +hip_stance 24 | # left_back = -hip_stance 25 | # right_back = +hip_stance 26 | # left_middle = 0 27 | # right_middle = 0 28 | HIP_STANCE_MAX_ANGLE = 45 29 | 30 | # Too slow? set UPDATE_MODE='mouseup' 31 | # Makes widgets only start updating when you release the mouse button 32 | UPDATE_MODE = "drag" 33 | 34 | DEBUG_MODE = False 35 | ASSERTION_ENABLED = False 36 | 37 | # The inverse kinematics solver already updates the points of the hexapod 38 | # But there is no guarantee that this pose is correct 39 | # So better update a fresh hexapod with the resulting poses 40 | RECOMPUTE_HEXAPOD = True 41 | 42 | PRINT_IK_LOCAL_LEG = False 43 | PRINT_IK = False 44 | PRINT_MODEL_ON_UPDATE = False 45 | 46 | # 1 - Use the daq slider UI 47 | # 2 - Use the generic slider UI 48 | # Anything else defaults to the generic input UI, which I prefer 49 | WHICH_POSE_CONTROL_UI = 0 50 | 51 | # Make it more granular to prevent overloading the server 52 | SLIDER_ANGLE_RESOLUTION = 1.5 53 | INPUT_DIMENSIONS_RESOLUTION = 1 54 | 55 | UI_GRAPH_HEIGHT = "600px" 56 | UI_GRAPH_WIDTH = "63%" 57 | UI_SIDEBAR_WIDTH = "37%" 58 | -------------------------------------------------------------------------------- /widgets/leg_patterns_ui.py: -------------------------------------------------------------------------------- 1 | # Widgets used to set the leg pose of all legs uniformly 2 | import dash_core_components as dcc 3 | import dash_html_components as html 4 | from dash.dependencies import Input 5 | import dash_daq 6 | from texts import PATTERNS_WIDGETS_HEADER 7 | from settings import ( 8 | ALPHA_MAX_ANGLE, 9 | BETA_MAX_ANGLE, 10 | GAMMA_MAX_ANGLE, 11 | UPDATE_MODE, 12 | SLIDER_ANGLE_RESOLUTION, 13 | ) 14 | from style_settings import SLIDER_THEME, SLIDER_HANDLE_COLOR, SLIDER_COLOR 15 | 16 | 17 | def make_slider(slider_id, name, max_angle): 18 | 19 | handle_style = { 20 | "showCurrentValue": True, 21 | "color": SLIDER_HANDLE_COLOR, 22 | "label": name, 23 | } 24 | 25 | daq_slider = dash_daq.Slider( # pylint: disable=not-callable 26 | id=slider_id, 27 | min=-max_angle, 28 | max=max_angle, 29 | value=1.5, 30 | step=SLIDER_ANGLE_RESOLUTION, 31 | size=300, 32 | updatemode=UPDATE_MODE, 33 | handleLabel=handle_style, 34 | color={"default": SLIDER_COLOR}, 35 | theme=SLIDER_THEME, 36 | ) 37 | 38 | return html.Div(daq_slider, style={"padding": "2em"}) 39 | 40 | 41 | # ................................ 42 | # COMPONENTS 43 | # ................................ 44 | 45 | HEADER = html.Label(dcc.Markdown(f"**{PATTERNS_WIDGETS_HEADER}**")) 46 | WIDGET_NAMES = ["alpha", "beta", "gamma"] 47 | PATTERNS_WIDGET_IDS = [f"widget-{name}" for name in WIDGET_NAMES] 48 | PATTERNS_CALLBACK_INPUTS = [Input(i, "value") for i in PATTERNS_WIDGET_IDS] 49 | 50 | max_angles = [ALPHA_MAX_ANGLE, BETA_MAX_ANGLE, GAMMA_MAX_ANGLE] 51 | widgets = [ 52 | make_slider(id, name, angle) 53 | for id, name, angle in zip(PATTERNS_WIDGET_IDS, WIDGET_NAMES, max_angles) 54 | ] 55 | PATTERNS_WIDGETS_SECTION = html.Div([HEADER] + widgets) 56 | -------------------------------------------------------------------------------- /widgets/dimensions_ui.py: -------------------------------------------------------------------------------- 1 | # Widgets used to set the dimensions of the hexapod 2 | import dash_core_components as dcc 3 | import dash_html_components as html 4 | from dash.dependencies import Input 5 | from texts import DIMENSIONS_WIDGETS_HEADER 6 | from settings import INPUT_DIMENSIONS_RESOLUTION 7 | from style_settings import NUMBER_INPUT_STYLE 8 | from widgets.section_maker import make_section_type3 9 | 10 | 11 | def make_number_widget(_name, _value): 12 | return dcc.Input( 13 | id=_name, 14 | type="number", 15 | value=_value, 16 | min=0, 17 | step=INPUT_DIMENSIONS_RESOLUTION, 18 | style=NUMBER_INPUT_STYLE, 19 | ) 20 | 21 | 22 | def _code(name): 23 | return dcc.Markdown(f"`{name}`") 24 | 25 | 26 | # ................................ 27 | # COMPONENTS 28 | # ................................ 29 | 30 | HEADER = html.Label(dcc.Markdown(f"**{DIMENSIONS_WIDGETS_HEADER}**")) 31 | WIDGET_NAMES = ["front", "side", "middle", "coxia", "femur", "tibia"] 32 | DIMENSION_WIDGET_IDS = [f"widget-dimension-{name}" for name in WIDGET_NAMES] 33 | DIMENSION_CALLBACK_INPUTS = [Input(id, "value") for id in DIMENSION_WIDGET_IDS] 34 | 35 | widgets = [make_number_widget(widget_id, 100) for widget_id in DIMENSION_WIDGET_IDS] 36 | sections = [ 37 | make_section_type3( 38 | widgets[0], 39 | widgets[1], 40 | widgets[2], 41 | _code(WIDGET_NAMES[0]), 42 | _code(WIDGET_NAMES[1]), 43 | _code(WIDGET_NAMES[2]), 44 | ), 45 | make_section_type3( 46 | widgets[3], 47 | widgets[4], 48 | widgets[5], 49 | _code(WIDGET_NAMES[3]), 50 | _code(WIDGET_NAMES[4]), 51 | _code(WIDGET_NAMES[5]), 52 | ), 53 | ] 54 | 55 | DIMENSIONS_WIDGETS_SECTION = html.Div( 56 | [HEADER, html.Div(sections, style={"display": "flex"}), html.Br()] 57 | ) 58 | -------------------------------------------------------------------------------- /pages/page_patterns.py: -------------------------------------------------------------------------------- 1 | import json 2 | from dash.dependencies import Output 3 | from app import app 4 | from hexapod.models import VirtualHexapod 5 | from hexapod.const import BASE_PLOTTER 6 | from widgets.leg_patterns_ui import PATTERNS_WIDGETS_SECTION, PATTERNS_CALLBACK_INPUTS 7 | from pages import helpers, shared 8 | 9 | 10 | # ...................... 11 | # Page layout 12 | # ...................... 13 | 14 | GRAPH_ID = "graph-patterns" 15 | MESSAGE_SECTION_ID = "message-patterns" 16 | PARAMETERS_SECTION_ID = "parameters-pattens" 17 | 18 | sidebar = shared.make_standard_page_sidebar( 19 | MESSAGE_SECTION_ID, PARAMETERS_SECTION_ID, PATTERNS_WIDGETS_SECTION 20 | ) 21 | 22 | layout = shared.make_standard_page_layout(GRAPH_ID, sidebar) 23 | 24 | 25 | # ...................... 26 | # Update page 27 | # ...................... 28 | 29 | outputs, inputs, states = shared.make_standard_page_callback_params( 30 | GRAPH_ID, PARAMETERS_SECTION_ID, MESSAGE_SECTION_ID 31 | ) 32 | 33 | 34 | @app.callback(outputs, inputs, states) 35 | def update_patterns_page(dimensions_json, poses_json, relayout_data, figure): 36 | 37 | dimensions = helpers.load_params(dimensions_json, "dims") 38 | poses = helpers.load_params(poses_json, "pose") 39 | hexapod = VirtualHexapod(dimensions) 40 | 41 | try: 42 | hexapod.update(poses) 43 | except Exception as alert: 44 | return figure, helpers.make_alert_message(alert) 45 | 46 | BASE_PLOTTER.update(figure, hexapod) 47 | helpers.change_camera_view(figure, relayout_data) 48 | return figure, "" 49 | 50 | 51 | # ...................... 52 | # Update parameters 53 | # ...................... 54 | 55 | output_parameter = Output(PARAMETERS_SECTION_ID, "children") 56 | input_parameters = PATTERNS_CALLBACK_INPUTS 57 | 58 | 59 | @app.callback(output_parameter, input_parameters) 60 | def update_poses_alpha_beta_gamma(alpha, beta, gamma): 61 | return json.dumps(helpers.make_pose(alpha, beta, gamma)) 62 | -------------------------------------------------------------------------------- /tests/ik_cases/case1.py: -------------------------------------------------------------------------------- 1 | description = "IK Random Pose #1" 2 | 3 | # ******************************** 4 | # Dimensions 5 | # ******************************** 6 | 7 | given_dimensions = { 8 | "front": 70, 9 | "side": 115, 10 | "middle": 120, 11 | "coxia": 60, 12 | "femur": 130, 13 | "tibia": 150, 14 | } 15 | 16 | # ******************************** 17 | # IK Parameters 18 | # ******************************** 19 | 20 | given_ik_parameters = { 21 | "hip_stance": 7, 22 | "leg_stance": 32, 23 | "percent_x": 0.35, 24 | "percent_y": 0.25, 25 | "percent_z": -0.2, 26 | "rot_x": 2.5, 27 | "rot_y": -9, 28 | "rot_z": 14, 29 | } 30 | 31 | # ******************************** 32 | # Poses 33 | # ******************************** 34 | 35 | correct_poses = { 36 | 0: { 37 | "name": "right-middle", 38 | "id": 0, 39 | "coxia": -36.89755490432384, 40 | "femur": 26.276957259313683, 41 | "tibia": -38.39772518650969, 42 | }, 43 | 1: { 44 | "name": "right-front", 45 | "id": 1, 46 | "coxia": -31.715493484789533, 47 | "femur": 27.717090725335396, 48 | "tibia": -41.67638594657396, 49 | }, 50 | 2: { 51 | "name": "left-front", 52 | "id": 2, 53 | "coxia": -3.1127758531426934, 54 | "femur": 64.38109364320302, 55 | "tibia": -41.751719577946915, 56 | }, 57 | 3: { 58 | "name": "left-middle", 59 | "id": 3, 60 | "coxia": -14.447799823858816, 61 | "femur": 64.61701942138204, 62 | "tibia": -27.21279908137491, 63 | }, 64 | 4: { 65 | "name": "left-back", 66 | "id": 4, 67 | "coxia": -27.925865837440085, 68 | "femur": 57.5357711909659, 69 | "tibia": -15.824751016445546, 70 | }, 71 | 5: { 72 | "name": "right-back", 73 | "id": 5, 74 | "coxia": -34.07683786865073, 75 | "femur": 40.018917104647784, 76 | "tibia": -36.78650126914302, 77 | }, 78 | } 79 | -------------------------------------------------------------------------------- /tests/ik_cases/case2.py: -------------------------------------------------------------------------------- 1 | description = "IK Random Pose #2" 2 | 3 | # ******************************** 4 | # Dimensions 5 | # ******************************** 6 | 7 | given_dimensions = { 8 | "front": 76, 9 | "side": 114, 10 | "middle": 125, 11 | "coxia": 63, 12 | "femur": 142, 13 | "tibia": 171, 14 | } 15 | 16 | # ******************************** 17 | # IK Parameters 18 | # ******************************** 19 | 20 | given_ik_parameters = { 21 | "hip_stance": 10.5, 22 | "leg_stance": 25.5, 23 | "percent_x": 0.3, 24 | "percent_y": 0.05, 25 | "percent_z": -0.15, 26 | "rot_x": -1, 27 | "rot_y": 12.5, 28 | "rot_z": -8.5, 29 | } 30 | 31 | # ******************************** 32 | # Poses 33 | # ******************************** 34 | 35 | correct_poses = { 36 | 0: { 37 | "name": "right-middle", 38 | "id": 0, 39 | "coxia": 13.43107675540267, 40 | "femur": 77.7924770301091, 41 | "tibia": -60.647267530564136, 42 | }, 43 | 1: { 44 | "name": "right-front", 45 | "id": 1, 46 | "coxia": 14.431630572348844, 47 | "femur": 66.33077021197852, 48 | "tibia": -57.05016213919879, 49 | }, 50 | 2: { 51 | "name": "left-front", 52 | "id": 2, 53 | "coxia": 30.081030614307394, 54 | "femur": 11.619722581700444, 55 | "tibia": -1.6253249676582442, 56 | }, 57 | 3: { 58 | "name": "left-middle", 59 | "id": 3, 60 | "coxia": 14.685705676958776, 61 | "femur": 3.177447435672022, 62 | "tibia": 4.800985597502901, 63 | }, 64 | 4: { 65 | "name": "left-back", 66 | "id": 4, 67 | "coxia": -0.9115332668632732, 68 | "femur": 9.309907080761477, 69 | "tibia": -4.315425166528982, 70 | }, 71 | 5: { 72 | "name": "right-back", 73 | "id": 5, 74 | "coxia": 14.382064236242854, 75 | "femur": 59.28098836470138, 76 | "tibia": -49.19348454681213, 77 | }, 78 | } 79 | -------------------------------------------------------------------------------- /tests/ik_cases/case3.py: -------------------------------------------------------------------------------- 1 | description = "IK Pose where x, y translation, rot y and z are close to extreme" 2 | 3 | # ******************************** 4 | # Dimensions 5 | # ******************************** 6 | 7 | given_dimensions = { 8 | "front": 73, 9 | "side": 100, 10 | "middle": 130, 11 | "coxia": 75, 12 | "femur": 129, 13 | "tibia": 154, 14 | } 15 | 16 | # ******************************** 17 | # IK Parameters 18 | # ******************************** 19 | 20 | given_ik_parameters = { 21 | "hip_stance": 10.5, 22 | "leg_stance": 30, 23 | "percent_x": 0.7, 24 | "percent_y": -0.4, 25 | "percent_z": 0.2, 26 | "rot_x": 1.5, 27 | "rot_y": -16, 28 | "rot_z": -14.5, 29 | } 30 | 31 | # ******************************** 32 | # Poses 33 | # ******************************** 34 | 35 | correct_poses = { 36 | 0: { 37 | "name": "right-middle", 38 | "id": 0, 39 | "coxia": 55.56073526445866, 40 | "femur": -24.206649398630788, 41 | "tibia": -8.608209643253758, 42 | }, 43 | 1: { 44 | "name": "right-front", 45 | "id": 1, 46 | "coxia": 51.072817817160114, 47 | "femur": -5.7010123660724545, 48 | "tibia": -7.5181271777452565, 49 | }, 50 | 2: { 51 | "name": "left-front", 52 | "id": 2, 53 | "coxia": 33.84438606600443, 54 | "femur": 30.76781937225195, 55 | "tibia": 16.92773639721497, 56 | }, 57 | 3: { 58 | "name": "left-middle", 59 | "id": 3, 60 | "coxia": 13.117976527545807, 61 | "femur": 48.11622597324919, 62 | "tibia": -0.4754969993002618, 63 | }, 64 | 4: { 65 | "name": "left-back", 66 | "id": 4, 67 | "coxia": -8.445805980297905, 68 | "femur": 53.09741126074167, 69 | "tibia": -21.001329834229722, 70 | }, 71 | 5: { 72 | "name": "right-back", 73 | "id": 5, 74 | "coxia": 26.002763598408308, 75 | "femur": -17.792174423794933, 76 | "tibia": -25.026074825755416, 77 | }, 78 | } 79 | -------------------------------------------------------------------------------- /tests/test_ik.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from hexapod.const import BASE_DIMENSIONS 3 | from hexapod.models import VirtualHexapod 4 | from hexapod.points import Vector 5 | from hexapod.ik_solver import ik_solver, ik_solver2 6 | from hexapod.ik_solver.shared import update_hexapod_points 7 | 8 | from tests.ik_cases import case1, case2, case3 9 | from tests.helpers import assert_poses_equal, assert_two_hexapods_equal 10 | 11 | CASES = [case1, case2, case3] 12 | 13 | 14 | def assert_ik_solver(ik_function, case): 15 | hexapod = VirtualHexapod(case.given_dimensions) 16 | result_poses, _ = ik_function(hexapod, case.given_ik_parameters) 17 | assert_poses_equal(result_poses, case.correct_poses, case.description) 18 | 19 | 20 | def assert_ik_points(case, assume_ground_targets): 21 | hexapod = VirtualHexapod(case.given_dimensions) 22 | hexapod_ik = deepcopy(hexapod) 23 | hexapod_k = deepcopy(hexapod) 24 | 25 | poses, _ = ik_solver2.inverse_kinematics_update(hexapod, case.given_ik_parameters) 26 | 27 | hexapod_ik.update(poses, assume_ground_targets) 28 | hexapod_k.update(case.correct_poses, assume_ground_targets) 29 | 30 | assert_two_hexapods_equal(hexapod_ik, hexapod_k, case.description) 31 | 32 | 33 | def test_sample_ik(): 34 | for case in CASES: 35 | assert_ik_solver(ik_solver2.inverse_kinematics_update, case) 36 | assert_ik_solver(ik_solver.inverse_kinematics_update, case) 37 | assert_ik_solver(ik_solver2.inverse_kinematics_update, case) 38 | assert_ik_solver(ik_solver.inverse_kinematics_update, case) 39 | 40 | 41 | def test_points_ik2_assume_ground_targets(): 42 | for case in CASES: 43 | assert_ik_points(case, True) 44 | 45 | 46 | def test_points_ik2_dont_assume_ground_targets(): 47 | for case in CASES: 48 | assert_ik_points(case, False) 49 | 50 | 51 | def test_shared_set_points(): 52 | points = [ 53 | Vector(1, 2, 3, "a"), 54 | Vector(1, 2, 3, "b"), 55 | Vector(1, 2, 3, "c"), 56 | Vector(1, 2, 3, "d"), 57 | ] 58 | 59 | vh = VirtualHexapod(BASE_DIMENSIONS) 60 | update_hexapod_points(vh, 1, points) 61 | for leg_point, point in zip(vh.legs[1].all_points, points): 62 | assert leg_point is point 63 | -------------------------------------------------------------------------------- /pages/helpers.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | import json 3 | import dash_core_components as dcc 4 | from hexapod.const import ( 5 | BASE_PLOTTER, 6 | BASE_POSE, 7 | BASE_IK_PARAMS, 8 | BASE_DIMENSIONS, 9 | NAMES_LEG, 10 | ) 11 | 12 | NEW_POSES = deepcopy(BASE_POSE) 13 | POSES_MSG_HEADER = """ 14 | +----------------+------------+------------+------------+ 15 | | leg name | coxia | femur | tibia | 16 | +----------------+------------+------------+------------+""" 17 | POSES_MSG_LAST_ROW = "\n+----------------+------------+------------+------------+" 18 | 19 | 20 | def make_pose(alpha, beta, gamma, poses=NEW_POSES): 21 | 22 | for k in poses.keys(): 23 | poses[k] = { 24 | "id": k, 25 | "name": NAMES_LEG[k], 26 | "coxia": alpha, 27 | "femur": beta, 28 | "tibia": gamma, 29 | } 30 | return poses 31 | 32 | 33 | def change_camera_view(figure, relayout_data): 34 | if relayout_data and "scene.camera" in relayout_data: 35 | camera = relayout_data["scene.camera"] 36 | BASE_PLOTTER.change_camera_view(figure, camera) 37 | 38 | return figure 39 | 40 | 41 | def load_params(params_json, params_type): 42 | try: 43 | params = json.loads(params_json) 44 | except Exception as e: 45 | print(f"Error loading json of type {params_type}. {e} | {params_json}") 46 | 47 | if params_type == "dims": 48 | return BASE_DIMENSIONS 49 | if params_type == "pose": 50 | return BASE_POSE 51 | if params_type == "ik": 52 | return BASE_IK_PARAMS 53 | 54 | raise Exception( 55 | f'params_type must be "dims", "pose" or "ik", not {params_type}' 56 | ) from e 57 | 58 | return params 59 | 60 | 61 | def make_monospace(text): 62 | return dcc.Markdown(f" ```{text}") 63 | 64 | 65 | def make_poses_message(poses): 66 | message = POSES_MSG_HEADER 67 | 68 | for pose in poses.values(): 69 | name = pose["name"] 70 | coxia = pose["coxia"] 71 | femur = pose["femur"] 72 | tibia = pose["tibia"] 73 | row = f"\n| {name:14} | {coxia:<+10.2f} | {femur:<+10.2f} | {tibia:<+10.2f} |" 74 | message += row 75 | 76 | return make_monospace(message + POSES_MSG_LAST_ROW) 77 | 78 | 79 | def make_alert_message(alert): 80 | return make_monospace(f"❗❗❗ALERT❗❗❗\n⚠️ {alert} 🔴") 81 | -------------------------------------------------------------------------------- /hexapod/ground_contact_solver/shared.py: -------------------------------------------------------------------------------- 1 | from math import isclose 2 | from hexapod.points import ( 3 | Vector, 4 | dot, 5 | cross, 6 | vector_from_to, 7 | ) 8 | 9 | # Prioritize legs that are not adjacent to each other 10 | SOME_LEG_TRIOS = [ 11 | (0, 1, 3), 12 | (0, 1, 4), 13 | (0, 2, 3), 14 | (0, 2, 4), 15 | (0, 2, 5), 16 | (0, 3, 4), 17 | (0, 3, 5), 18 | (1, 2, 4), 19 | (1, 2, 5), 20 | (1, 3, 4), 21 | (1, 3, 5), 22 | (1, 4, 5), 23 | (2, 3, 5), 24 | (2, 4, 5), 25 | ] 26 | 27 | ADJACENT_LEG_TRIOS = [ 28 | (0, 1, 2), 29 | (1, 2, 3), 30 | (2, 3, 4), 31 | (3, 4, 5), 32 | (0, 4, 5), 33 | (0, 1, 5), 34 | ] 35 | 36 | LEG_TRIOS = SOME_LEG_TRIOS + ADJACENT_LEG_TRIOS 37 | 38 | 39 | # math.stackexchange.com/questions/544946/ 40 | # determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle 41 | # gamedev.stackexchange.com/questions/23743/ 42 | # whats-the-most-efficient-way-to-find-barycentric-coordinates 43 | # en.wikipedia.org/wiki/Barycentric_coordinate_system 44 | def is_stable(p1, p2, p3, tol=0.001): 45 | """ 46 | Determines stability of the pose. 47 | Determine if projection of 3D point p 48 | onto the plane defined by p1, p2, p3 49 | is within a triangle defined by p1, p2, p3. 50 | """ 51 | p = Vector(0, 0, 0) 52 | u = vector_from_to(p1, p2) 53 | v = vector_from_to(p1, p3) 54 | n = cross(u, v) 55 | w = vector_from_to(p1, p) 56 | n2 = dot(n, n) 57 | beta = dot(cross(u, w), n) / n2 58 | gamma = dot(cross(w, v), n) / n2 59 | alpha = 1 - gamma - beta 60 | # then coordinate of the projected point (p_) of point p 61 | # p_ = alpha * p1 + beta * p2 + gamma * p3 62 | min_val = -tol 63 | max_val = 1 + tol 64 | cond1 = min_val <= alpha <= max_val 65 | cond2 = min_val <= beta <= max_val 66 | cond3 = min_val <= gamma <= max_val 67 | return cond1 and cond2 and cond3 68 | 69 | 70 | def is_lower(point, height, n, tol=1): 71 | _height = -dot(n, point) 72 | return _height > height + tol 73 | 74 | 75 | def find_legs_on_ground(legs, n, height, tol=1): 76 | legs_on_ground = [] 77 | for leg in legs: 78 | for point in reversed(leg.all_points[1:]): 79 | _height = -dot(n, point) 80 | if isclose(height, _height, abs_tol=tol): 81 | legs_on_ground.append(leg) 82 | break 83 | 84 | return legs_on_ground 85 | -------------------------------------------------------------------------------- /widgets/pose_control/joint_widget_maker.py: -------------------------------------------------------------------------------- 1 | # Used to build the widgets for changing the joint angles 2 | import dash_core_components as dcc 3 | import dash_daq 4 | from hexapod.const import NAMES_JOINT, NAMES_LEG 5 | from settings import ( 6 | ALPHA_MAX_ANGLE, 7 | BETA_MAX_ANGLE, 8 | GAMMA_MAX_ANGLE, 9 | UPDATE_MODE, 10 | ) 11 | from style_settings import ( 12 | NUMBER_INPUT_STYLE, 13 | SLIDER_THEME, 14 | SLIDER_HANDLE_COLOR, 15 | SLIDER_COLOR, 16 | ) 17 | 18 | max_angles = { 19 | "coxia": ALPHA_MAX_ANGLE, 20 | "femur": BETA_MAX_ANGLE, 21 | "tibia": GAMMA_MAX_ANGLE, 22 | } 23 | 24 | 25 | # widget id format: 26 | # 'widget' + '-' + leg_x + '-' + leg_y + '-' leg_joint 27 | # leg_x = ['left', 'right'] 28 | # leg_y = ['front', 'middle', 'back'] 29 | # leg_joint = ['coxia', 'femur', 'tibia'] 30 | # input dictionary structure 31 | # all_joint_widgets['left-front']['coxia'] = joint_widget 32 | # all_joint_widgets['right-middle']['femur'] = joint_widget 33 | def make_all_joint_widgets(joint_input_function): 34 | all_joint_widgets = {} 35 | 36 | for leg_name in NAMES_LEG: 37 | leg_joint_widget = {} 38 | 39 | for joint_name in NAMES_JOINT: 40 | widget_id = "widget-{}-{}".format(leg_name, joint_name) 41 | leg_joint_widget[joint_name] = joint_input_function( 42 | widget_id, max_angles[joint_name] 43 | ) 44 | 45 | all_joint_widgets[leg_name] = leg_joint_widget 46 | 47 | return all_joint_widgets 48 | 49 | 50 | def make_daq_slider(widget_id, max_angle): 51 | _, _, _, angle = widget_id.split("-") 52 | 53 | handle_style = { 54 | "showCurrentValue": True, 55 | "color": SLIDER_HANDLE_COLOR, 56 | "label": angle, 57 | } 58 | 59 | return dash_daq.Slider( # pylint: disable=not-callable 60 | id=widget_id, 61 | min=-max_angle, 62 | max=max_angle, 63 | value=1.5, 64 | step=1.5, 65 | size=80, 66 | vertical=True, 67 | updatemode=UPDATE_MODE, 68 | handleLabel=handle_style, 69 | color={"default": SLIDER_COLOR}, 70 | theme=SLIDER_THEME, 71 | ) 72 | 73 | 74 | def make_slider(widget_id, max_angle): 75 | slider_marks = {tick: str(tick) for tick in [-45, 0, 45]} 76 | return dcc.Slider( 77 | id=widget_id, min=-max_angle, max=max_angle, marks=slider_marks, value=0, step=5 78 | ) 79 | 80 | 81 | def make_number_widget(widget_id, max_angle): 82 | return dcc.Input( 83 | id=widget_id, 84 | type="number", 85 | value=0.0, 86 | min=-max_angle, 87 | max=max_angle, 88 | style=NUMBER_INPUT_STYLE, 89 | ) 90 | -------------------------------------------------------------------------------- /pages/page_inverse.py: -------------------------------------------------------------------------------- 1 | import json 2 | from dash.dependencies import Output 3 | from app import app 4 | from settings import RECOMPUTE_HEXAPOD 5 | from hexapod.models import VirtualHexapod 6 | from hexapod.const import BASE_PLOTTER 7 | from hexapod.ik_solver.ik_solver2 import inverse_kinematics_update 8 | from hexapod.ik_solver.recompute_hexapod import recompute_hexapod 9 | from widgets.ik_ui import IK_WIDGETS_SECTION, IK_CALLBACK_INPUTS 10 | from pages import helpers, shared 11 | 12 | 13 | # ...................... 14 | # Page layout 15 | # ...................... 16 | 17 | GRAPH_ID = "graph-inverse" 18 | MESSAGE_SECTION_ID = "message-inverse" 19 | PARAMETERS_SECTION_ID = "parameters-inverse" 20 | 21 | sidebar = shared.make_standard_page_sidebar( 22 | MESSAGE_SECTION_ID, PARAMETERS_SECTION_ID, IK_WIDGETS_SECTION 23 | ) 24 | 25 | layout = shared.make_standard_page_layout(GRAPH_ID, sidebar) 26 | 27 | 28 | # ...................... 29 | # Update page 30 | # ...................... 31 | 32 | outputs, inputs, states = shared.make_standard_page_callback_params( 33 | GRAPH_ID, PARAMETERS_SECTION_ID, MESSAGE_SECTION_ID 34 | ) 35 | 36 | 37 | @app.callback(outputs, inputs, states) 38 | def update_inverse_page(dimensions_json, ik_parameters_json, relayout_data, figure): 39 | 40 | dimensions = helpers.load_params(dimensions_json, "dims") 41 | ik_parameters = helpers.load_params(ik_parameters_json, "ik") 42 | hexapod = VirtualHexapod(dimensions) 43 | 44 | try: 45 | poses, hexapod = inverse_kinematics_update(hexapod, ik_parameters) 46 | except Exception as alert: 47 | return figure, helpers.make_alert_message(alert) 48 | 49 | if RECOMPUTE_HEXAPOD: 50 | try: 51 | hexapod = recompute_hexapod(dimensions, ik_parameters, poses) 52 | except Exception as alert: 53 | return figure, helpers.make_alert_message(alert) 54 | 55 | BASE_PLOTTER.update(figure, hexapod) 56 | helpers.change_camera_view(figure, relayout_data) 57 | return figure, helpers.make_poses_message(poses) 58 | 59 | 60 | # ...................... 61 | # Update parameters 62 | # ...................... 63 | 64 | output_parameter = Output(PARAMETERS_SECTION_ID, "children") 65 | input_parameters = IK_CALLBACK_INPUTS 66 | 67 | 68 | @app.callback(output_parameter, input_parameters) 69 | def update_ik_parameters( 70 | hip_stance, leg_stance, percent_x, percent_y, percent_z, rot_x, rot_y, rot_z 71 | ): 72 | 73 | return json.dumps( 74 | { 75 | "hip_stance": hip_stance or 0, 76 | "leg_stance": leg_stance or 0, 77 | "percent_x": percent_x or 0, 78 | "percent_y": percent_y or 0, 79 | "percent_z": percent_z or 0, 80 | "rot_x": rot_x or 0, 81 | "rot_y": rot_y or 0, 82 | "rot_z": rot_z or 0, 83 | } 84 | ) 85 | -------------------------------------------------------------------------------- /index.py: -------------------------------------------------------------------------------- 1 | import dash_core_components as dcc 2 | import dash_html_components as html 3 | from dash.dependencies import Input, Output 4 | from texts import ( 5 | URL_KOFI, 6 | URL_REPO, 7 | KINEMATICS_PAGE_PATH, 8 | IK_PAGE_PATH, 9 | PATTERNS_PAGE_PATH, 10 | ROOT_PATH, 11 | ) 12 | from settings import DEBUG_MODE 13 | from style_settings import GLOBAL_PAGE_STYLE 14 | from app import app 15 | from pages import page_inverse, page_kinematics, page_patterns, page_landing 16 | 17 | server = app.server 18 | 19 | # .................... 20 | # Navigation partials 21 | # .................... 22 | icon_link_style = {"margin": "0 0 0 0.5em"} 23 | 24 | div_header = html.Div( 25 | [ 26 | html.A(html.H6("👾"), href=URL_REPO, target="_blank", style=icon_link_style), 27 | html.A(html.H6("☕"), href=URL_KOFI, target="_blank", style=icon_link_style), 28 | dcc.Link(html.H6("●"), href=PATTERNS_PAGE_PATH, style=icon_link_style), 29 | dcc.Link(html.H6("●"), href=IK_PAGE_PATH, style=icon_link_style), 30 | dcc.Link(html.H6("●"), href=KINEMATICS_PAGE_PATH, style=icon_link_style), 31 | dcc.Link(html.H6("●"), href=ROOT_PATH, style=icon_link_style), 32 | ], 33 | style={"display": "flex", "flex-direction": "row"}, 34 | ) 35 | 36 | div_footer = html.Div( 37 | [ 38 | html.A("👾 Source Code", href=URL_REPO, target="_blank"), 39 | html.Br(), 40 | html.A("☕ Buy Mithi coffee", href=URL_KOFI, target="_blank"), 41 | html.Br(), 42 | dcc.Link("● Leg Patterns", href=PATTERNS_PAGE_PATH), 43 | html.Br(), 44 | dcc.Link("● Inverse Kinematics", href=IK_PAGE_PATH), 45 | html.Br(), 46 | dcc.Link("● Kinematics", href=KINEMATICS_PAGE_PATH), 47 | html.Br(), 48 | dcc.Link("● Root", href=ROOT_PATH), 49 | html.Br(), 50 | ], 51 | ) 52 | 53 | # .................... 54 | # Page layout 55 | # .................... 56 | app.layout = html.Div( 57 | [ 58 | div_header, 59 | dcc.Location(id="url", refresh=False), 60 | html.Div(id="page-content"), 61 | div_footer, 62 | ], 63 | style=GLOBAL_PAGE_STYLE, 64 | ) 65 | 66 | 67 | # .................... 68 | # URL redirection 69 | # .................... 70 | PAGES = { 71 | IK_PAGE_PATH: page_inverse.layout, 72 | KINEMATICS_PAGE_PATH: page_kinematics.layout, 73 | PATTERNS_PAGE_PATH: page_patterns.layout, 74 | ROOT_PATH: page_landing.layout, 75 | } 76 | 77 | 78 | # .................... 79 | # Callback to display page given URL 80 | # .................... 81 | @app.callback(Output("page-content", "children"), [Input("url", "pathname")]) 82 | def display_page(pathname): 83 | try: 84 | return PAGES[pathname] 85 | except KeyError: 86 | return PAGES[ROOT_PATH] 87 | 88 | 89 | # .................... 90 | # Run server 91 | # .................... 92 | if __name__ == "__main__": 93 | app.run_server( 94 | debug=DEBUG_MODE, dev_tools_ui=DEBUG_MODE, dev_tools_props_check=DEBUG_MODE 95 | ) 96 | -------------------------------------------------------------------------------- /pages/shared.py: -------------------------------------------------------------------------------- 1 | import json 2 | import dash_core_components as dcc 3 | from dash.dependencies import Output, Input, State 4 | import dash_html_components as html 5 | from app import app 6 | from settings import ( 7 | UI_SIDEBAR_WIDTH, 8 | UI_GRAPH_WIDTH, 9 | UI_GRAPH_HEIGHT, 10 | ) 11 | from widgets.dimensions_ui import DIMENSION_CALLBACK_INPUTS, DIMENSIONS_WIDGETS_SECTION 12 | from hexapod.const import BASE_FIGURE 13 | 14 | 15 | # ...................... 16 | # Update hexapod dimensions callback 17 | # ...................... 18 | 19 | DIMENSIONS_HIDDEN_SECTION_ID = "hexapod-dimensions-values" 20 | DIMENSIONS_HIDDEN_SECTION = html.Div( 21 | id=DIMENSIONS_HIDDEN_SECTION_ID, style={"display": "none"} 22 | ) 23 | DIMS_JSON_CALLBACK_INPUT = Input(DIMENSIONS_HIDDEN_SECTION_ID, "children") 24 | DIMS_JSON_CALLBACK_OUTPUT = Output(DIMENSIONS_HIDDEN_SECTION_ID, "children") 25 | 26 | 27 | @app.callback(DIMS_JSON_CALLBACK_OUTPUT, DIMENSION_CALLBACK_INPUTS) 28 | def update_dimensions(front, side, middle, coxia, femur, tibia): 29 | dimensions = { 30 | "front": front or 0, 31 | "side": side or 0, 32 | "middle": middle or 0, 33 | "coxia": coxia or 0, 34 | "femur": femur or 0, 35 | "tibia": tibia or 0, 36 | } 37 | return json.dumps(dimensions) 38 | 39 | 40 | # ...................... 41 | # Make uniform layout 42 | # Graph on the right, controls on the left 43 | # ...................... 44 | 45 | 46 | def make_standard_page_layout(graph_id, sidebar_sections): 47 | sidebar = html.Div(sidebar_sections, style={"width": UI_SIDEBAR_WIDTH}) 48 | graph = dcc.Graph( 49 | id=graph_id, 50 | figure=BASE_FIGURE, 51 | style={"width": UI_GRAPH_WIDTH, "height": UI_GRAPH_HEIGHT}, 52 | ) 53 | 54 | layout = html.Div([sidebar, graph], style={"display": "flex"}) 55 | return layout 56 | 57 | 58 | # ...................... 59 | # Make standard sidebar 60 | # ...................... 61 | 62 | 63 | def make_standard_page_sidebar( 64 | message_section_id, params_hidden_section_id, params_widgets_section 65 | ): 66 | params_hidden_section = html.Div( 67 | id=params_hidden_section_id, style={"display": "none"} 68 | ) 69 | message_section = html.Div(id=message_section_id) 70 | 71 | return [ 72 | DIMENSIONS_WIDGETS_SECTION, 73 | params_widgets_section, 74 | message_section, 75 | DIMENSIONS_HIDDEN_SECTION, 76 | params_hidden_section, 77 | ] 78 | 79 | 80 | # ...................... 81 | # Make outputs, inputs, and states for page update callbacks 82 | # ..................... 83 | 84 | 85 | def make_standard_page_callback_params(graph_id, params_section_id, message_section_id): 86 | 87 | message_callback_output = Output(message_section_id, "children") 88 | params_json_callback_input = Input(params_section_id, "children") 89 | outputs = [Output(graph_id, "figure"), message_callback_output] 90 | inputs = [DIMS_JSON_CALLBACK_INPUT, params_json_callback_input] 91 | states = [State(graph_id, "relayoutData"), State(graph_id, "figure")] 92 | return outputs, inputs, states 93 | -------------------------------------------------------------------------------- /pages/page_kinematics.py: -------------------------------------------------------------------------------- 1 | import json 2 | from dash.dependencies import Output 3 | from app import app 4 | from settings import WHICH_POSE_CONTROL_UI 5 | from hexapod.models import VirtualHexapod 6 | from hexapod.const import BASE_PLOTTER 7 | from widgets.pose_control.components import KINEMATICS_CALLBACK_INPUTS 8 | from pages import helpers, shared 9 | 10 | if WHICH_POSE_CONTROL_UI == 1: 11 | from widgets.pose_control.generic_daq_slider_ui import KINEMATICS_WIDGETS_SECTION 12 | elif WHICH_POSE_CONTROL_UI == 2: 13 | from widgets.pose_control.generic_slider_ui import KINEMATICS_WIDGETS_SECTION 14 | else: 15 | from widgets.pose_control.generic_input_ui import KINEMATICS_WIDGETS_SECTION 16 | 17 | # ...................... 18 | # Page layout 19 | # ...................... 20 | 21 | GRAPH_ID = "graph-kinematics" 22 | MESSAGE_SECTION_ID = "message-kinematics" 23 | PARAMETERS_SECTION_ID = "parameters-kinematics" 24 | 25 | sidebar = shared.make_standard_page_sidebar( 26 | MESSAGE_SECTION_ID, PARAMETERS_SECTION_ID, KINEMATICS_WIDGETS_SECTION 27 | ) 28 | 29 | layout = shared.make_standard_page_layout(GRAPH_ID, sidebar) 30 | 31 | 32 | # ...................... 33 | # Update page 34 | # ...................... 35 | 36 | outputs, inputs, states = shared.make_standard_page_callback_params( 37 | GRAPH_ID, PARAMETERS_SECTION_ID, MESSAGE_SECTION_ID 38 | ) 39 | 40 | 41 | @app.callback(outputs, inputs, states) 42 | def update_kinematics_page(dimensions_json, poses_json, relayout_data, figure): 43 | 44 | dimensions = helpers.load_params(dimensions_json, "dims") 45 | poses = helpers.load_params(poses_json, "pose") 46 | hexapod = VirtualHexapod(dimensions) 47 | 48 | try: 49 | hexapod.update(poses, assume_ground_targets=False) 50 | except Exception as alert: 51 | return figure, helpers.make_alert_message(alert) 52 | 53 | BASE_PLOTTER.update(figure, hexapod) 54 | helpers.change_camera_view(figure, relayout_data) 55 | return figure, "" 56 | 57 | 58 | # ...................... 59 | # Update parameters 60 | # ...................... 61 | 62 | 63 | output_parameter = Output(PARAMETERS_SECTION_ID, "children") 64 | input_parameters = KINEMATICS_CALLBACK_INPUTS 65 | 66 | # fmt: off 67 | 68 | 69 | @app.callback(output_parameter, input_parameters) 70 | def update_poses( 71 | rmc, rmf, rmt, 72 | rfc, rff, rft, 73 | lfc, lff, lft, 74 | lmc, lmf, lmt, 75 | lbc, lbf, lbt, 76 | rbc, rbf, rbt, 77 | ): 78 | 79 | return json.dumps({ 80 | 0: {"coxia": rmc or 0, "femur": rmf or 0, "tibia": rmt or 0, "name": "right-middle", "id": 0}, 81 | 1: {"coxia": rfc or 0, "femur": rff or 0, "tibia": rft or 0, "name": "right-front", "id": 1}, 82 | 2: {"coxia": lfc or 0, "femur": lff or 0, "tibia": lft or 0, "name": "left-front", "id": 2}, 83 | 3: {"coxia": lmc or 0, "femur": lmf or 0, "tibia": lmt or 0, "name": "left-middle", "id": 3}, 84 | 4: {"coxia": lbc or 0, "femur": lbf or 0, "tibia": lbt or 0, "name": "left-back", "id": 4}, 85 | 5: {"coxia": rbc or 0, "femur": rbf or 0, "tibia": rbt or 0, "name": "right-back", "id": 5}, 86 | }) 87 | 88 | # fmt: on 89 | -------------------------------------------------------------------------------- /style_settings.py: -------------------------------------------------------------------------------- 1 | DARKMODE = True 2 | 3 | DARK_CSS_PATH = "https://mithi.github.io/hexapod-robot-simulator/dark.css" 4 | LIGHT_CSS_PATH = "https://mithi.github.io/hexapod-robot-simulator/light.css" 5 | 6 | EXTERNAL_STYLESHEETS = [DARK_CSS_PATH] 7 | if not DARKMODE: 8 | EXTERNAL_STYLESHEETS = [LIGHT_CSS_PATH] 9 | 10 | 11 | # *************************************** 12 | # GLOBAL PAGE STYLE 13 | # *************************************** 14 | 15 | DARK_BG_COLOR = "#222f3e" 16 | DARK_FONT_COLOR = "#32ff7e" 17 | 18 | GLOBAL_PAGE_STYLE = { 19 | "background": DARK_BG_COLOR, 20 | "color": DARK_FONT_COLOR, 21 | "padding": "0em", 22 | } 23 | 24 | if not DARKMODE: 25 | GLOBAL_PAGE_STYLE = {"background": "#ffffff", "color": "#2c3e50", "padding": "0em"} 26 | 27 | 28 | # *************************************** 29 | # NUMBER FIELD INPUT WIDGET 30 | # *************************************** 31 | 32 | NUMBER_INPUT_STYLE = { 33 | "marginRight": "5%", 34 | "width": "95%", 35 | "marginBottom": "5%", 36 | "borderRadius": "10px", 37 | "border": "solid 1px", 38 | "fontFamily": "Courier New", 39 | } 40 | 41 | if DARKMODE: 42 | NUMBER_INPUT_STYLE["backgroundColor"] = "#2c3e50" 43 | NUMBER_INPUT_STYLE["color"] = "#2ecc71" 44 | NUMBER_INPUT_STYLE["borderColor"] = "#2980b9" 45 | 46 | 47 | # *************************************** 48 | # DAQ SLIDER INPUT WIDGET 49 | # *************************************** 50 | 51 | IK_SLIDER_SIZE = 100 52 | 53 | SLIDER_THEME = { 54 | "dark": DARKMODE, 55 | "detail": "#ffffff", 56 | "primary": "#ffffff", 57 | "secondary": "#ffffff", 58 | } 59 | 60 | SLIDER_HANDLE_COLOR = "#2ecc71" 61 | SLIDER_COLOR = "#FC427B" 62 | 63 | if not DARKMODE: 64 | SLIDER_HANDLE_COLOR = "#2c3e50" 65 | SLIDER_COLOR = "#8e44ad" 66 | 67 | 68 | # *************************************** 69 | # HEXAPOD GRAPH 70 | # *************************************** 71 | 72 | BODY_MESH_COLOR = "#ff6348" 73 | BODY_MESH_OPACITY = 0.3 74 | BODY_COLOR = "#FC427B" 75 | BODY_OUTLINE_WIDTH = 12 76 | COG_COLOR = "#32ff7e" 77 | COG_SIZE = 14 78 | HEAD_SIZE = 14 79 | LEG_COLOR = "#EE5A24" # "#b71540" 80 | LEG_OUTLINE_WIDTH = 10 81 | SUPPORT_POLYGON_MESH_COLOR = "#3c6382" 82 | SUPPORT_POLYGON_MESH_OPACITY = 0.2 83 | LEGENDS_BG_COLOR = "rgba(44, 62, 80, 0.8)" 84 | AXIS_ZERO_LINE_COLOR = "#079992" 85 | PAPER_BG_COLOR = "#222f3e" 86 | GROUND_COLOR = "#0a3d62" 87 | LEGEND_FONT_COLOR = "#2ecc71" 88 | 89 | if not DARKMODE: 90 | BODY_MESH_COLOR = "#8e44ad" 91 | BODY_MESH_OPACITY = 0.9 92 | BODY_COLOR = "#8e44ad" 93 | BODY_OUTLINE_WIDTH = 10 94 | COG_COLOR = "#2c3e50" 95 | COG_SIZE = 15 96 | HEAD_COLOR = "#8e44ad" 97 | HEAD_SIZE = 12 98 | LEG_COLOR = "#2c3e50" 99 | LEG_OUTLINE_WIDTH = 10 100 | SUPPORT_POLYGON_MESH_COLOR = "#ffa801" 101 | SUPPORT_POLYGON_MESH_OPACITY = 0.3 102 | LEGENDS_BG_COLOR = "rgba(255, 255, 255, 0.5)" 103 | AXIS_ZERO_LINE_COLOR = "#ffa801" 104 | PAPER_BG_COLOR = "white" 105 | GROUND_COLOR = "rgb(240, 240, 240)" 106 | LEGEND_FONT_COLOR = "#34495e" 107 | -------------------------------------------------------------------------------- /tests/pattern_cases/case1.py: -------------------------------------------------------------------------------- 1 | from hexapod.points import Vector 2 | 3 | description = "Patterns Random Pose #1" 4 | alpha = 42 5 | beta = 66 6 | gamma = -34.5 7 | 8 | # ******************************** 9 | # Dimensions 10 | # ******************************** 11 | 12 | given_dimensions = { 13 | "front": 76, 14 | "side": 92, 15 | "middle": 123, 16 | "coxia": 58, 17 | "femur": 177, 18 | "tibia": 151, 19 | } 20 | 21 | # ******************************** 22 | # Correct Body Vectors 23 | # ******************************** 24 | 25 | correct_body_points = [ 26 | Vector(x=+123.00, y=+0.00, z=+0.00, name="right-middle"), 27 | Vector(x=+76.00, y=+92.00, z=+0.00, name="right-front"), 28 | Vector(x=-76.00, y=+92.00, z=+0.00, name="left-front"), 29 | Vector(x=-123.00, y=+0.00, z=+0.00, name="left-middle"), 30 | Vector(x=-76.00, y=-92.00, z=+0.00, name="left-back"), 31 | Vector(x=+76.00, y=-92.00, z=+0.00, name="right-back"), 32 | Vector(x=+0.00, y=+0.00, z=+0.00, name="center-of-gravity"), 33 | Vector(x=+0.00, y=+92.00, z=+0.00, name="head"), 34 | ] 35 | 36 | 37 | # ******************************** 38 | # Correct Leg Vectors 39 | # ******************************** 40 | 41 | leg0_points = [ 42 | Vector(x=+123.00, y=+0.00, z=+0.00, name="right-middle-body-contact"), 43 | Vector(x=+166.10, y=+38.81, z=+0.00, name="right-middle-coxia"), 44 | Vector(x=+219.60, y=+86.98, z=+161.70, name="right-middle-femur"), 45 | Vector(x=+278.24, y=+139.77, z=+32.95, name="right-middle-tibia"), 46 | ] 47 | 48 | leg1_points = [ 49 | Vector(x=+76.00, y=+92.00, z=+0.00, name="right-front-body-contact"), 50 | Vector(x=+79.04, y=+149.92, z=+0.00, name="right-front-coxia"), 51 | Vector(x=+82.80, y=+221.81, z=+161.70, name="right-front-femur"), 52 | Vector(x=+86.93, y=+300.60, z=+32.95, name="right-front-tibia"), 53 | ] 54 | 55 | leg2_points = [ 56 | Vector(x=-76.00, y=+92.00, z=+0.00, name="left-front-body-contact"), 57 | Vector(x=-133.92, y=+95.04, z=+0.00, name="left-front-coxia"), 58 | Vector(x=-205.81, y=+98.80, z=+161.70, name="left-front-femur"), 59 | Vector(x=-284.60, y=+102.93, z=+32.95, name="left-front-tibia"), 60 | ] 61 | 62 | leg3_points = [ 63 | Vector(x=-123.00, y=+0.00, z=+0.00, name="left-middle-body-contact"), 64 | Vector(x=-166.10, y=-38.81, z=+0.00, name="left-middle-coxia"), 65 | Vector(x=-219.60, y=-86.98, z=+161.70, name="left-middle-femur"), 66 | Vector(x=-278.24, y=-139.77, z=+32.95, name="left-middle-tibia"), 67 | ] 68 | 69 | leg4_points = [ 70 | Vector(x=-76.00, y=-92.00, z=+0.00, name="left-back-body-contact"), 71 | Vector(x=-79.04, y=-149.92, z=+0.00, name="left-back-coxia"), 72 | Vector(x=-82.80, y=-221.81, z=+161.70, name="left-back-femur"), 73 | Vector(x=-86.93, y=-300.60, z=+32.95, name="left-back-tibia"), 74 | ] 75 | 76 | leg5_points = [ 77 | Vector(x=+76.00, y=-92.00, z=+0.00, name="right-back-body-contact"), 78 | Vector(x=+133.92, y=-95.04, z=+0.00, name="right-back-coxia"), 79 | Vector(x=+205.81, y=-98.80, z=+161.70, name="right-back-femur"), 80 | Vector(x=+284.60, y=-102.93, z=+32.95, name="right-back-tibia"), 81 | ] 82 | 83 | 84 | correct_leg_points = [ 85 | leg0_points, 86 | leg1_points, 87 | leg2_points, 88 | leg3_points, 89 | leg4_points, 90 | leg5_points, 91 | ] 92 | -------------------------------------------------------------------------------- /tests/pattern_cases/case2.py: -------------------------------------------------------------------------------- 1 | from hexapod.points import Vector 2 | 3 | description = "Patterns Random Pose #2" 4 | alpha = -28.5 5 | beta = 72 6 | gamma = -54 7 | 8 | # ******************************** 9 | # Dimensions 10 | # ******************************** 11 | 12 | given_dimensions = { 13 | "front": 65, 14 | "side": 101, 15 | "middle": 122, 16 | "coxia": 83, 17 | "femur": 108, 18 | "tibia": 177, 19 | } 20 | 21 | # ******************************** 22 | # Correct Body Vectors 23 | # ******************************** 24 | 25 | correct_body_points = [ 26 | Vector(x=+116.86, y=+35.03, z=+65.62, name="right-middle"), 27 | Vector(x=+33.27, y=+115.41, z=+65.62, name="right-front"), 28 | Vector(x=-91.26, y=+78.09, z=+65.62, name="left-front"), 29 | Vector(x=-116.86, y=-35.03, z=+65.62, name="left-middle"), 30 | Vector(x=-33.27, y=-115.41, z=+65.62, name="left-back"), 31 | Vector(x=+91.26, y=-78.09, z=+65.62, name="right-back"), 32 | Vector(x=+0.00, y=+0.00, z=+65.62, name="center-of-gravity"), 33 | Vector(x=-29.00, y=+96.75, z=+65.62, name="head"), 34 | ] 35 | 36 | # ******************************** 37 | # Correct Leg Vectors 38 | # ******************************** 39 | 40 | leg0_points = [ 41 | Vector(x=+116.86, y=+35.03, z=+65.62, name="right-middle-body-contact"), 42 | Vector(x=+198.11, y=+18.03, z=+65.62, name="right-middle-coxia"), 43 | Vector(x=+230.77, y=+11.20, z=+168.34, name="right-middle-femur"), 44 | Vector(x=+284.31, y=+0.00, z=+0.00, name="right-middle-tibia"), 45 | ] 46 | 47 | leg1_points = [ 48 | Vector(x=+33.27, y=+115.41, z=+65.62, name="right-front-body-contact"), 49 | Vector(x=+102.73, y=+160.84, z=+65.62, name="right-front-coxia"), 50 | Vector(x=+130.66, y=+179.11, z=+168.34, name="right-front-femur"), 51 | Vector(x=+176.44, y=+209.04, z=+0.00, name="right-front-tibia"), 52 | ] 53 | 54 | leg2_points = [ 55 | Vector(x=-91.26, y=+78.09, z=+65.62, name="left-front-body-contact"), 56 | Vector(x=-136.69, y=+147.55, z=+65.62, name="left-front-coxia"), 57 | Vector(x=-154.96, y=+175.48, z=+168.34, name="left-front-femur"), 58 | Vector(x=-184.90, y=+221.26, z=+0.00, name="left-front-tibia"), 59 | ] 60 | 61 | leg3_points = [ 62 | Vector(x=-116.86, y=-35.03, z=+65.62, name="left-middle-body-contact"), 63 | Vector(x=-198.11, y=-18.03, z=+65.62, name="left-middle-coxia"), 64 | Vector(x=-230.77, y=-11.20, z=+168.34, name="left-middle-femur"), 65 | Vector(x=-284.31, y=+0.00, z=+0.00, name="left-middle-tibia"), 66 | ] 67 | 68 | leg4_points = [ 69 | Vector(x=-33.27, y=-115.41, z=+65.62, name="left-back-body-contact"), 70 | Vector(x=-102.73, y=-160.84, z=+65.62, name="left-back-coxia"), 71 | Vector(x=-130.66, y=-179.11, z=+168.34, name="left-back-femur"), 72 | Vector(x=-176.44, y=-209.04, z=+0.00, name="left-back-tibia"), 73 | ] 74 | 75 | leg5_points = [ 76 | Vector(x=+91.26, y=-78.09, z=+65.62, name="right-back-body-contact"), 77 | Vector(x=+136.69, y=-147.55, z=+65.62, name="right-back-coxia"), 78 | Vector(x=+154.96, y=-175.48, z=+168.34, name="right-back-femur"), 79 | Vector(x=+184.90, y=-221.26, z=+0.00, name="right-back-tibia"), 80 | ] 81 | 82 | correct_leg_points = [ 83 | leg0_points, 84 | leg1_points, 85 | leg2_points, 86 | leg3_points, 87 | leg4_points, 88 | leg5_points, 89 | ] 90 | -------------------------------------------------------------------------------- /widgets/ik_ui.py: -------------------------------------------------------------------------------- 1 | # Widgets used to set the inverse kinematics parameters 2 | import dash_core_components as dcc 3 | import dash_html_components as html 4 | from dash.dependencies import Input 5 | import dash_daq 6 | from texts import IK_WIDGETS_HEADER 7 | from style_settings import ( 8 | SLIDER_THEME, 9 | SLIDER_HANDLE_COLOR, 10 | SLIDER_COLOR, 11 | IK_SLIDER_SIZE, 12 | ) 13 | from settings import ( 14 | UPDATE_MODE, 15 | BODY_MAX_ANGLE, 16 | HIP_STANCE_MAX_ANGLE, 17 | LEG_STANCE_MAX_ANGLE, 18 | SLIDER_ANGLE_RESOLUTION, 19 | ) 20 | 21 | 22 | def make_row(divs): 23 | widget_style = {"padding": "1.0em 0 0 4.0em"} 24 | row_style = {"display": "flex", "flex-direction": "row"} 25 | widgets = [html.Div(div, style=widget_style) for div in divs] 26 | return html.Div(widgets, style=row_style) 27 | 28 | 29 | def make_translate_slider(name, slider_label): 30 | handle_style = { 31 | "showCurrentValue": True, 32 | "color": SLIDER_HANDLE_COLOR, 33 | "label": slider_label, 34 | } 35 | 36 | return dash_daq.Slider( # pylint: disable=not-callable 37 | id=name, 38 | min=-1.0, 39 | max=1.0, 40 | value=0.05, 41 | step=0.05, 42 | vertical=True, 43 | size=IK_SLIDER_SIZE, 44 | updatemode=UPDATE_MODE, 45 | handleLabel=handle_style, 46 | color={"default": SLIDER_COLOR}, 47 | theme=SLIDER_THEME, 48 | ) 49 | 50 | 51 | def make_rotate_slider(name, slider_label, max_angle=BODY_MAX_ANGLE): 52 | handle_style = { 53 | "showCurrentValue": True, 54 | "color": SLIDER_HANDLE_COLOR, 55 | "label": slider_label, 56 | } 57 | return dash_daq.Slider( # pylint: disable=not-callable 58 | id=name, 59 | min=-max_angle, 60 | max=max_angle, 61 | value=1.5, 62 | step=SLIDER_ANGLE_RESOLUTION, 63 | vertical=True, 64 | size=IK_SLIDER_SIZE, 65 | updatemode=UPDATE_MODE, 66 | handleLabel=handle_style, 67 | color={"default": SLIDER_COLOR}, 68 | theme=SLIDER_THEME, 69 | ) 70 | 71 | 72 | # ................................ 73 | # COMPONENTS 74 | # ................................ 75 | 76 | HEADER = html.Label(dcc.Markdown(f"**{IK_WIDGETS_HEADER}**")) 77 | IK_WIDGETS_IDS = [ 78 | "widget-start-hip-stance", 79 | "widget-start-leg-stance", 80 | "widget-percent-x", 81 | "widget-percent-y", 82 | "widget-percent-z", 83 | "widget-rot-x", 84 | "widget-rot-y", 85 | "widget-rot-z", 86 | ] 87 | IK_CALLBACK_INPUTS = [Input(input_id, "value") for input_id in IK_WIDGETS_IDS] 88 | 89 | w_hips = make_rotate_slider( 90 | IK_WIDGETS_IDS[0], "start\nhip.stance", HIP_STANCE_MAX_ANGLE 91 | ) 92 | w_legs = make_rotate_slider( 93 | IK_WIDGETS_IDS[1], "start\nleg.stance", LEG_STANCE_MAX_ANGLE 94 | ) 95 | 96 | w_tx = make_translate_slider(IK_WIDGETS_IDS[2], "percent.x") 97 | w_ty = make_translate_slider(IK_WIDGETS_IDS[3], "percent.y") 98 | w_tz = make_translate_slider(IK_WIDGETS_IDS[4], "percent.z") 99 | 100 | w_rx = make_rotate_slider(IK_WIDGETS_IDS[5], "rot.x") 101 | w_ry = make_rotate_slider(IK_WIDGETS_IDS[6], "rot.y") 102 | w_rz = make_rotate_slider(IK_WIDGETS_IDS[7], "rot.z") 103 | 104 | row1 = make_row([w_hips, w_tx, w_ty, w_tz]) 105 | row2 = make_row([w_legs, w_rx, w_ry, w_rz]) 106 | 107 | IK_WIDGETS_SECTION = html.Div([HEADER, row1, row2]) 108 | -------------------------------------------------------------------------------- /tests/kinematics_cases/case1.py: -------------------------------------------------------------------------------- 1 | from hexapod.points import Vector 2 | 3 | description = "Kinematics Random Pose #1" 4 | 5 | # ******************************** 6 | # Dimensions 7 | # ******************************** 8 | 9 | given_dimensions = { 10 | "front": 75, 11 | "side": 100, 12 | "middle": 125, 13 | "coxia": 50, 14 | "femur": 130, 15 | "tibia": 200, 16 | } 17 | 18 | # ******************************** 19 | # Poses 20 | # ******************************** 21 | 22 | given_poses = { 23 | 0: {"coxia": -40, "femur": 19, "tibia": 6, "name": "right-middle", "id": 0}, 24 | 1: {"coxia": 33, "femur": 85, "tibia": -60, "name": "right-front", "id": 1}, 25 | 2: {"coxia": -20, "femur": 90, "tibia": -13, "name": "left-front", "id": 2}, 26 | 3: {"coxia": -12, "femur": -25, "tibia": 3, "name": "left-middle", "id": 3}, 27 | 4: {"coxia": 0, "femur": 94, "tibia": -70, "name": "left-back", "id": 4}, 28 | 5: {"coxia": -5, "femur": 17, "tibia": 2, "name": "right-back", "id": 5}, 29 | } 30 | 31 | 32 | # ******************************** 33 | # Correct Body Vectors 34 | # ******************************** 35 | 36 | correct_body_points = [ 37 | Vector(x=+97.74, y=+69.20, z=+123.97, name="right-middle"), 38 | Vector(x=-3.68, y=+111.78, z=+103.96, name="right-front"), 39 | Vector(x=-120.97, y=+28.74, z=+146.94, name="left-front"), 40 | Vector(x=-97.74, y=-69.20, z=+195.60, name="left-middle"), 41 | Vector(x=+3.68, y=-111.78, z=+215.60, name="left-back"), 42 | Vector(x=+120.97, y=-28.74, z=+172.63, name="right-back"), 43 | Vector(x=+0.00, y=+0.00, z=+159.78, name="center-of-gravity"), 44 | Vector(x=-62.33, y=+70.26, z=+125.45, name="head"), 45 | ] 46 | 47 | # ******************************** 48 | # Leg Vectors 49 | # ******************************** 50 | 51 | leg0_points = [ 52 | Vector(x=+97.74, y=+69.20, z=+123.97, name="right-middle-body-contact"), 53 | Vector(x=+147.72, y=+67.82, z=+124.03, name="right-middle-coxia"), 54 | Vector(x=+271.07, y=+83.36, z=+162.03, name="right-middle-femur"), 55 | Vector(x=+353.52, y=-0.00, z=+0.00, name="right-middle-tibia"), 56 | ] 57 | 58 | leg1_points = [ 59 | Vector(x=-3.68, y=+111.78, z=+103.96, name="right-front-body-contact"), 60 | Vector(x=-26.03, y=+151.90, z=+84.19, name="right-front-coxia"), 61 | Vector(x=-29.64, y=+218.89, z=+195.55, name="right-front-femur"), 62 | Vector(x=-69.47, y=+205.68, z=+0.00, name="right-front-tibia"), 63 | ] 64 | 65 | leg2_points = [ 66 | Vector(x=-120.97, y=+28.74, z=+146.94, name="left-front-body-contact"), 67 | Vector(x=-165.74, y=+48.88, z=+137.44, name="left-front-coxia"), 68 | Vector(x=-164.27, y=+107.00, z=+253.72, name="left-front-femur"), 69 | Vector(x=-339.26, y=+165.39, z=+176.44, name="left-front-tibia"), 70 | ] 71 | 72 | leg3_points = [ 73 | Vector(x=-97.74, y=-69.20, z=+195.60, name="left-middle-body-contact"), 74 | Vector(x=-142.46, y=-88.97, z=+206.04, name="left-middle-coxia"), 75 | Vector(x=-248.46, y=-160.12, z=+181.51, name="left-middle-femur"), 76 | Vector(x=-183.54, y=-213.39, z=+0.00, name="left-middle-tibia"), 77 | ] 78 | 79 | leg4_points = [ 80 | Vector(x=+3.68, y=-111.78, z=+215.60, name="left-back-body-contact"), 81 | Vector(x=-1.93, y=-156.20, z=+237.87, name="left-back-coxia"), 82 | Vector(x=+0.55, y=-90.17, z=+349.83, name="left-back-femur"), 83 | Vector(x=-10.63, y=-244.11, z=+222.63, name="left-back-tibia"), 84 | ] 85 | 86 | leg5_points = [ 87 | Vector(x=+120.97, y=-28.74, z=+172.63, name="right-back-body-contact"), 88 | Vector(x=+169.97, y=-37.86, z=+176.57, name="right-back-coxia"), 89 | Vector(x=+292.24, y=-43.54, z=+220.36, name="right-back-femur"), 90 | Vector(x=+353.93, y=-139.96, z=+56.35, name="right-back-tibia"), 91 | ] 92 | 93 | correct_leg_points = [ 94 | leg0_points, 95 | leg1_points, 96 | leg2_points, 97 | leg3_points, 98 | leg4_points, 99 | leg5_points, 100 | ] 101 | -------------------------------------------------------------------------------- /hexapod/ground_contact_solver/ground_contact_solver2.py: -------------------------------------------------------------------------------- 1 | """ 2 | ❗❗❗This is a more general algorithm to account for the cases 3 | that are not handled correctly by the other ground_contact_solver. 4 | This will only be used by the kinematics-page of the app. 5 | This algorithm can be optimized or replaced if a more elegant 6 | algorithm is available. 7 | ❗❗❗ 8 | 9 | We have 18 points total. 10 | (6 legs, three possible points per leg) 11 | 12 | We have a total of 540 combinations 13 | - get three legs out of six (20 combinations) 14 | - we have three possible points for each leg, that's 27 (3^3) 15 | - 27 * 20 is 540 16 | 17 | For each combination: 18 | 1. Check if stable if not, next 19 | - Check if the projection of the center of gravity to the plane 20 | defined by the three points lies inside the triangle, if not, next 21 | 2. Get the height and normal of the height and normal of the triangle plane 22 | (We need this for the next part) 23 | 3. For each of the three leg, check if the two other points on the leg is not a 24 | lower height, if condition if broken, next. (6 points total) 25 | 4. For each of the three other legs, check all points (3 points of each leg) 26 | if so, next. (9 points total) 27 | 5. If no condition is violated, then this is good, return this! 28 | """ 29 | import random 30 | from hexapod.ground_contact_solver.shared import ( 31 | is_stable, 32 | is_lower, 33 | find_legs_on_ground, 34 | SOME_LEG_TRIOS, 35 | ADJACENT_LEG_TRIOS, 36 | ) 37 | from hexapod.points import get_normal_given_three_points, dot 38 | 39 | OTHER_POINTS_MAP = {1: (2, 3), 2: (3, 1), 3: (1, 2)} 40 | 41 | JOINT_TRIOS = [] 42 | for i in range(3, 0, -1): 43 | for j in range(3, 0, -1): 44 | for k in range(3, 0, -1): 45 | JOINT_TRIOS.append((i, j, k)) 46 | 47 | 48 | def compute_orientation_properties(legs): 49 | """ 50 | Returns: 51 | - Which legs are on the ground 52 | - Normal vector of the plane defined by these legs 53 | - Distance of this plane to center of gravity 54 | """ 55 | # prefer leg combinations where legs are not adjacent to each other 56 | # introduce some randomness so we are not bias in 57 | # choosing one stable position over another 58 | shuffled_some_leg_trios = random.sample(SOME_LEG_TRIOS, len(SOME_LEG_TRIOS)) 59 | leg_trios = shuffled_some_leg_trios + ADJACENT_LEG_TRIOS 60 | 61 | for leg_trio in leg_trios: 62 | 63 | other_leg_trio = [i for i in range(6) if i not in leg_trio] 64 | other_three_legs = [legs[i] for i in other_leg_trio] 65 | three_legs = [legs[i] for i in leg_trio] 66 | 67 | for joint_trio in JOINT_TRIOS: 68 | 69 | p0, p1, p2 = [legs[i].get_point(j) for i, j in zip(leg_trio, joint_trio)] 70 | 71 | if not is_stable(p0, p1, p2): 72 | continue 73 | 74 | n = get_normal_given_three_points(p0, p1, p2) 75 | height = -dot(n, p0) 76 | 77 | if same_leg_joints_break_condition(three_legs, joint_trio, n, height): 78 | continue 79 | 80 | if other_leg_joints_break_condition(other_three_legs, n, height): 81 | continue 82 | 83 | legs_on_ground = find_legs_on_ground(legs, n, height) 84 | return legs_on_ground, n, height 85 | 86 | return [], None, None 87 | 88 | 89 | def same_leg_joints_break_condition(three_legs, three_point_ids, n, height): 90 | for leg, point_id in zip(three_legs, three_point_ids): 91 | for other_point_id in OTHER_POINTS_MAP[point_id]: 92 | point = leg.get_point(other_point_id) 93 | if is_lower(point, height, n): 94 | return True 95 | return False 96 | 97 | 98 | def other_leg_joints_break_condition(other_three_legs, n, height): 99 | for leg in other_three_legs: 100 | for point in leg.all_points[1:]: 101 | if is_lower(point, height, n): 102 | return True 103 | return False 104 | -------------------------------------------------------------------------------- /hexapod/plotter.py: -------------------------------------------------------------------------------- 1 | # ******************** 2 | # This module updates the figure dictionary 3 | # that plotly uses to draw the 3d graph 4 | # it takes in a hexapod model, and the figure to update 5 | # you can also update the camera view with it by passing a camera dictionary 6 | # ******************** 7 | 8 | 9 | class HexapodPlotter: 10 | def __init__(self): 11 | pass 12 | 13 | @staticmethod 14 | def update(fig, hexapod): 15 | HexapodPlotter._draw_hexapod(fig, hexapod) 16 | HexapodPlotter._draw_scene(fig, hexapod) 17 | 18 | @staticmethod 19 | def change_camera_view(fig, camera): 20 | # camera = { 'up': {'x': 0, 'y': 0, 'z': 0}, 21 | # 'center': {'x': 0, 'y': 0, 'z': 0}, 22 | # 'eye': {'x': 0, 'y': 0, 'z': 0)}} 23 | fig["layout"]["scene"]["camera"] = camera 24 | 25 | @staticmethod 26 | def _draw_hexapod(fig, hexapod): 27 | # Body 28 | points = hexapod.body.vertices + [hexapod.body.vertices[0]] 29 | 30 | # Body Surface Mesh 31 | fig["data"][0]["x"] = [point.x for point in points] 32 | fig["data"][0]["y"] = [point.y for point in points] 33 | fig["data"][0]["z"] = [point.z for point in points] 34 | 35 | # Body Outline 36 | fig["data"][1]["x"] = fig["data"][0]["x"] 37 | fig["data"][1]["y"] = fig["data"][0]["y"] 38 | fig["data"][1]["z"] = fig["data"][0]["z"] 39 | 40 | fig["data"][2]["x"] = [hexapod.body.cog.x] 41 | fig["data"][2]["y"] = [hexapod.body.cog.y] 42 | fig["data"][2]["z"] = [hexapod.body.cog.z] 43 | 44 | fig["data"][3]["x"] = [hexapod.body.head.x] 45 | fig["data"][3]["y"] = [hexapod.body.head.y] 46 | fig["data"][3]["z"] = [hexapod.body.head.z] 47 | 48 | for n, leg in zip(range(4, 10), hexapod.legs): 49 | points = leg.all_points 50 | fig["data"][n]["x"] = [point.x for point in points] 51 | fig["data"][n]["y"] = [point.y for point in points] 52 | fig["data"][n]["z"] = [point.z for point in points] 53 | 54 | # Hexapod Support Polygon 55 | # Draw a mesh for body contact on ground 56 | dz = -1 # Mesh must be slightly below ground 57 | ground_contacts = hexapod.ground_contacts 58 | fig["data"][10]["x"] = [point.x for point in ground_contacts] 59 | fig["data"][10]["y"] = [point.y for point in ground_contacts] 60 | fig["data"][10]["z"] = [(point.z + dz) for point in ground_contacts] 61 | 62 | @staticmethod 63 | def _draw_scene(fig, hexapod): 64 | # Change range of view for all axes 65 | RANGE = hexapod.sum_of_dimensions() 66 | AXIS_RANGE = [-RANGE, RANGE] 67 | 68 | z_start = -10 69 | fig["layout"]["scene"]["xaxis"]["range"] = AXIS_RANGE 70 | fig["layout"]["scene"]["yaxis"]["range"] = AXIS_RANGE 71 | fig["layout"]["scene"]["zaxis"]["range"] = [z_start, (RANGE - z_start) * 2] 72 | 73 | axis_scale = hexapod.front / 2 74 | 75 | # Draw the hexapod local frame 76 | cog = hexapod.body.cog 77 | x_axis = hexapod.x_axis 78 | y_axis = hexapod.y_axis 79 | z_axis = hexapod.z_axis 80 | 81 | fig["data"][11]["x"] = [cog.x, cog.x + axis_scale * x_axis.x] 82 | fig["data"][11]["y"] = [cog.y, cog.y + axis_scale * x_axis.y] 83 | fig["data"][11]["z"] = [cog.z, cog.z + axis_scale * x_axis.z] 84 | 85 | fig["data"][12]["x"] = [cog.x, cog.x + axis_scale * y_axis.x] 86 | fig["data"][12]["y"] = [cog.y, cog.y + axis_scale * y_axis.y] 87 | fig["data"][12]["z"] = [cog.z, cog.z + axis_scale * y_axis.z] 88 | 89 | fig["data"][13]["x"] = [cog.x, cog.x + axis_scale * z_axis.x] 90 | fig["data"][13]["y"] = [cog.y, cog.y + axis_scale * z_axis.y] 91 | fig["data"][13]["z"] = [cog.z, cog.z + axis_scale * z_axis.z] 92 | 93 | # Scale the global coordinate frame 94 | fig["data"][14]["x"] = [0, axis_scale] 95 | fig["data"][14]["y"] = [0, 0] 96 | fig["data"][14]["z"] = [0, 0] 97 | 98 | fig["data"][15]["x"] = [0, 0] 99 | fig["data"][15]["y"] = [0, axis_scale] 100 | fig["data"][15]["z"] = [0, 0] 101 | 102 | fig["data"][16]["x"] = [0, 0] 103 | fig["data"][16]["y"] = [0, 0] 104 | fig["data"][16]["z"] = [0, axis_scale] 105 | -------------------------------------------------------------------------------- /hexapod/ground_contact_solver/ground_contact_solver.py: -------------------------------------------------------------------------------- 1 | """ 2 | ❗❗❗An algorithm for computing the robot's orientation 3 | assuming probably ground contacts are known. 4 | 5 | This algorithm rests upon the assumption that it 6 | knows which point of the each leg is in contact with the ground. 7 | This assumption seems to be true for all possible cases for 8 | leg-patterns page and inverse-kinematics page. 9 | 10 | But this is not true for all possible 11 | angle combinations (18 angles) that can be defined in 12 | the kinematics page. 13 | 14 | This module is used for the leg-patterns page, 15 | and the inverse-kinematics page. 16 | 17 | The other module will be used for the kinematics page. 18 | ❗❗❗ 19 | 20 | This module is responsible for the following: 21 | 1. determining which legs of the hexapod is on the ground 22 | 2. Computing the normal vector of the triangle defined by at least three legs on the ground 23 | the normal vector wrt to the old normal vector that is defined by the legs on the ground in 24 | the hexapod's neutral position 25 | 3. Determining the height of the center of gravity (cog) wrt to the ground. 26 | ie this height is distance between the cog and the plane defined by ground contacts. 27 | """ 28 | from hexapod.points import dot, get_normal_given_three_points 29 | from hexapod.ground_contact_solver.shared import ( 30 | is_stable, 31 | is_lower, 32 | find_legs_on_ground, 33 | LEG_TRIOS, 34 | ) 35 | 36 | 37 | def compute_orientation_properties(legs): 38 | """ 39 | Returns: 40 | - Which legs are on the ground 41 | - Normal vector of the plane defined by these legs 42 | - Distance of this plane to center of gravity 43 | """ 44 | n, height = find_ground_plane_properties(legs) 45 | 46 | # this pose is unstable, The hexapod has no balance 47 | if n is None: 48 | return [], None, None 49 | 50 | return find_legs_on_ground(legs, n, height), n, height 51 | 52 | 53 | def find_ground_plane_properties(legs): 54 | """ 55 | Return three legs forming a stable position from legs, 56 | or None if no three legs satisfy this requirement. 57 | It also returns the normal vector of the plane 58 | defined by the three ground contacts, and the 59 | computed distance of the hexapod body to the ground plane 60 | """ 61 | ground_contacts = [leg.ground_contact() for leg in legs] 62 | 63 | # (2, 3, 5) is a trio from the set [0, 1, 2, 3, 4, 5] 64 | # the corresponding other_trio of (2, 3, 5) is (0, 1, 4) 65 | # order is not important ie (2, 3, 5) is the same as (5, 3, 2) 66 | for trio in LEG_TRIOS: 67 | p0, p1, p2 = [ground_contacts[i] for i in trio] 68 | 69 | if not is_stable(p0, p1, p2): 70 | continue 71 | 72 | # Get the vector normal to plane defined by these points 73 | # ❗IMPORTANT: The normal is always pointing up 74 | # because of how we specified the order of the trio 75 | # (and the legs in general) 76 | # starting from middle-right (id:0) to right back (id:5) 77 | # always towards one direction (ccw) 78 | n = get_normal_given_three_points(p0, p1, p2) 79 | 80 | # p0 is vector from cog (0, 0, 0) to ground contact 81 | # dot product of this and normal we get the 82 | # hypothetical (negative) height of ground contact to cog 83 | # 84 | # cog * ^ (normal_vector) ---- 85 | # \ | | 86 | # \ | -height 87 | # \| | 88 | # V p0 (foot_tip) ------ 89 | # 90 | # using p0, p1 or p2 should yield the same result 91 | height = -dot(n, p0) 92 | 93 | # height should be the highest since 94 | # the plane defined by this trio is on the ground 95 | # the other legs ground contact cannot be lower than the ground 96 | other_trio = [i for i in range(6) if i not in trio] 97 | other_points = [ground_contacts[i] for i in other_trio] 98 | if no_other_legs_lower(n, height, other_points): 99 | # Found one! 100 | return n, height 101 | 102 | # Nothing met the condition 103 | return None, None 104 | 105 | 106 | def no_other_legs_lower(n, height, other_points): 107 | for point in other_points: 108 | if is_lower(point, height, n): 109 | return False 110 | 111 | return True 112 | -------------------------------------------------------------------------------- /tests/kinematics_cases/case2.py: -------------------------------------------------------------------------------- 1 | from hexapod.points import Vector 2 | 3 | description = "Kinematics Random Pose #2" 4 | 5 | # ******************************** 6 | # Dimensions 7 | # ******************************** 8 | 9 | given_dimensions = { 10 | "front": 53, 11 | "side": 112, 12 | "middle": 124, 13 | "coxia": 65, 14 | "femur": 147, 15 | "tibia": 158, 16 | } 17 | 18 | # ******************************** 19 | # Poses 20 | # ******************************** 21 | 22 | given_poses = { 23 | 0: { 24 | "name": "right-middle", 25 | "id": 0, 26 | "coxia": -46.173564612682185, 27 | "femur": -0.5639873561713742, 28 | "tibia": -22.853557731606656, 29 | }, 30 | 1: { 31 | "name": "right-front", 32 | "id": 1, 33 | "coxia": -38.57261437211969, 34 | "femur": -3.2736722565308938, 35 | "tibia": -24.640160005779748, 36 | }, 37 | 2: { 38 | "name": "left-front", 39 | "id": 2, 40 | "coxia": 2.0526054688295687, 41 | "femur": 35.09407799312794, 42 | "tibia": -31.188148885325916, 43 | }, 44 | 3: { 45 | "name": "left-middle", 46 | "id": 3, 47 | "coxia": -16.947073091191385, 48 | "femur": 46.44561383735447, 49 | "tibia": -19.412041056143877, 50 | }, 51 | 4: { 52 | "name": "left-back", 53 | "id": 4, 54 | "coxia": -33.39847023693062, 55 | "femur": 41.05787103974741, 56 | "tibia": -5.900804146706449, 57 | }, 58 | 5: { 59 | "name": "right-back", 60 | "id": 5, 61 | "coxia": -38.67228081907621, 62 | "femur": 18.790558327957655, 63 | "tibia": -30.220554892132796, 64 | }, 65 | } 66 | 67 | # ******************************** 68 | # Correct Body Vectors 69 | # ******************************** 70 | 71 | correct_body_points = [ 72 | Vector(x=+112.68, y=+45.33, z=+126.66, name="right-middle"), 73 | Vector(x=+5.57, y=+122.87, z=+116.68, name="right-front"), 74 | Vector(x=-90.76, y=+84.12, z=+95.32, name="left-front"), 75 | Vector(x=-112.68, y=-45.33, z=+76.69, name="left-middle"), 76 | Vector(x=-5.57, y=-122.87, z=+86.67, name="left-back"), 77 | Vector(x=+90.76, y=-84.12, z=+108.03, name="right-back"), 78 | Vector(x=+0.00, y=+0.00, z=+101.67, name="center-of-gravity"), 79 | Vector(x=-42.60, y=+103.49, z=+106.00, name="head"), 80 | ] 81 | 82 | 83 | # ******************************** 84 | # Leg Vectors 85 | # ******************************** 86 | 87 | leg0_points = [ 88 | Vector(x=+112.68, y=+45.33, z=+126.66, name="right-middle-body-contact"), 89 | Vector(x=+171.42, y=+18.46, z=+133.92, name="right-middle-coxia"), 90 | Vector(x=+304.49, y=-42.16, z=+148.91, name="right-middle-femur"), 91 | Vector(x=+272.70, y=+0.00, z=-0.00, name="right-middle-tibia"), 92 | ] 93 | 94 | 95 | leg1_points = [ 96 | Vector(x=+5.57, y=+122.87, z=+116.68, name="right-front-body-contact"), 97 | Vector(x=+61.49, y=+153.21, z=+129.97, name="right-front-coxia"), 98 | Vector(x=+189.21, y=+222.64, z=+151.78, name="right-front-femur"), 99 | Vector(x=+149.60, y=+203.72, z=-0.00, name="right-front-tibia"), 100 | ] 101 | 102 | leg2_points = [ 103 | Vector(x=-90.76, y=+84.12, z=+95.32, name="left-front-body-contact"), 104 | Vector(x=-150.83, y=+107.65, z=+87.44, name="left-front-coxia"), 105 | Vector(x=-276.55, y=+141.74, z=+155.58, name="left-front-femur"), 106 | Vector(x=-259.37, y=+163.25, z=+0.00, name="left-front-tibia"), 107 | ] 108 | 109 | leg3_points = [ 110 | Vector(x=-112.68, y=-45.33, z=+76.69, name="left-middle-body-contact"), 111 | Vector(x=-176.39, y=-50.56, z=+64.89, name="left-middle-coxia"), 112 | Vector(x=-293.99, y=-70.60, z=+150.78, name="left-middle-femur"), 113 | Vector(x=-340.16, y=-60.64, z=-0.00, name="left-middle-tibia"), 114 | ] 115 | 116 | leg4_points = [ 117 | Vector(x=-5.57, y=-122.87, z=+86.67, name="left-back-body-contact"), 118 | Vector(x=-58.45, y=-158.23, z=+73.33, name="left-back-coxia"), 119 | Vector(x=-165.26, y=-229.31, z=+145.09, name="left-back-femur"), 120 | Vector(x=-217.06, y=-264.36, z=+0.00, name="left-back-tibia"), 121 | ] 122 | 123 | leg5_points = [ 124 | Vector(x=+90.76, y=-84.12, z=+108.03, name="right-back-body-contact"), 125 | Vector(x=+121.84, y=-141.20, z=+106.97, name="right-back-coxia"), 126 | Vector(x=+180.23, y=-268.69, z=+151.07, name="right-back-femur"), 127 | Vector(x=+191.91, y=-223.89, z=+0.00, name="right-back-tibia"), 128 | ] 129 | 130 | 131 | correct_leg_points = [ 132 | leg0_points, 133 | leg1_points, 134 | leg2_points, 135 | leg3_points, 136 | leg4_points, 137 | leg5_points, 138 | ] 139 | -------------------------------------------------------------------------------- /hexapod/ik_solver/recompute_hexapod.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | import numpy as np 3 | from hexapod.models import VirtualHexapod, Hexagon 4 | from hexapod.points import ( 5 | angle_between, 6 | is_counter_clockwise, 7 | Vector, 8 | rotz, 9 | vector_from_to, 10 | length, 11 | ) 12 | from settings import ASSERTION_ENABLED, PRINT_IK 13 | 14 | 15 | def recompute_hexapod(dimensions, ik_parameters, poses): 16 | 17 | # make the hexapod with all angles = 0 18 | # update the hexapod so that we know which given points are in contact with the ground 19 | old_hexapod = VirtualHexapod(dimensions) 20 | old_hexapod.update_stance(ik_parameters["hip_stance"], ik_parameters["leg_stance"]) 21 | old_contacts = deepcopy(old_hexapod.ground_contacts) 22 | 23 | # make a new hexapod with all angles = 0 24 | # and update given the poses/ angles we've computed 25 | new_hexapod = VirtualHexapod(dimensions) 26 | new_hexapod.update(poses) 27 | new_contacts = deepcopy(new_hexapod.ground_contacts) 28 | 29 | # get two points that are on the ground before and after 30 | # updating to the given poses 31 | id1, id2 = find_two_same_leg_ids(old_contacts, new_contacts) 32 | 33 | old_p1 = deepcopy(old_hexapod.legs[id1].ground_contact()) 34 | old_p2 = deepcopy(old_hexapod.legs[id2].ground_contact()) 35 | new_p1 = deepcopy(new_hexapod.legs[id1].ground_contact()) 36 | new_p2 = deepcopy(new_hexapod.legs[id2].ground_contact()) 37 | 38 | # we must translate and rotate the hexapod with the pose 39 | # so that the hexapod is stepping on the old predefined ground contact points 40 | old_vector = vector_from_to(old_p1, old_p2) 41 | new_vector = vector_from_to(new_p1, new_p2) 42 | 43 | _, twist_frame = find_twist_to_recompute_hexapod(new_vector, old_vector) 44 | new_hexapod.rotate_and_shift(twist_frame, 0) 45 | 46 | twisted_p2 = new_hexapod.legs[id2].foot_tip() 47 | translate_vector = vector_from_to(twisted_p2, old_p2) 48 | new_hexapod.move_xyz(translate_vector.x, translate_vector.y, 0) 49 | 50 | might_sanity_check_points(new_p1, new_p2, old_p1, old_p2, new_vector, old_vector) 51 | 52 | return new_hexapod 53 | 54 | 55 | def make_contact_dict(ground_contact_list): 56 | # map index in ground_contact_list 57 | contact_dict = {} 58 | for contact in ground_contact_list: 59 | left_or_right, front_mid_back, _ = contact.name.split("-") 60 | leg_placement = left_or_right + "-" + front_mid_back 61 | leg_id = Hexagon.VERTEX_NAMES.index(leg_placement) 62 | contact_dict[leg_id] = leg_placement 63 | 64 | return contact_dict 65 | 66 | 67 | def find_two_same_leg_ids(old_contacts, new_contacts): 68 | same_ids = [] 69 | old_contact_dict = make_contact_dict(old_contacts) 70 | new_contact_dict = make_contact_dict(new_contacts) 71 | 72 | if PRINT_IK: 73 | print("In recomputing hexapod:") 74 | print("...old contacts:", old_contact_dict) 75 | print("...new_contacts: ", old_contact_dict) 76 | 77 | for leg_id in old_contact_dict: 78 | if leg_id not in new_contact_dict: 79 | continue 80 | 81 | same_ids.append(leg_id) 82 | if len(same_ids) == 2: 83 | return same_ids[0], same_ids[1] 84 | 85 | raise Exception( 86 | f"Need at least two same points on ground.\n\ 87 | old: {old_contact_dict}\n new: {new_contact_dict}" 88 | ) 89 | 90 | 91 | def find_twist_to_recompute_hexapod(a, b): 92 | twist = angle_between(a, b) 93 | z_axis = Vector(0, 0, -1) 94 | is_ccw = is_counter_clockwise(a, b, z_axis) 95 | if is_ccw: 96 | twist = -twist 97 | 98 | twist_frame = rotz(twist) 99 | return twist, twist_frame 100 | 101 | 102 | def should_be_on_ground_msg(point): 103 | return f"Vector should be on the ground:\n{point}, z != 0" 104 | 105 | 106 | def might_sanity_check_points(new_p1, new_p2, old_p1, old_p2, new_vector, old_vector): 107 | if not ASSERTION_ENABLED: 108 | return 109 | 110 | print("Sanity check on hexapod.ik_solver.recompute_hexapod") 111 | assert np.isclose(new_p1.z, 0, atol=1), should_be_on_ground_msg(new_p1) 112 | assert np.isclose(new_p2.z, 0, atol=1), should_be_on_ground_msg(new_p2) 113 | assert np.isclose(old_p1.z, 0, atol=1), should_be_on_ground_msg(old_p1) 114 | assert np.isclose(old_p2.z, 0, atol=1), should_be_on_ground_msg(old_p2) 115 | 116 | assert new_p1.name == old_p1.name, f"Should be the same name:\n{old_p1}\n{new_p1}" 117 | assert new_p2.name == old_p2.name, f"Should be the same name:\n{old_p2}\n{new_p2}" 118 | assert np.isclose( 119 | length(new_vector), length(old_vector), atol=1.0 120 | ), f"Should be same length.\nnew_vector:{new_vector}\n old_vector:{old_vector}" 121 | -------------------------------------------------------------------------------- /hexapod/ik_solver/helpers.py: -------------------------------------------------------------------------------- 1 | # Used for checking edge cases 2 | # and also for printing final results 3 | import json 4 | import numpy as np 5 | from settings import ( 6 | PRINT_IK_LOCAL_LEG, 7 | ASSERTION_ENABLED, 8 | PRINT_IK, 9 | BETA_MAX_ANGLE, 10 | GAMMA_MAX_ANGLE, 11 | ) 12 | from hexapod.points import length, vector_from_to, angle_between 13 | 14 | COXIA_ON_GROUND_ALERT_MSG = "Impossible at given height.\ncoxia joint shoved on ground" 15 | BODY_ON_GROUND_ALERT_MSG = "Impossible at given height.\nbody contact shoved on ground" 16 | 17 | 18 | def cant_reach_alert_msg(leg_name, problem): 19 | msg = "Cannot reach target ground point.\n" 20 | if problem == "femur": 21 | msg += f"Femur length of {leg_name} leg is too long." 22 | if problem == "tibia": 23 | msg += f"Femur length of {leg_name} leg is too long." 24 | else: 25 | # blocking 26 | msg = f"The {leg_name} leg cannot reach it because the ground is blocking the path." 27 | return msg 28 | 29 | 30 | def body_contact_shoved_on_ground(hexapod): 31 | for i in range(hexapod.LEG_COUNT): 32 | body_contact = hexapod.body.vertices[i] 33 | foot_tip = hexapod.legs[i].foot_tip() 34 | if body_contact.z < foot_tip.z: 35 | return True 36 | return False 37 | 38 | 39 | def legs_too_short(legs): 40 | # True when 41 | # if three of her left legs are up or 42 | # if three of her right legs are up or 43 | # if four legs are up 44 | if len(legs) >= 4: 45 | return True, f"Unstable. Too many legs off the floor.\n{legs}" 46 | 47 | leg_positions = [leg.split("-")[0] for leg in legs] 48 | if leg_positions.count("left") == 3: 49 | return True, f"Unstable. All left legs off the ground.\n{legs}" 50 | if leg_positions.count("right") == 3: 51 | return True, f"Unstable. All right legs off the ground.\n{legs}" 52 | 53 | return False, None 54 | 55 | 56 | def angle_above_limit(angle, angle_range, leg_name, angle_name): 57 | if np.abs(angle) > angle_range: 58 | alert_msg = f"The {angle_name} (of {leg_name} leg) required\n\ 59 | to do this pose is beyond the range of motion.\n\ 60 | Required: {angle} degrees. Limit: {angle_range} degrees." 61 | return True, alert_msg 62 | 63 | return False, None 64 | 65 | 66 | def beta_gamma_not_in_range(beta, gamma, leg_name): 67 | limit, msg = angle_above_limit(beta, BETA_MAX_ANGLE, leg_name, "(beta/femur)") 68 | if limit: 69 | return True, msg 70 | 71 | limit, msg = angle_above_limit(gamma, GAMMA_MAX_ANGLE, leg_name, "(gamma/tibia)") 72 | if limit: 73 | return True, msg 74 | 75 | return False, None 76 | 77 | 78 | def wrong_length_msg(leg_name, limb_name, limb_value): 79 | return f"Wrong {limb_name} vector length. {leg_name} coxia:{limb_value}" 80 | 81 | 82 | def might_sanity_leg_lengths_check(hexapod, leg_name, points): 83 | if not ASSERTION_ENABLED: 84 | return 85 | 86 | coxia = length(vector_from_to(points[0], points[1])) 87 | femur = length(vector_from_to(points[1], points[2])) 88 | tibia = length(vector_from_to(points[2], points[3])) 89 | 90 | same_length = np.isclose(hexapod.coxia, coxia, atol=1) 91 | assert same_length, wrong_length_msg(leg_name, "coxia", coxia) 92 | 93 | same_length = np.isclose(hexapod.femur, femur, atol=1) 94 | assert same_length, wrong_length_msg(leg_name, "femur", femur) 95 | 96 | same_length = np.isclose(hexapod.tibia, tibia, atol=1) 97 | assert same_length, wrong_length_msg(leg_name, "tibia", tibia) 98 | 99 | 100 | def might_sanity_beta_gamma_check(beta, gamma, leg_name, points): 101 | if not ASSERTION_ENABLED: 102 | return 103 | 104 | coxia = vector_from_to(points[0], points[1]) 105 | femur = vector_from_to(points[1], points[2]) 106 | tibia = vector_from_to(points[2], points[3]) 107 | result_beta = angle_between(coxia, femur) 108 | 109 | same_beta = np.isclose(np.abs(beta), result_beta, atol=1) 110 | assert same_beta, f"{leg_name} leg: expected: |{beta}|, found: {result_beta}" 111 | 112 | # ❗IMPORTANT: Sometimes both are zero. Is this wrong? 113 | femur_tibia_angle = angle_between(femur, tibia) 114 | is_90 = np.isclose(90, femur_tibia_angle + gamma, atol=1) 115 | 116 | if not is_90: 117 | alert_msg = f"{leg_name} leg:\ 118 | {femur_tibia_angle} (femur-tibia angle) + {gamma} (gamma) != 90." 119 | print(alert_msg) 120 | 121 | 122 | def might_print_ik(poses, ik_parameters, hexapod): 123 | if not PRINT_IK: 124 | return 125 | 126 | print("█████████████████████████████") 127 | print("█ START INVERSE KINEMATICS █") 128 | print("█████████████████████████████") 129 | 130 | print(".....................") 131 | print("... hexapod dimensions: ") 132 | print(".....................") 133 | print(json.dumps(hexapod.dimensions, indent=4)) 134 | 135 | print(".....................") 136 | print("... ik parameters: ") 137 | print(".....................") 138 | print(json.dumps(ik_parameters, indent=4)) 139 | 140 | print(".....................") 141 | print("... poses: ") 142 | print(".....................") 143 | print(json.dumps(poses, indent=4)) 144 | 145 | print("█████████████████████████████") 146 | print("█ END INVERSE KINEMATICS █") 147 | print("█████████████████████████████") 148 | 149 | 150 | def might_print_points(points, leg_name): 151 | if not PRINT_IK_LOCAL_LEG: 152 | return 153 | 154 | print(leg_name, "leg") 155 | for i, point in enumerate(points): 156 | print(f"...p{i}: {point}") 157 | print() 158 | -------------------------------------------------------------------------------- /hexapod/ik_solver/README.md: -------------------------------------------------------------------------------- 1 | # The Inverse Kinematics Algorithm 2 | 3 | ## Overview 4 | 5 | ```python 6 | # Given we know: 7 | # - The point where the hexapod body connect with each hexapod leg (point p0 aka body contact) 8 | # - Where the hexapod leg should connect with the ground (p3 aka target ground point) 9 | # 10 | # We want to solve for: 11 | # - All the 18 angles (alpha, beta, gamma) 12 | # - All the 12 joint points (coxia joint p1) and (femur joint p2) 13 | 14 | ``` 15 | 16 | ## Rough algorithm in the hexapod frame 17 | 18 | In the hexapod frame, the coxia vector is the projected vector (in the plane of the hexapod body) of the vector from the body contact pointing to the target ground point. 19 | 20 | ```python 21 | # body_to_foot_vector = vector from p0 to p3 22 | # coxia vector = projection of the body_to_foot vector to the hexapod body (hexagon) plane 23 | # 24 | # * (p3) Target foot tip 25 | # *----*----* / 26 | # / \ / 27 | # / cog \ / 28 | # * * */ (p0) body contact 29 | # \ / 30 | # \ / 31 | # *----*----* 32 | # 33 | # hexapod y_axis 34 | # | 35 | # | 36 | # * - -> hexapod x_axis 37 | ``` 38 | 39 | We can find the angle between the coxia vector wrt to the x axis of the hexapod. 40 | Then we can find the angle alpha which is the relative x axis for each attached linkage 41 | as shown in the figure which is the alpha. 42 | 43 | ```python 44 | # Relative x-axis, for each attached linkage 45 | # Angle each respective relative angle makes wrt to the 46 | # COXIA_AXES = [0, 45, 135, 180, 225, 315] starting from x0 to x5 47 | # 48 | # x2 x1 49 | # \ / 50 | # *---*---* 51 | # / | \ 52 | # / | \ 53 | # / | \ 54 | # x3 --*------cog------*-- x0 55 | # \ | / 56 | # \ | / 57 | # \ | / 58 | # *---*---* 59 | # / \ 60 | # x4 x5 61 | # 62 | # hexapod y 63 | # | 64 | # | 65 | # * - - hexapod x 66 | ``` 67 | 68 | ## Rough algorithm in the local leg frame 69 | 70 | ```python 71 | # |--coxia-----\ ---- 72 | # p0 ------- p1 \ \ 73 | # \ femur 74 | # \ \ 75 | # p2 --- 76 | # / / 77 | # / tibia 78 | # / / 79 | # p3 ------- 80 | # 81 | # Given / Knowns: 82 | # - point p0 83 | # - point p3 84 | # - coxia length 85 | # - femur length 86 | # - tibia length 87 | # - given p0 and p1 lie on the same known axis 88 | # 89 | # Find /Unknowns: 90 | # p1 91 | # p2 92 | # beta - angle between coxia vector (leg x axis) and femur vector 93 | # gamma - angle between tibia vector and perpendicular vector to femur vector 94 | ``` 95 | 96 | ## Definitions and Cases 97 | 98 | ```python 99 | # 100 | # 101 | # ********************** 102 | # DEFINITIONS 103 | # ********************** 104 | # p0: Body contact point 105 | # p1: coxia point / coxia joint (point between coxia limb and femur limb) 106 | # p2: tibia point / tibia joint (point between femur limb and tibia limb) 107 | # p3: foot tip / ground contact 108 | # coxia vector - vector from p0 to p1 109 | # hexapod.coxia - coxia vector length 110 | # femur vector - vector from p1 to p2 111 | # hexapod.femur - femur vector length 112 | # tibia vector - vector from p2 to p3 113 | # hexapod.tibia - tibia vector length 114 | # body_to_foot vector - vector from p0 to p3 115 | # coxia_to_foot vector - vector from p1 to p3 116 | # d: coxia_to_foot_length 117 | # body_to_foot_length 118 | # 119 | # rho 120 | # -- angle between coxia vector (leg x axis) and body to foot vector 121 | # -- angle between point p1, p0, and p3. (p0 at center) 122 | # theta 123 | # --- angle between femur vector and coxia to foot vector 124 | # --- angle between point p2, p1, and p3. (p1 at center) 125 | # phi 126 | # --- angle between coxia vector (leg x axis) and coxia to foot vector 127 | # 128 | # beta 129 | # --- angle between coxia vector (leg x axis) and femur vector 130 | # --- positive is counter clockwise 131 | # gamma 132 | # --- angle between tibia vector and perpendicular vector to femur vector 133 | # --- positive is counter clockwise 134 | # alpha 135 | # --- angle between leg coordinate frame and axis defined by line from 136 | # hexapod's center of gravity to body contact. 137 | # 138 | # 139 | # For CASE 1 and CASE 2: 140 | # beta = theta - phi 141 | # beta is positive when phi < theta (case 1) 142 | # beta is negative when phi > theta (case 2) 143 | # ***************** 144 | # Case 1 (beta IS POSITIVE) 145 | # ***************** 146 | # 147 | # ^ (p2) 148 | # | * 149 | # (leg_z_axis) / | 150 | # | / | 151 | # | / | 152 | # (p0) (p1)/ | 153 | # *------- *-----| ----------------> (leg_x_axis) 154 | # \ \ | 155 | # \ \ | 156 | # \ \ | 157 | # \ \ | 158 | # \ \ | 159 | # \ | 160 | # \ * (p3) 161 | # 162 | # 163 | # ***************** 164 | # Case 2 (beta is negative) 165 | # ***************** 166 | # ^ 167 | # | 168 | # (leg_z_axis direction) 169 | # (p0) (p1) | 170 | # *------- *----------------|------> (leg_x_axis direction) 171 | # \ | \ 172 | # \ | \ 173 | # \ | \ 174 | # \ | * (p2) 175 | # \ | / 176 | # \ | / 177 | # \ | / 178 | # \ | / 179 | # \| / 180 | # * 181 | # 182 | # ***************** 183 | # Case 3 (p3 is above p1) then beta = phi + theta 184 | # ***************** 185 | # * (p2) 186 | # / \ 187 | # / | 188 | # / * (p3) 189 | # / / 190 | # *------ * / 191 | # (p0) (p1) 192 | # 193 | # 194 | ``` 195 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![](https://img.shields.io/badge/Buy%20me%20-coffee!-orange.svg?logo=buy-me-a-coffee&color=795548)](https://ko-fi.com/minimithi) 2 | [![Build Status](https://travis-ci.com/mithi/hexapod-robot-simulator.svg?branch=master)](https://travis-ci.com/github/mithi/hexapod-robot-simulator) 3 | [![codecov](https://codecov.io/gh/mithi/hexapod-robot-simulator/branch/master/graph/badge.svg)](https://codecov.io/gh/mithi/hexapod-robot-simulator) 4 | [![Code Climate](https://codeclimate.com/github/mithi/hexapod-robot-simulator/badges/gpa.svg)](https://codeclimate.com/github/mithi/hexapod-robot-simulator) 5 | [![](https://img.shields.io/codeclimate/tech-debt/mithi/hexapod-robot-simulator)](https://codeclimate.com/github/mithi/hexapod-robot-simulator/trends/technical_debt) 6 | [![HitCount](https://hits.dwyl.com/mithi/hexapod-robot-simulator.svg)](https://hits.dwyl.com/mithi/hexapod-robot-simulator) 7 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](./LICENSE) 8 | [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) 9 | [![PRs welcome!](https://img.shields.io/badge/contributions-welcome-orange.svg?style=flat)](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3A%22help+wanted%22) 10 | [![first-timers-only](https://img.shields.io/badge/first--timers--only-friendly-blueviolet.svg?style=flat)](https://www.firsttimersonly.com/) 11 | 12 | # Mithi's Hexapod Robot Simulator 13 | 14 | - A bare minimum browser-based hexapod robot simulator built from first principles 🕷️ 15 | - If you like this project, consider [buying me a few ☕ cups of coffee](https://ko-fi.com/minimithi). 💕 16 | 17 | | | | | | 18 | |---------|---------|---------|---------| 19 | |![Twisting turning and tilting](https://mithi.github.io/robotics-blog/robot-only-x1.gif)|||![Adjusting camera view](https://mithi.github.io/robotics-blog/robot-only-x3.gif)| 20 | 21 | # Announcement 22 | 23 | You might be interested in checking out my [rewrite in Javascript](http://github.com/mithi/hexapod), live at: https://hexapod.netlify.app/ , which is like 10000000x faster. If you'd like to build you're own user interface with Node, you can download the algorithm alone as a package in the npm registry: [Hexapod Kinematics Library](https://github.com/mithi/hexapod-kinematics-library). There is also [a "fork" modified where you can use the app to control a physical hexapod robot](https://github.com/mithi/hexapod-irl) as you can see in the gif below. 24 | 25 |

26 | drawing 27 |

28 | 29 | # Features 30 | 31 | | STATUS | FEATURE | DESCRIPTION | 32 | |---|-----------|--------------| 33 | | 🎉 | Forward Kinematics | Given the angles of each joint, what does the robot look like?| 34 | | 🎉 | Inverse Kinematics | What are the angles of each joint to make the robot look the way I want? Is it even possible? Why or why not? | 35 | | 🎉 | Uniform Movements | If all of the legs behaved the same way, how will the hexapod robot as a whole behave? | 36 | | 🎉 | Customizability | Set the dimensions and shape of the robot's body and legs. (6 parameters) | 37 | | 🎉 | Usability | Control the camera view, pan, tilt, zoom, whatever. | 38 | | 🎉 | Simplicity | Minimal dependencies. Depends solely on Numpy for calculations. Uses only Plotly Dash for plotting, Dash can be safely replaced if a better 3d plotting library is available. | 39 | | ❗ | Stability Check (WIP) | If we pose the robot in a particular way, will it fall over? | 40 | | ❗ | Fast | Okay, it's not as fast as I wanted, but when run locally, it's okay | 41 | | ❗ | Bug-free | Fine, right now there's still room for improvement | 42 | | ❗ | Well-tested | Yeah, I need to compile test cases first | 43 | 44 | ## Preview 45 | 46 | |![image](https://mithi.github.io/robotics-blog/v2-ik-ui.gif)|![image](https://mithi.github.io/robotics-blog/v2-kinematics-ui.gif)| 47 | |----|----| 48 | | ![image](https://mithi.github.io/robotics-blog/UI-1.gif) | ![image](https://mithi.github.io/robotics-blog/UI-2.gif) | 49 | 50 | ## Requirements 51 | 52 | - [x] Python 3.8.1 53 | - [x] Plotly Dash 1.18.1 54 | - [x] Plotly Dash Daq 0.5.0 55 | - [x] Numpy 1.19.5 56 | - [x] See also [./requirements.txt](./requirements.txt) 57 | 58 | ## Run 59 | 60 | ```bash 61 | $ python index.py 62 | Running on http://127.0.0.1:8050/ 63 | ``` 64 | 65 | - Modify default settings with [./settings.py](./settings.py) 66 | - Dark Mode is the default - modify page styles with [./style_settings.py](./style_settings.py) 67 | 68 | ## Screenshots 69 | 70 | | ![Kinematics](https://mithi.github.io/robotics-blog/v2-kinematics-screenshot.png)| 71 | |---| 72 | | ![IK](https://mithi.github.io/robotics-blog/v2-ik-screenshot.png)| 73 | 74 | ## More Information 75 | Check the [Wiki](https://github.com/mithi/hexapod-robot-simulator/wiki/Notes) for more additional information 76 | 77 | ## 🤗 Contributors 78 | 79 | - [@mithi](https://github.com/mithi/) 80 | - [@philippeitis](https://github.com/philippeitis/) 81 | - [@mikong](https://github.com/mikong/) 82 | - [@guilyx](https://github.com/guilyx) 83 | - [@markkulube](https://github.com/markkulube) 84 | 85 | ![](https://img.shields.io/github/last-commit/mithi/hexapod-robot-simulator) 86 | ![](https://img.shields.io/github/commit-activity/y/mithi/hexapod-robot-simulator) 87 | ![](https://img.shields.io/github/languages/code-size/mithi/hexapod-robot-simulator?color=yellow) 88 | ![](https://img.shields.io/github/repo-size/mithi/hexapod-robot-simulator?color=violet) 89 | ![](https://tokei.rs/b1/github/mithi/hexapod-robot-simulator?category=blanks) 90 | ![](https://tokei.rs/b1/github/mithi/hexapod-robot-simulator?category=lines) 91 | ![](https://tokei.rs/b1/github/mithi/hexapod-robot-simulator?category=files) 92 | ![](https://tokei.rs/b1/github/mithi/hexapod-robot-simulator?category=comments) 93 | ![](https://tokei.rs/b1/github/mithi/hexapod-robot-simulator?category=code) 94 | ![](https://img.shields.io/github/languages/top/mithi/hexapod-robot-simulator) 95 | -------------------------------------------------------------------------------- /hexapod/linkage.py: -------------------------------------------------------------------------------- 1 | # ------------- 2 | # LINKAGE 3 | # ------------- 4 | # Neutral position of the linkages (alpha=0, beta=0, gamma=0) 5 | # note that at neutral position: 6 | # link b and link c are perpendicular to each other 7 | # link a and link b form a straight line 8 | # link a and the leg x axis are aligned 9 | # 10 | # alpha - the angle linkage a makes with x_axis about z axis 11 | # beta - the angle that linkage a makes with linkage b 12 | # gamma - the angle that linkage c make with the line perpendicular to linkage b 13 | # 14 | # 15 | # MEASUREMENTS 16 | # 17 | # |--- a--------|--b--| 18 | # |=============|=====| p2 ------- 19 | # p0 p1 | | 20 | # | | 21 | # | c 22 | # | | 23 | # | | 24 | # | p3 ------ 25 | # 26 | # p0 - body contact 27 | # p1 - coxia point 28 | # p2 - femur point 29 | # p3 - foot tip 30 | # 31 | # z axis 32 | # | 33 | # | 34 | # |------- x axis 35 | # origin 36 | # 37 | # 38 | # ANGLES beta and gamma 39 | # / 40 | # / beta 41 | # ---- /* --------- 42 | # / //\\ \ 43 | # b // \\ \ 44 | # / // \\ c 45 | # / //beta \\ \ 46 | # *=======* ----> \\ \ 47 | # |---a---| \\ \ 48 | # *----------- 49 | # 50 | # |--a--|---b----| 51 | # *=====*=========* ------------- 52 | # | \\ \ 53 | # | \\ \ 54 | # | \\ c 55 | # | \\ \ 56 | # |gamma\\ \ 57 | # | *---------------- 58 | # 59 | from copy import deepcopy 60 | import numpy as np 61 | from hexapod.points import ( 62 | Vector, 63 | frame_yrotate_xtranslate, 64 | frame_zrotate_xytranslate, 65 | ) 66 | 67 | 68 | class Linkage: 69 | POINT_NAMES = ["coxia", "femur", "tibia"] 70 | 71 | __slots__ = ( 72 | "a", 73 | "b", 74 | "c", 75 | "alpha", 76 | "beta", 77 | "gamma", 78 | "coxia_axis", 79 | "new_origin", 80 | "name", 81 | "id", 82 | "all_points", 83 | "ground_contact_point", 84 | ) 85 | 86 | def __init__( 87 | self, 88 | a, 89 | b, 90 | c, 91 | alpha=0, 92 | beta=0, 93 | gamma=0, 94 | coxia_axis=0, 95 | new_origin=Vector(0, 0, 0), 96 | name=None, 97 | id_number=None, 98 | ): 99 | self.a = a 100 | self.b = b 101 | self.c = c 102 | self.new_origin = new_origin 103 | self.coxia_axis = coxia_axis 104 | self.id = id_number 105 | self.name = name 106 | self.change_pose(alpha, beta, gamma) 107 | 108 | def coxia_angle(self): 109 | return self.alpha 110 | 111 | def body_contact(self): 112 | return self.all_points[0] 113 | 114 | def coxia_point(self): 115 | return self.all_points[1] 116 | 117 | def femur_point(self): 118 | return self.all_points[2] 119 | 120 | def foot_tip(self): 121 | return self.all_points[3] 122 | 123 | def ground_contact(self): 124 | return self.ground_contact_point 125 | 126 | def get_point(self, i): 127 | return self.all_points[i] 128 | 129 | def change_pose(self, alpha, beta, gamma): 130 | self.alpha = alpha 131 | self.beta = beta 132 | self.gamma = gamma 133 | 134 | # frame_ab is the pose of frame_b wrt frame_a 135 | frame_01 = frame_yrotate_xtranslate(theta=-self.beta, x=self.a) 136 | frame_12 = frame_yrotate_xtranslate(theta=90 - self.gamma, x=self.b) 137 | frame_23 = frame_yrotate_xtranslate(theta=0, x=self.c) 138 | 139 | frame_02 = np.matmul(frame_01, frame_12) 140 | frame_03 = np.matmul(frame_02, frame_23) 141 | new_frame = frame_zrotate_xytranslate( 142 | self.coxia_axis + self.alpha, self.new_origin.x, self.new_origin.y 143 | ) 144 | 145 | # find points wrt to body contact point 146 | p0 = Vector(0, 0, 0) 147 | p1 = p0.get_point_wrt(frame_01) 148 | p2 = p0.get_point_wrt(frame_02) 149 | p3 = p0.get_point_wrt(frame_03) 150 | 151 | # find points wrt to center of gravity 152 | p0 = deepcopy(self.new_origin) 153 | p0.name += "-body-contact" 154 | p1 = p1.get_point_wrt(new_frame, name=self.name + "-coxia") 155 | p2 = p2.get_point_wrt(new_frame, name=self.name + "-femur") 156 | p3 = p3.get_point_wrt(new_frame, name=self.name + "-tibia") 157 | 158 | self.all_points = [p0, p1, p2, p3] 159 | self.ground_contact_point = self.compute_ground_contact() 160 | 161 | def update_leg_wrt(self, frame, height): 162 | for point in self.all_points: 163 | point.update_point_wrt(frame, height) 164 | 165 | def compute_ground_contact(self): 166 | # ❗IMPORTANT: Verify if this assumption is correct 167 | # ❗VERIFIED: This assumption is indeed wrong 168 | ground_contact = self.all_points[3] 169 | for point in reversed(self.all_points): 170 | if point.z < ground_contact.z: 171 | ground_contact = point 172 | 173 | return ground_contact 174 | 175 | def __str__(self): 176 | leg_string = f"{self!r}\n" 177 | leg_string += f"Vectors of {self.name} leg:\n" 178 | 179 | for point in self.all_points: 180 | leg_string += f" {point}\n" 181 | 182 | leg_string += f" ground contact: {self.ground_contact()}\n" 183 | return leg_string 184 | 185 | def __repr__(self): 186 | return f"""Linkage( 187 | a={self.a}, 188 | b={self.b}, 189 | c={self.c}, 190 | alpha={self.alpha}, 191 | beta={self.beta}, 192 | gamma={self.gamma}, 193 | coxia_axis={self.coxia_axis}, 194 | id_number={self.id}, 195 | name='{self.name}', 196 | new_origin={self.new_origin}, 197 | )""" 198 | 199 | 200 | # 201 | # /* 202 | # //\\ 203 | # // \\ 204 | # // \\ 205 | # // \\ 206 | # *===* ----> \\ --------- 207 | # \\ | 208 | # \\ tip height (positive) 209 | # \\ | 210 | # \\ ----- 211 | # 212 | # 213 | # *===*=======* 214 | # | \\ 215 | # | \\ 216 | # (positive)| \\ 217 | # tip height \\ 218 | # | \\ 219 | # ------ *---- 220 | # 221 | # *=========* ----- 222 | # // | 223 | # // (negative) tip height 224 | # // | 225 | # *===*=======* ------------------- 226 | # Negative only if body contact point 227 | # is touching the ground 228 | -------------------------------------------------------------------------------- /hexapod/points.py: -------------------------------------------------------------------------------- 1 | # This module contains the Vector class 2 | # and functions for manipulating vectors 3 | # and finding properties and relationships of vectors 4 | # computing reference frames 5 | from math import sqrt, radians, sin, cos, degrees, acos, isnan 6 | import numpy as np 7 | 8 | from settings import DEBUG_MODE 9 | 10 | 11 | class Vector: 12 | __slots__ = ("x", "y", "z", "name") 13 | 14 | def __init__(self, x, y, z, name=None): 15 | self.x = x 16 | self.y = y 17 | self.z = z 18 | self.name = name 19 | 20 | def get_point_wrt(self, reference_frame, name=None): 21 | """ 22 | Given frame_ab which is the pose of frame_b wrt frame_a 23 | and that this point is defined wrt to frame_b 24 | Return point defined wrt to frame a 25 | """ 26 | p = np.array([self.x, self.y, self.z, 1]) 27 | p = np.matmul(reference_frame, p) 28 | return Vector(p[0], p[1], p[2], name) 29 | 30 | def update_point_wrt(self, reference_frame, z=0): 31 | p = np.array([self.x, self.y, self.z, 1]) 32 | p = np.matmul(reference_frame, p) 33 | self.x = p[0] 34 | self.y = p[1] 35 | self.z = p[2] + z 36 | 37 | def move_xyz(self, x, y, z): 38 | self.x += x 39 | self.y += y 40 | self.z += z 41 | 42 | def move_up(self, z): 43 | self.z += z 44 | 45 | @property 46 | def vec(self): 47 | return self.x, self.y, self.z 48 | 49 | def __repr__(self): 50 | s = f"Vector(x={self.x:>+8.2f}, y={self.y:>+8.2f}, z={self.z:>+8.2f}, name='{self.name}')" 51 | return s 52 | 53 | def __str__(self): 54 | return repr(self) 55 | 56 | def __eq__(self, other, percent_tol=0.0075): 57 | if not isinstance(other, Vector): 58 | return False 59 | 60 | tol = length(self) * percent_tol 61 | equal_val = np.allclose(self.vec, other.vec, atol=tol) 62 | equal_name = self.name == other.name 63 | return equal_val and equal_name 64 | 65 | 66 | # ********************************************* 67 | # https://stackoverflow.com/questions/2049582/how-to-determine-if-a-point-is-in-a-2d-triangle 68 | # https://www.geeksforgeeks.org/check-whether-a-given-point-lies-inside-a-triangle-or-not/ 69 | # It works like this: 70 | # - Walk clockwise or counterclockwise around the triangle 71 | # and project the point onto the segment we are crossing 72 | # by using the dot product. 73 | # - Check that the vector created is on the same side 74 | # for each of the triangle's segments 75 | def is_point_inside_triangle(p, a, b, c): 76 | ab = (p.x - b.x) * (a.y - b.y) - (a.x - b.x) * (p.y - b.y) 77 | bc = (p.x - c.x) * (b.y - c.y) - (b.x - c.x) * (p.y - c.y) 78 | ca = (p.x - a.x) * (c.y - a.y) - (c.x - a.x) * (p.y - a.y) 79 | # must be all positive or all negative 80 | return (ab < 0.0) == (bc < 0.0) == (ca < 0.0) 81 | 82 | 83 | def is_triangle(a, b, c): 84 | return (a + b > c) and (a + c > b) and (b + c > a) 85 | 86 | 87 | # https://www.maplesoft.com/support/help/Maple/view.aspx?path=MathApps%2FProjectionOfVectorOntoPlane 88 | # u is the vector, n is the plane normal 89 | def project_vector_onto_plane(u, n): 90 | s = dot(u, n) / dot(n, n) 91 | temporary_vector = scalar_multiply(n, s) 92 | return subtract_vectors(u, temporary_vector) 93 | 94 | 95 | def might_print_angle_between_error(a, b): 96 | if DEBUG_MODE: 97 | print( 98 | f"❗❗❗ERROR: angle_between({a}, {b}) is NAN\ 99 | ... One of the might be a zero vector\ 100 | ... the vectors might be pointing at the same direction or\ 101 | ... something else entirely. 🤔" 102 | ) 103 | 104 | 105 | def angle_between(a, b): 106 | # returns the shortest angle between two vectors 107 | cos_theta = dot(a, b) / sqrt(dot(a, a) * dot(b, b)) 108 | theta = degrees(acos(cos_theta)) 109 | 110 | if isnan(theta): 111 | might_print_angle_between_error(a, b) 112 | return 0.0 113 | 114 | return theta 115 | 116 | 117 | def angle_opposite_of_last_side(a, b, c): 118 | ratio = (a * a + b * b - c * c) / (2 * a * b) 119 | return degrees(acos(ratio)) 120 | 121 | 122 | # Check if angle from vector a to b about normal n is positive 123 | # Rotating from vector a to is moving into a conter clockwise direction 124 | def is_counter_clockwise(a, b, n): 125 | return dot(a, cross(b, n)) > 0 126 | 127 | 128 | # https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d 129 | def frame_to_align_vector_a_to_b(a, b): 130 | v = cross(a, b) 131 | s = length(v) 132 | 133 | # When angle between a and b is zero or 180 degrees 134 | # cross product is 0, R = I 135 | if s == 0.0: 136 | return np.eye(4) 137 | c = dot(a, b) 138 | i = np.eye(3) # Identity matrix 3x3 139 | 140 | # skew symmetric cross product 141 | vx = skew(v) 142 | d = (1 - c) / (s * s) 143 | r = i + vx + np.matmul(vx, vx) * d 144 | 145 | # r00 r01 r02 0 146 | # r10 r11 r12 0 147 | # r20 r21 r22 0 148 | # 0 0 0 1 149 | r = np.hstack((r, [[0], [0], [0]])) 150 | r = np.vstack((r, [0, 0, 0, 1])) 151 | return r 152 | 153 | 154 | # rotate about y, translate in x 155 | def frame_yrotate_xtranslate(theta, x): 156 | c, s = _return_sin_and_cos(theta) 157 | 158 | return np.array([[c, 0, s, x], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) 159 | 160 | 161 | # rotate about z, translate in x and y 162 | def frame_zrotate_xytranslate(theta, x, y): 163 | c, s = _return_sin_and_cos(theta) 164 | 165 | return np.array([[c, -s, 0, x], [s, c, 0, y], [0, 0, 1, 0], [0, 0, 0, 1]]) 166 | 167 | 168 | def frame_rotxyz(a, b, c): 169 | rx = rotx(a) 170 | ry = roty(b) 171 | rz = rotz(c) 172 | rxy = np.matmul(rx, ry) 173 | rxyz = np.matmul(rxy, rz) 174 | return rxyz 175 | 176 | 177 | def rotx(theta): 178 | c, s = _return_sin_and_cos(theta) 179 | return np.array([[1, 0, 0, 0], [0, c, -s, 0], [0, s, c, 0], [0, 0, 0, 1]]) 180 | 181 | 182 | def roty(theta): 183 | c, s = _return_sin_and_cos(theta) 184 | return np.array([[c, 0, s, 0], [0, 1, 0, 0], [-s, 0, c, 0], [0, 0, 0, 1]]) 185 | 186 | 187 | def rotz(theta): 188 | c, s = _return_sin_and_cos(theta) 189 | return np.array([[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) 190 | 191 | 192 | def _return_sin_and_cos(theta): 193 | d = radians(theta) 194 | c = cos(d) 195 | s = sin(d) 196 | return c, s 197 | 198 | 199 | # get vector pointing from point a to point b 200 | def vector_from_to(a, b): 201 | return Vector(b.x - a.x, b.y - a.y, b.z - a.z) 202 | 203 | 204 | def scale(v, d): 205 | return Vector(v.x / d, v.y / d, v.z / d) 206 | 207 | 208 | def dot(a, b): 209 | return a.x * b.x + a.y * b.y + a.z * b.z 210 | 211 | 212 | def cross(a, b): 213 | x = a.y * b.z - a.z * b.y 214 | y = a.z * b.x - a.x * b.z 215 | z = a.x * b.y - a.y * b.x 216 | return Vector(x, y, z) 217 | 218 | 219 | def length(v): 220 | return sqrt(dot(v, v)) 221 | 222 | 223 | def add_vectors(a, b): 224 | return Vector(a.x + b.x, a.y + b.y, a.z + b.z) 225 | 226 | 227 | def subtract_vectors(a, b): 228 | return Vector(a.x - b.x, a.y - b.y, a.z - b.z) 229 | 230 | 231 | def scalar_multiply(p, s): 232 | return Vector(s * p.x, s * p.y, s * p.z) 233 | 234 | 235 | def get_unit_vector(v): 236 | return scale(v, length(v)) 237 | 238 | 239 | def get_normal_given_three_points(a, b, c): 240 | """ 241 | Get the unit normal vector to the 242 | plane defined by the points a, b, c. 243 | """ 244 | ab = vector_from_to(a, b) 245 | ac = vector_from_to(a, c) 246 | v = cross(ab, ac) 247 | v = scale(v, length(v)) 248 | return v 249 | 250 | 251 | def skew(p): 252 | return np.array([[0, -p.z, p.y], [p.z, 0, -p.x], [-p.y, p.x, 0]]) 253 | -------------------------------------------------------------------------------- /hexapod/ik_solver/ik_solver.py: -------------------------------------------------------------------------------- 1 | # Please look at the discussion of the Inverse Kinematics algorithm 2 | # As detailed in the README of this directory 3 | 4 | from copy import deepcopy 5 | import numpy as np 6 | from settings import ASSERTION_ENABLED, ALPHA_MAX_ANGLE 7 | from hexapod.ik_solver.helpers import ( 8 | BODY_ON_GROUND_ALERT_MSG, 9 | COXIA_ON_GROUND_ALERT_MSG, 10 | cant_reach_alert_msg, 11 | body_contact_shoved_on_ground, 12 | legs_too_short, 13 | beta_gamma_not_in_range, 14 | angle_above_limit, 15 | might_sanity_leg_lengths_check, 16 | might_sanity_beta_gamma_check, 17 | might_print_ik, 18 | might_print_points, 19 | ) 20 | from hexapod.points import ( 21 | Vector, 22 | length, 23 | add_vectors, 24 | scalar_multiply, 25 | vector_from_to, 26 | get_unit_vector, 27 | is_triangle, 28 | project_vector_onto_plane, 29 | angle_between, 30 | angle_opposite_of_last_side, 31 | ) 32 | from hexapod.ik_solver.shared import ( 33 | update_hexapod_points, 34 | find_twist_frame, 35 | compute_twist_wrt_to_world, 36 | ) 37 | from hexapod.const import HEXAPOD_POSE 38 | 39 | 40 | # Please checkout the definition of the 41 | # inverse_kinematics_update function 42 | # as described in hexapod.ik_solver.ik_solver2 43 | poses = deepcopy(HEXAPOD_POSE) 44 | 45 | 46 | def inverse_kinematics_update(hexapod, ik_parameters): 47 | 48 | tx = ik_parameters["percent_x"] * hexapod.mid 49 | ty = ik_parameters["percent_y"] * hexapod.side 50 | tz = ik_parameters["percent_z"] * hexapod.tibia 51 | rotx, roty, rotz = ( 52 | ik_parameters["rot_x"], 53 | ik_parameters["rot_y"], 54 | ik_parameters["rot_z"], 55 | ) 56 | 57 | hexapod.update_stance(ik_parameters["hip_stance"], ik_parameters["leg_stance"]) 58 | hexapod.detach_body_rotate_and_translate(rotx, roty, rotz, tx, ty, tz) 59 | 60 | if body_contact_shoved_on_ground(hexapod): 61 | raise Exception(BODY_ON_GROUND_ALERT_MSG) 62 | 63 | x_axis = Vector(1, 0, 0) 64 | legs_up_in_the_air = [] 65 | 66 | for i in range(hexapod.LEG_COUNT): 67 | leg_name = hexapod.legs[i].name 68 | body_contact = hexapod.body.vertices[i] 69 | foot_tip = hexapod.legs[i].foot_tip() 70 | body_to_foot_vector = vector_from_to(body_contact, foot_tip) 71 | 72 | # find the coxia vector which is the vector 73 | # from body contact point to joint between coxia and femur limb 74 | projection = project_vector_onto_plane(body_to_foot_vector, hexapod.z_axis) 75 | unit_coxia_vector = get_unit_vector(projection) 76 | coxia_vector = scalar_multiply(unit_coxia_vector, hexapod.coxia) 77 | 78 | # coxia point / joint is the point connecting the coxia and tibia limbs 79 | coxia_point = add_vectors(body_contact, coxia_vector) 80 | if coxia_point.z < foot_tip.z: 81 | raise Exception(COXIA_ON_GROUND_ALERT_MSG) 82 | 83 | # ******************* 84 | # 1. Compute p0, p1 and (possible) p3 wrt leg frame 85 | # ******************* 86 | p0 = Vector(0, 0, 0) 87 | p1 = Vector(hexapod.coxia, 0, 0) 88 | 89 | # Find p3 aka foot tip (ground contact) with respect to the local leg frame 90 | rho = angle_between(unit_coxia_vector, body_to_foot_vector) 91 | body_to_foot_length = length(body_to_foot_vector) 92 | p3x = body_to_foot_length * np.cos(np.radians(rho)) 93 | p3z = -body_to_foot_length * np.sin(np.radians(rho)) 94 | p3 = Vector(p3x, 0, p3z) 95 | 96 | # ******************* 97 | # 2. Compute p2, beta, gamma and final p3 wrt leg frame 98 | # ******************* 99 | 100 | # These values are needed to compute 101 | # p2 aka tibia joint (point between femur limb and tibia limb) 102 | coxia_to_foot_vector2d = vector_from_to(p1, p3) 103 | d = length(coxia_to_foot_vector2d) 104 | 105 | # If we can form this triangle this means 106 | # we can probably can reach the target ground contact point 107 | if is_triangle(hexapod.tibia, hexapod.femur, d): 108 | # ................................. 109 | # CASE A: a triangle can be formed with 110 | # coxia to foot vector, hexapod's femur and tibia 111 | # ................................. 112 | theta = angle_opposite_of_last_side(d, hexapod.femur, hexapod.tibia) 113 | phi = angle_between(coxia_to_foot_vector2d, x_axis) 114 | 115 | beta = theta - phi # case 1 or 2 116 | if p3.z > 0: # case 3 117 | beta = theta + phi 118 | 119 | z_ = hexapod.femur * np.sin(np.radians(beta)) 120 | x_ = p1.x + hexapod.femur * np.cos(np.radians(beta)) 121 | 122 | p2 = Vector(x_, 0, z_) 123 | femur_vector = vector_from_to(p1, p2) 124 | tibia_vector = vector_from_to(p2, p3) 125 | gamma = 90 - angle_between(femur_vector, tibia_vector) 126 | 127 | if p2.z < p3.z: 128 | raise Exception(cant_reach_alert_msg(leg_name, "blocking")) 129 | else: 130 | # ................................. 131 | # CASE B: It's impossible to reach target ground point 132 | # ................................. 133 | if d + hexapod.tibia < hexapod.femur: 134 | raise Exception(cant_reach_alert_msg(leg_name, "femur")) 135 | if d + hexapod.femur < hexapod.tibia: 136 | raise Exception(cant_reach_alert_msg(leg_name, "tibia")) 137 | 138 | # Then hexapod.femur + hexapod.tibia < d: 139 | legs_up_in_the_air.append(leg_name) 140 | LEGS_TOO_SHORT, alert_msg = legs_too_short(legs_up_in_the_air) 141 | if LEGS_TOO_SHORT: 142 | raise Exception(alert_msg) 143 | 144 | femur_tibia_direction = get_unit_vector(coxia_to_foot_vector2d) 145 | femur_vector = scalar_multiply(femur_tibia_direction, hexapod.femur) 146 | p2 = add_vectors(p1, femur_vector) 147 | tibia_vector = scalar_multiply(femur_tibia_direction, hexapod.tibia) 148 | p3 = add_vectors(p2, tibia_vector) 149 | 150 | # Find beta and gamma 151 | gamma = 0.0 152 | leg_x_axis = Vector(1, 0, 0) 153 | beta = angle_between(leg_x_axis, femur_vector) 154 | if femur_vector.z < 0: 155 | beta = -beta 156 | 157 | # Final p1, p2, p3, beta and gamma computed at this point 158 | not_within_range, alert_msg = beta_gamma_not_in_range(beta, gamma, leg_name) 159 | if not_within_range: 160 | raise Exception(alert_msg) 161 | 162 | # ******************* 163 | # 3. Compute alpha and twist_frame 164 | # Find frame used to twist the leg frame wrt to hexapod's body contact point's x axis 165 | # ******************* 166 | alpha, twist_frame = find_twist_frame(hexapod, unit_coxia_vector) 167 | alpha = compute_twist_wrt_to_world(alpha, hexapod.body.COXIA_AXES[i]) 168 | alpha_limit, alert_msg = angle_above_limit( 169 | alpha, ALPHA_MAX_ANGLE, leg_name, "(alpha/coxia)" 170 | ) 171 | 172 | if alpha_limit: 173 | raise Exception(alert_msg) 174 | 175 | # ******************* 176 | # 4. Update hexapod points and finally update the pose 177 | # ******************* 178 | points = [p0, p1, p2, p3] 179 | might_print_points(points, leg_name) 180 | 181 | # Convert points from local leg coordinate frame to world coordinate frame 182 | for point in points: 183 | point.update_point_wrt(twist_frame) 184 | if ASSERTION_ENABLED: 185 | assert hexapod.body_rotation_frame is not None, "No rotation frame!" 186 | point.update_point_wrt(hexapod.body_rotation_frame) 187 | point.move_xyz(body_contact.x, body_contact.y, body_contact.z) 188 | 189 | might_sanity_leg_lengths_check(hexapod, leg_name, points) 190 | might_sanity_beta_gamma_check(beta, gamma, leg_name, points) 191 | 192 | # Update hexapod's points to what we computed 193 | update_hexapod_points(hexapod, i, points) 194 | 195 | poses[i]["coxia"] = alpha 196 | poses[i]["femur"] = beta 197 | poses[i]["tibia"] = gamma 198 | 199 | might_print_ik(poses, ik_parameters, hexapod) 200 | return poses, hexapod 201 | -------------------------------------------------------------------------------- /hexapod/templates/figure_template.py: -------------------------------------------------------------------------------- 1 | from style_settings import ( 2 | BODY_MESH_COLOR, 3 | BODY_MESH_OPACITY, 4 | BODY_COLOR, 5 | BODY_OUTLINE_WIDTH, 6 | COG_COLOR, 7 | COG_SIZE, 8 | HEAD_SIZE, 9 | LEG_COLOR, 10 | LEG_OUTLINE_WIDTH, 11 | SUPPORT_POLYGON_MESH_COLOR, 12 | SUPPORT_POLYGON_MESH_OPACITY, 13 | LEGENDS_BG_COLOR, 14 | AXIS_ZERO_LINE_COLOR, 15 | PAPER_BG_COLOR, 16 | GROUND_COLOR, 17 | LEGEND_FONT_COLOR, 18 | ) 19 | 20 | data = [ 21 | { 22 | "name": "body mesh", 23 | "showlegend": True, 24 | "type": "mesh3d", 25 | "opacity": BODY_MESH_OPACITY, 26 | "color": BODY_MESH_COLOR, 27 | "uid": "1f821e07-2c02-4a64-8ce3-61ecfe2a91b6", 28 | "x": [100.0, 100.0, -100.0, -100.0, -100.0, 100.0, 100.0], 29 | "y": [0.0, 100.0, 100.0, 0.0, -100.0, -100.0, 0.0], 30 | "z": [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0], 31 | }, 32 | { 33 | "line": {"color": BODY_COLOR, "opacity": 1.0, "width": BODY_OUTLINE_WIDTH}, 34 | "name": "body", 35 | "showlegend": True, 36 | "type": "scatter3d", 37 | "uid": "1f821e07-2c02-4a64-8ce3-61ecfe2a91b6", 38 | "x": [100.0, 100.0, -100.0, -100.0, -100.0, 100.0, 100.0], 39 | "y": [0.0, 100.0, 100.0, 0.0, -100.0, -100.0, 0.0], 40 | "z": [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0], 41 | }, 42 | { 43 | "marker": {"color": COG_COLOR, "opacity": 1, "size": COG_SIZE}, 44 | "mode": "markers", 45 | "name": "cog", 46 | "type": "scatter3d", 47 | "uid": "a819d0e4-ddaa-476b-b3e4-48fd766e749c", 48 | "x": [0.0], 49 | "y": [0.0], 50 | "z": [100.0], 51 | }, 52 | { 53 | "marker": {"color": BODY_COLOR, "opacity": 1.0, "size": HEAD_SIZE}, 54 | "mode": "markers", 55 | "name": "head", 56 | "type": "scatter3d", 57 | "uid": "508caa99-c538-4cb6-b022-fbbb31c2350b", 58 | "x": [0.0], 59 | "y": [100.0], 60 | "z": [100.0], 61 | }, 62 | { 63 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 64 | "name": "leg 1", 65 | "showlegend": False, 66 | "type": "scatter3d", 67 | "uid": "f217db57-fe6e-4b40-90f8-4e1c20ef595e", 68 | "x": [100.0, 200.0, 300.0, 300.0], 69 | "y": [0.0, 0.0, 0.0, 0.0], 70 | "z": [100.0, 100.0, 100.0, 0.0], 71 | }, 72 | { 73 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 74 | "name": "leg 2", 75 | "showlegend": False, 76 | "type": "scatter3d", 77 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 78 | "x": [100.0, 170.71067811865476, 241.4213562373095, 241.4213562373095], 79 | "y": [100.0, 170.71067811865476, 241.42135623730948, 241.42135623730948], 80 | "z": [100.0, 100.0, 100.0, 0.0], 81 | }, 82 | { 83 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 84 | "name": "leg 3", 85 | "showlegend": False, 86 | "type": "scatter3d", 87 | "uid": "9f13f416-f2b7-4eb7-993c-1e26e2e7a908", 88 | "x": [-100.0, -170.71067811865476, -241.42135623730948, -241.42135623730948], 89 | "y": [100.0, 170.71067811865476, 241.4213562373095, 241.4213562373095], 90 | "z": [100.0, 100.0, 100.0, 0.0], 91 | }, 92 | { 93 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 94 | "name": "leg 4", 95 | "showlegend": False, 96 | "type": "scatter3d", 97 | "uid": "0d426c49-19a4-4051-b938-81b30c962dff", 98 | "x": [-100.0, -200.0, -300.0, -300.0], 99 | "y": [ 100 | 0.0, 101 | 1.2246467991473532e-14, 102 | 2.4492935982947064e-14, 103 | 2.4492935982947064e-14, 104 | ], 105 | "z": [100.0, 100.0, 100.0, 0.0], 106 | }, 107 | { 108 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 109 | "name": "leg 5", 110 | "showlegend": False, 111 | "type": "scatter3d", 112 | "uid": "5ba25594-2fb5-407e-a16f-118f12769e28", 113 | "x": [-100.0, -170.71067811865476, -241.42135623730954, -241.42135623730954], 114 | "y": [-100.0, -170.71067811865476, -241.42135623730948, -241.42135623730948], 115 | "z": [100.0, 100.0, 100.0, 0.0], 116 | }, 117 | { 118 | "line": {"color": LEG_COLOR, "width": LEG_OUTLINE_WIDTH}, 119 | "name": "leg 6", 120 | "showlegend": False, 121 | "type": "scatter3d", 122 | "uid": "fa4b5f98-7d68-4eb9-bd38-a6f8dabef8a4", 123 | "x": [100.0, 170.71067811865476, 241.42135623730948, 241.42135623730948], 124 | "y": [-100.0, -170.71067811865476, -241.42135623730954, -241.42135623730954], 125 | "z": [100.0, 100.0, 100.0, 0.0], 126 | }, 127 | { 128 | "name": "support polygon mesh", 129 | "showlegend": True, 130 | "type": "mesh3d", 131 | "opacity": SUPPORT_POLYGON_MESH_OPACITY, 132 | "color": SUPPORT_POLYGON_MESH_COLOR, 133 | "uid": "1f821e07-2c02-4a64-8ce3-61ecfe2a91b6", 134 | "x": [ 135 | 300.0, 136 | 241.4213562373095, 137 | -241.42135623730948, 138 | -300.0, 139 | -241.42135623730954, 140 | 241.42135623730948, 141 | ], 142 | "y": [ 143 | 0.0, 144 | 241.42135623730948, 145 | 241.4213562373095, 146 | 2.4492935982947064e-14, 147 | -241.42135623730948, 148 | -241.42135623730954, 149 | ], 150 | "z": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 151 | }, 152 | { 153 | "line": {"color": "#2f3640", "width": 2}, 154 | "name": "hexapod x", 155 | "mode": "lines", 156 | "showlegend": False, 157 | "opacity": 1.0, 158 | "type": "scatter3d", 159 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 160 | "x": [0.0, 50.0], 161 | "y": [0.0, 0.0], 162 | "z": [100.0, 100.0], 163 | }, 164 | { 165 | "line": {"color": "#e67e22", "width": 2}, 166 | "name": "hexapod y", 167 | "mode": "lines", 168 | "showlegend": False, 169 | "opacity": 1.0, 170 | "type": "scatter3d", 171 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 172 | "x": [0.0, 0.0], 173 | "y": [0.0, 50.0], 174 | "z": [100.0, 100.0], 175 | }, 176 | { 177 | "line": {"color": "#0097e6", "width": 2}, 178 | "name": "hexapod z", 179 | "mode": "lines", 180 | "showlegend": False, 181 | "opacity": 1.0, 182 | "type": "scatter3d", 183 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 184 | "x": [0.0, 0.0], 185 | "y": [0.0, 0.0], 186 | "z": [100.0, 150.0], 187 | }, 188 | { 189 | "line": {"color": "#2f3640", "width": 2}, 190 | "name": "x direction", 191 | "showlegend": False, 192 | "mode": "lines", 193 | "opacity": 1.0, 194 | "type": "scatter3d", 195 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 196 | "x": [0, 50], 197 | "y": [0, 0], 198 | "z": [0, 0], 199 | }, 200 | { 201 | "line": {"color": "#e67e22", "width": 2}, 202 | "name": "y direction", 203 | "showlegend": False, 204 | "mode": "lines", 205 | "opacity": 1.0, 206 | "type": "scatter3d", 207 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 208 | "x": [0, 0], 209 | "y": [0, 50], 210 | "z": [0, 0], 211 | }, 212 | { 213 | "line": {"color": "#0097e6", "width": 2}, 214 | "name": "z direction", 215 | "showlegend": False, 216 | "mode": "lines", 217 | "opacity": 1.0, 218 | "type": "scatter3d", 219 | "uid": "d5690122-cd54-460d-ab3e-1f910eb88f0f", 220 | "x": [0, 0], 221 | "y": [0, 0], 222 | "z": [0, 50], 223 | }, 224 | ] 225 | 226 | HEXAPOD_FIGURE = { 227 | "data": data, 228 | "layout": { 229 | "paper_bgcolor": PAPER_BG_COLOR, 230 | "hovermode": "closest", 231 | "legend": { 232 | "x": 0, 233 | "y": 0, 234 | "bgcolor": LEGENDS_BG_COLOR, 235 | "font": {"family": "courier", "size": 12, "color": LEGEND_FONT_COLOR}, 236 | }, 237 | "margin": {"b": 20, "l": 10, "r": 10, "t": 20}, 238 | "scene": { 239 | "aspectmode": "manual", 240 | "aspectratio": {"x": 1, "y": 1, "z": 1}, 241 | "camera": { 242 | "center": { 243 | "x": 0.0348603742736399, 244 | "y": 0.16963779995083, 245 | "z": -0.394903376555686, 246 | }, 247 | "eye": { 248 | "x": 0.193913968006015, 249 | "y": 0.45997575676993, 250 | "z": -0.111568465000231, 251 | }, 252 | "up": {"x": 0, "y": 0, "z": 1}, 253 | }, 254 | "xaxis": { 255 | "nticks": 1, 256 | "range": [-600, 600], 257 | "zerolinecolor": AXIS_ZERO_LINE_COLOR, 258 | "showbackground": False, 259 | }, 260 | "yaxis": { 261 | "nticks": 1, 262 | "range": [-600, 600], 263 | "zerolinecolor": AXIS_ZERO_LINE_COLOR, 264 | "showbackground": False, 265 | }, 266 | "zaxis": { 267 | "nticks": 1, 268 | "range": [-600, -10], 269 | "zerolinecolor": AXIS_ZERO_LINE_COLOR, 270 | "showbackground": True, 271 | "backgroundcolor": GROUND_COLOR, 272 | }, 273 | }, 274 | }, 275 | } 276 | -------------------------------------------------------------------------------- /hexapod/ik_solver/ik_solver2.py: -------------------------------------------------------------------------------- 1 | # Please look at the discussion of the Inverse Kinematics algorithm 2 | # As detailed in the README of this directory 3 | from copy import deepcopy 4 | import numpy as np 5 | from settings import ASSERTION_ENABLED, ALPHA_MAX_ANGLE 6 | from hexapod.ik_solver.helpers import ( 7 | BODY_ON_GROUND_ALERT_MSG, 8 | COXIA_ON_GROUND_ALERT_MSG, 9 | cant_reach_alert_msg, 10 | body_contact_shoved_on_ground, 11 | legs_too_short, 12 | beta_gamma_not_in_range, 13 | angle_above_limit, 14 | might_sanity_leg_lengths_check, 15 | might_sanity_beta_gamma_check, 16 | might_print_ik, 17 | might_print_points, 18 | ) 19 | from hexapod.points import ( 20 | Vector, 21 | length, 22 | add_vectors, 23 | scalar_multiply, 24 | vector_from_to, 25 | get_unit_vector, 26 | is_triangle, 27 | project_vector_onto_plane, 28 | angle_between, 29 | angle_opposite_of_last_side, 30 | ) 31 | from hexapod.ik_solver.shared import ( 32 | update_hexapod_points, 33 | find_twist_frame, 34 | compute_twist_wrt_to_world, 35 | ) 36 | from hexapod.const import HEXAPOD_POSE 37 | 38 | # This function inverse_kinematics_update() 39 | # computes the joint angles required to 40 | # rotate and translate the hexapod given the parameters 41 | # - rot_x, rot_y, rot_z are how the hexapod should be rotated 42 | # - percent_x, percent_y, percent_z are the shifts of the 43 | # center of gravity of the hexapod 44 | # 45 | # The final points of contact by the hexapod and the ground 46 | # are the same as the ground contacts right 47 | # after doing the leg stance and hip stance 48 | # unless the leg can't reach it. 49 | # 50 | # ❗❗❗IMPORTANT: The hexapod will be MODIFIED and returned along with 51 | # a dictionary of POSES containing the 18 computed angles 52 | # if the pose is impossible, this function will raise an error 53 | 54 | 55 | def inverse_kinematics_update(hexapod, ik_parameters): 56 | return IKSolver(hexapod, ik_parameters).pose_and_hexapod() 57 | 58 | 59 | class IKSolver: 60 | __slots__ = [ 61 | "hexapod", 62 | "params", 63 | "poses", 64 | "leg_x_axis", 65 | "p0", 66 | "p1", 67 | "p2", 68 | "p3", 69 | "points", 70 | "body_to_foot_vector", 71 | "coxia_vector", 72 | "unit_coxia_vector", 73 | "coxia_to_foot_vector2d", 74 | "d", 75 | "alpha", 76 | "beta", 77 | "gamma", 78 | "leg_name", 79 | "legs_up_in_the_air", 80 | "body_contact", 81 | "foot_tip", 82 | "twist_frame", 83 | ] 84 | 85 | def __init__(self, hexapod, ik_parameters): 86 | self.hexapod = hexapod 87 | self.params = ik_parameters 88 | self.leg_x_axis = Vector(1, 0, 0) 89 | self.update_body_and_ground_contact_points() 90 | self.poses = deepcopy(HEXAPOD_POSE) 91 | 92 | self.legs_up_in_the_air = [] 93 | for i in range(hexapod.LEG_COUNT): 94 | self.store_known_points(i) 95 | self.store_initial_vectors() 96 | self.compute_local_p0_p1_p3() 97 | self.compute_beta_gamma_local_p2() 98 | self.compute_alpha_and_twist_frame(i) 99 | self.points = [self.p0, self.p1, self.p2, self.p3] 100 | # Update point wrt world frame 101 | self.update_to_global_points() 102 | # Update hexapod's points to what we computed 103 | update_hexapod_points(self.hexapod, i, self.points) 104 | # Final p1, p2, p3, beta and gamma computed at this point 105 | self.poses[i]["coxia"] = self.alpha 106 | self.poses[i]["femur"] = self.beta 107 | self.poses[i]["tibia"] = self.gamma 108 | 109 | might_print_ik(self.poses, self.params, self.hexapod) 110 | 111 | def pose_and_hexapod(self): 112 | return self.poses, self.hexapod 113 | 114 | def update_body_and_ground_contact_points(self): 115 | tx = self.params["percent_x"] * self.hexapod.mid 116 | ty = self.params["percent_y"] * self.hexapod.side 117 | tz = self.params["percent_z"] * self.hexapod.tibia 118 | rotx, roty, rotz = ( 119 | self.params["rot_x"], 120 | self.params["rot_y"], 121 | self.params["rot_z"], 122 | ) 123 | 124 | self.hexapod.update_stance(self.params["hip_stance"], self.params["leg_stance"]) 125 | self.hexapod.detach_body_rotate_and_translate(rotx, roty, rotz, tx, ty, tz) 126 | 127 | if body_contact_shoved_on_ground(self.hexapod): 128 | raise Exception(BODY_ON_GROUND_ALERT_MSG) 129 | 130 | def store_known_points(self, i): 131 | self.leg_name = self.hexapod.legs[i].name 132 | self.body_contact = self.hexapod.body.vertices[i] 133 | self.foot_tip = self.hexapod.legs[i].foot_tip() 134 | 135 | def store_initial_vectors(self): 136 | self.body_to_foot_vector = vector_from_to(self.body_contact, self.foot_tip) 137 | 138 | # find the coxia vector which is the vector 139 | # from body contact point to joint between coxia and femur limb 140 | projection = project_vector_onto_plane( 141 | self.body_to_foot_vector, self.hexapod.z_axis 142 | ) 143 | self.unit_coxia_vector = get_unit_vector(projection) 144 | self.coxia_vector = scalar_multiply(self.unit_coxia_vector, self.hexapod.coxia) 145 | 146 | # coxia point / joint is the point connecting the coxia and tibia limbs 147 | coxia_point = add_vectors(self.body_contact, self.coxia_vector) 148 | if coxia_point.z < self.foot_tip.z: 149 | raise Exception(COXIA_ON_GROUND_ALERT_MSG) 150 | 151 | def compute_local_p0_p1_p3(self): 152 | self.p0 = Vector(0, 0, 0) 153 | self.p1 = Vector(self.hexapod.coxia, 0, 0) 154 | 155 | # Find p3 aka foot tip (ground contact) with respect to the local leg frame 156 | rho = angle_between(self.unit_coxia_vector, self.body_to_foot_vector) 157 | body_to_foot_length = length(self.body_to_foot_vector) 158 | p3x = body_to_foot_length * np.cos(np.radians(rho)) 159 | p3z = -body_to_foot_length * np.sin(np.radians(rho)) 160 | self.p3 = Vector(p3x, 0, p3z) 161 | 162 | def compute_beta_gamma_local_p2(self): 163 | # These values are needed to compute 164 | # p2 aka tibia joint (point between femur limb and tibia limb) 165 | self.coxia_to_foot_vector2d = vector_from_to(self.p1, self.p3) 166 | self.d = length(self.coxia_to_foot_vector2d) 167 | 168 | # If we can form this triangle 169 | # # this means we probably can reach the target ground contact point 170 | if is_triangle(self.hexapod.tibia, self.hexapod.femur, self.d): 171 | # CASE A: a triangle can be formed with 172 | # coxia to foot vector, hexapod's femur and tibia 173 | self.compute_when_triangle_can_form() 174 | else: 175 | # CASE B: It's impossible to reach target ground point 176 | self.compute_when_triangle_cannot_form() 177 | 178 | not_within_range, alert_msg = beta_gamma_not_in_range( 179 | self.beta, self.gamma, self.leg_name 180 | ) 181 | if not_within_range: 182 | raise Exception(alert_msg) 183 | 184 | def compute_when_triangle_can_form(self): 185 | theta = angle_opposite_of_last_side( 186 | self.d, self.hexapod.femur, self.hexapod.tibia 187 | ) 188 | phi = angle_between(self.coxia_to_foot_vector2d, self.leg_x_axis) 189 | 190 | self.beta = theta - phi # case 1 or 2 191 | if self.p3.z > 0: # case 3 192 | self.beta = theta + phi 193 | 194 | z_ = self.hexapod.femur * np.sin(np.radians(self.beta)) 195 | x_ = self.p1.x + self.hexapod.femur * np.cos(np.radians(self.beta)) 196 | 197 | self.p2 = Vector(x_, 0, z_) 198 | femur_vector = vector_from_to(self.p1, self.p2) 199 | tibia_vector = vector_from_to(self.p2, self.p3) 200 | self.gamma = 90 - angle_between(femur_vector, tibia_vector) 201 | 202 | if self.p2.z < self.p3.z: 203 | raise Exception(cant_reach_alert_msg(self.leg_name, "blocking")) 204 | 205 | def might_raise_cant_reach_target(self): 206 | if self.d + self.hexapod.tibia < self.hexapod.femur: 207 | raise Exception(cant_reach_alert_msg(self.leg_name, "femur")) 208 | if self.d + self.hexapod.femur < self.hexapod.tibia: 209 | raise Exception(cant_reach_alert_msg(self.leg_name, "tibia")) 210 | 211 | # Then hexapod.femur + hexapod.tibia < d: 212 | self.legs_up_in_the_air.append(self.leg_name) 213 | LEGS_TOO_SHORT, alert_msg = legs_too_short(self.legs_up_in_the_air) 214 | if LEGS_TOO_SHORT: 215 | raise Exception(alert_msg) 216 | 217 | def only_few_legs_cant_reach_target(self): 218 | # Try to reach it by making the legs stretch 219 | # i.e. p1, p2, p3 are all on the same line 220 | femur_tibia_direction = get_unit_vector(self.coxia_to_foot_vector2d) 221 | femur_vector = scalar_multiply(femur_tibia_direction, self.hexapod.femur) 222 | self.p2 = add_vectors(self.p1, femur_vector) 223 | tibia_vector = scalar_multiply(femur_tibia_direction, self.hexapod.tibia) 224 | self.p3 = add_vectors(self.p2, tibia_vector) 225 | 226 | # Find beta and gamma 227 | self.gamma = 0.0 228 | self.beta = angle_between(self.leg_x_axis, femur_vector) 229 | if femur_vector.z < 0: 230 | self.beta = -self.beta 231 | 232 | def compute_when_triangle_cannot_form(self): 233 | self.might_raise_cant_reach_target() 234 | self.only_few_legs_cant_reach_target() 235 | 236 | def compute_alpha_and_twist_frame(self, i): 237 | 238 | alpha, twist_frame = find_twist_frame(self.hexapod, self.unit_coxia_vector) 239 | alpha = compute_twist_wrt_to_world(alpha, self.hexapod.body.COXIA_AXES[i]) 240 | 241 | limit, msg = angle_above_limit( 242 | alpha, ALPHA_MAX_ANGLE, self.leg_name, "(alpha/coxia)" 243 | ) 244 | if limit: 245 | raise Exception(msg) 246 | 247 | self.alpha = alpha 248 | self.twist_frame = twist_frame 249 | 250 | def update_to_global_points(self): 251 | might_print_points(self.points, self.leg_name) 252 | 253 | # Convert points from local leg coordinate frame to world coordinate frame 254 | for point in self.points: 255 | point.update_point_wrt(self.twist_frame) 256 | if ASSERTION_ENABLED: 257 | assert ( 258 | self.hexapod.body_rotation_frame is not None 259 | ), "No rotation frame!" 260 | point.update_point_wrt(self.hexapod.body_rotation_frame) 261 | point.move_xyz( 262 | self.body_contact.x, self.body_contact.y, self.body_contact.z 263 | ) 264 | 265 | might_sanity_leg_lengths_check(self.hexapod, self.leg_name, self.points) 266 | might_sanity_beta_gamma_check(self.beta, self.gamma, self.leg_name, self.points) 267 | -------------------------------------------------------------------------------- /external_css/dark.css: -------------------------------------------------------------------------------- 1 | .hljs { 2 | background: #222f3e; 3 | color: #32ff7e; 4 | } 5 | 6 | @import url('https://fonts.googleapis.com/css2?family=Libre+Franklin:wght@300&display=swap'); 7 | 8 | /* Links 9 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 10 | a { 11 | color: #ef5777; 12 | text-decoration: none; 13 | cursor: pointer;} 14 | a:hover { 15 | color: #ffa801; } 16 | 17 | /* Base Styles 18 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 19 | /* NOTE 20 | html is set to 62.5% so that all the REM measurements throughout Skeleton 21 | are based on 10px sizing. So basically 1.5rem = 15px :) */ 22 | html { 23 | font-size: 62.5%; } 24 | body { 25 | font-size: 1.5em; /* currently ems cause chrome bug misinterpreting rems on body element */ 26 | line-height: 1.6; 27 | font-weight: 400; 28 | font-family: "Libre Franklin", "HelveticaNeue", "Helvetica Neue", Helvetica, Arial, sans-serif !important; 29 | color: #32ff7e; 30 | background: #222f3e; 31 | } 32 | 33 | /* Table of contents 34 | –––––––––––––––––––––––––––––––––––––––––––––––––– 35 | - Base Styles 36 | - Links 37 | - Plotly.js 38 | - Grid 39 | - Typography 40 | - Buttons 41 | - Forms 42 | - Lists 43 | - Code 44 | - Tables 45 | - Spacing 46 | - Utilities 47 | - Clearing 48 | - Media Queries 49 | */ 50 | 51 | /* PLotly.js 52 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 53 | /* plotly.js's modebar's z-index is 1001 by default 54 | * https://github.com/plotly/plotly.js/blob/7e4d8ab164258f6bd48be56589dacd9bdd7fded2/src/css/_modebar.scss#L5 55 | * In case a dropdown is above the graph, the dropdown's options 56 | * will be rendered below the modebar 57 | * Increase the select option's z-index 58 | */ 59 | 60 | /* This was actually not quite right - 61 | dropdowns were overlapping each other (edited October 26) 62 | 63 | .Select { 64 | z-index: 1002; 65 | }*/ 66 | 67 | 68 | /* Grid 69 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 70 | .container { 71 | position: relative; 72 | width: 100%; 73 | max-width: 960px; 74 | margin: 0 auto; 75 | padding: 0 20px; 76 | box-sizing: border-box; } 77 | .column, 78 | .columns { 79 | width: 100%; 80 | float: left; 81 | box-sizing: border-box; } 82 | 83 | /* For devices larger than 400px */ 84 | @media (min-width: 400px) { 85 | .container { 86 | width: 85%; 87 | padding: 0; } 88 | } 89 | 90 | /* For devices larger than 550px */ 91 | @media (min-width: 550px) { 92 | .container { 93 | width: 80%; } 94 | .column, 95 | .columns { 96 | margin-left: 4%; } 97 | .column:first-child, 98 | .columns:first-child { 99 | margin-left: 0; } 100 | 101 | .one.column, 102 | .one.columns { width: 4.66666666667%; } 103 | .two.columns { width: 13.3333333333%; } 104 | .three.columns { width: 22%; } 105 | .four.columns { width: 30.6666666667%; } 106 | .five.columns { width: 39.3333333333%; } 107 | .six.columns { width: 48%; } 108 | .seven.columns { width: 56.6666666667%; } 109 | .eight.columns { width: 65.3333333333%; } 110 | .nine.columns { width: 74.0%; } 111 | .ten.columns { width: 82.6666666667%; } 112 | .eleven.columns { width: 91.3333333333%; } 113 | .twelve.columns { width: 100%; margin-left: 0; } 114 | 115 | .one-third.column { width: 30.6666666667%; } 116 | .two-thirds.column { width: 65.3333333333%; } 117 | 118 | .one-half.column { width: 48%; } 119 | 120 | /* Offsets */ 121 | .offset-by-one.column, 122 | .offset-by-one.columns { margin-left: 8.66666666667%; } 123 | .offset-by-two.column, 124 | .offset-by-two.columns { margin-left: 17.3333333333%; } 125 | .offset-by-three.column, 126 | .offset-by-three.columns { margin-left: 26%; } 127 | .offset-by-four.column, 128 | .offset-by-four.columns { margin-left: 34.6666666667%; } 129 | .offset-by-five.column, 130 | .offset-by-five.columns { margin-left: 43.3333333333%; } 131 | .offset-by-six.column, 132 | .offset-by-six.columns { margin-left: 52%; } 133 | .offset-by-seven.column, 134 | .offset-by-seven.columns { margin-left: 60.6666666667%; } 135 | .offset-by-eight.column, 136 | .offset-by-eight.columns { margin-left: 69.3333333333%; } 137 | .offset-by-nine.column, 138 | .offset-by-nine.columns { margin-left: 78.0%; } 139 | .offset-by-ten.column, 140 | .offset-by-ten.columns { margin-left: 86.6666666667%; } 141 | .offset-by-eleven.column, 142 | .offset-by-eleven.columns { margin-left: 95.3333333333%; } 143 | 144 | .offset-by-one-third.column, 145 | .offset-by-one-third.columns { margin-left: 34.6666666667%; } 146 | .offset-by-two-thirds.column, 147 | .offset-by-two-thirds.columns { margin-left: 69.3333333333%; } 148 | 149 | .offset-by-one-half.column, 150 | .offset-by-one-half.columns { margin-left: 52%; } 151 | 152 | } 153 | 154 | /* Typography 155 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 156 | h1, h2, h3, h4, h5, h6 { 157 | margin-top: 0; 158 | margin-bottom: 0; 159 | font-weight: 300; } 160 | h1 { font-size: 4.5rem; line-height: 1.2; letter-spacing: -.1rem; margin-bottom: 2rem; } 161 | h2 { font-size: 3.6rem; line-height: 1.25; letter-spacing: -.1rem; margin-bottom: 1.8rem; margin-top: 1.8rem;} 162 | h3 { font-size: 3.0rem; line-height: 1.3; letter-spacing: -.1rem; margin-bottom: 1.5rem; margin-top: 1.5rem;} 163 | h4 { font-size: 2.6rem; line-height: 1.35; letter-spacing: -.08rem; margin-bottom: 1.2rem; margin-top: 1.2rem;} 164 | h5 { font-size: 2.2rem; line-height: 1.5; letter-spacing: -.05rem; margin-bottom: 0.6rem; margin-top: 0.6rem;} 165 | h6 { font-size: 2.0rem; line-height: 1.6; letter-spacing: 0; margin-bottom: 0.75rem; margin-top: 0.75rem;} 166 | 167 | p { 168 | margin-top: 0; } 169 | 170 | 171 | /* Blockquotes 172 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 173 | blockquote { 174 | border-left: 4px lightgrey solid; 175 | padding-left: 1rem; 176 | margin-top: 2rem; 177 | margin-bottom: 2rem; 178 | margin-left: 0rem; 179 | } 180 | 181 | 182 | 183 | 184 | 185 | /* Buttons 186 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 187 | .button, 188 | button, 189 | input[type="submit"], 190 | input[type="reset"], 191 | input[type="button"] { 192 | display: inline-block; 193 | height: 38px; 194 | padding: 0 30px; 195 | color: #555; 196 | text-align: center; 197 | font-size: 11px; 198 | font-weight: 600; 199 | line-height: 38px; 200 | letter-spacing: .1rem; 201 | text-transform: uppercase; 202 | text-decoration: none; 203 | white-space: nowrap; 204 | background-color: transparent; 205 | border-radius: 4px; 206 | border: 1px solid #bbb; 207 | cursor: pointer; 208 | box-sizing: border-box; } 209 | .button:hover, 210 | button:hover, 211 | input[type="submit"]:hover, 212 | input[type="reset"]:hover, 213 | input[type="button"]:hover, 214 | .button:focus, 215 | button:focus, 216 | input[type="submit"]:focus, 217 | input[type="reset"]:focus, 218 | input[type="button"]:focus { 219 | color: #333; 220 | border-color: #888; 221 | outline: 0; } 222 | .button.button-primary, 223 | button.button-primary, 224 | input[type="submit"].button-primary, 225 | input[type="reset"].button-primary, 226 | input[type="button"].button-primary { 227 | color: #FFF; 228 | background-color: #33C3F0; 229 | border-color: #33C3F0; } 230 | .button.button-primary:hover, 231 | button.button-primary:hover, 232 | input[type="submit"].button-primary:hover, 233 | input[type="reset"].button-primary:hover, 234 | input[type="button"].button-primary:hover, 235 | .button.button-primary:focus, 236 | button.button-primary:focus, 237 | input[type="submit"].button-primary:focus, 238 | input[type="reset"].button-primary:focus, 239 | input[type="button"].button-primary:focus { 240 | color: #FFF; 241 | background-color: #1EAEDB; 242 | border-color: #1EAEDB; } 243 | 244 | 245 | /* Forms 246 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 247 | input[type="email"], 248 | input[type="number"], 249 | input[type="search"], 250 | input[type="text"], 251 | input[type="tel"], 252 | input[type="url"], 253 | input[type="password"], 254 | textarea, 255 | select { 256 | height: 38px; 257 | padding: 6px 10px; /* The 6px vertically centers text on FF, ignored by Webkit */ 258 | background-color: #fff; 259 | border: 1px solid #D1D1D1; 260 | border-radius: 4px; 261 | box-shadow: none; 262 | box-sizing: border-box; 263 | font-family: inherit; 264 | font-size: inherit; /*https://stackoverflow.com/questions/6080413/why-doesnt-input-inherit-the-font-from-body*/} 265 | /* Removes awkward default styles on some inputs for iOS */ 266 | input[type="email"], 267 | input[type="number"], 268 | input[type="search"], 269 | input[type="text"], 270 | input[type="tel"], 271 | input[type="url"], 272 | input[type="password"], 273 | textarea { 274 | -webkit-appearance: none; 275 | -moz-appearance: none; 276 | appearance: none; } 277 | textarea { 278 | min-height: 65px; 279 | padding-top: 6px; 280 | padding-bottom: 6px; } 281 | input[type="email"]:focus, 282 | input[type="number"]:focus, 283 | input[type="search"]:focus, 284 | input[type="text"]:focus, 285 | input[type="tel"]:focus, 286 | input[type="url"]:focus, 287 | input[type="password"]:focus, 288 | textarea:focus, 289 | select:focus { 290 | border: 1px solid #33C3F0; 291 | outline: 0; } 292 | label, 293 | legend { 294 | display: block; 295 | margin-bottom: 0px; } 296 | fieldset { 297 | padding: 0; 298 | border-width: 0; } 299 | input[type="checkbox"], 300 | input[type="radio"] { 301 | display: inline; } 302 | label > .label-body { 303 | display: inline-block; 304 | margin-left: .5rem; 305 | font-weight: normal; } 306 | 307 | 308 | /* Lists 309 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 310 | ul { 311 | list-style: circle inside; } 312 | ol { 313 | list-style: decimal inside; } 314 | ol, ul { 315 | padding-left: 0; 316 | margin-top: 0; } 317 | ul ul, 318 | ul ol, 319 | ol ol, 320 | ol ul { 321 | margin: 1.5rem 0 1.5rem 3rem; 322 | font-size: 90%; } 323 | li { 324 | margin-bottom: 1rem; } 325 | 326 | 327 | /* Tables 328 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 329 | table { 330 | border-collapse: collapse; 331 | } 332 | th:not(.CalendarDay), 333 | td:not(.CalendarDay) { 334 | padding: 12px 15px; 335 | text-align: left; 336 | border-bottom: 1px solid #E1E1E1; } 337 | th:first-child:not(.CalendarDay), 338 | td:first-child:not(.CalendarDay) { 339 | padding-left: 0; } 340 | th:last-child:not(.CalendarDay), 341 | td:last-child:not(.CalendarDay) { 342 | padding-right: 0; } 343 | 344 | 345 | /* Spacing 346 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 347 | button, 348 | .button { 349 | margin-bottom: 0rem; } 350 | input, 351 | textarea, 352 | select, 353 | fieldset { 354 | margin-bottom: 0rem; } 355 | pre, 356 | dl, 357 | figure, 358 | table, 359 | form { 360 | margin-bottom: 0rem; } 361 | p, 362 | ul, 363 | ol { 364 | margin-bottom: 0.75rem; } 365 | 366 | /* Utilities 367 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 368 | .u-full-width { 369 | width: 100%; 370 | box-sizing: border-box; } 371 | .u-max-full-width { 372 | max-width: 100%; 373 | box-sizing: border-box; } 374 | .u-pull-right { 375 | float: right; } 376 | .u-pull-left { 377 | float: left; } 378 | 379 | 380 | /* Misc 381 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 382 | hr { 383 | margin-top: 3rem; 384 | margin-bottom: 3.5rem; 385 | border-width: 0; 386 | border-top: 1px solid #E1E1E1; } 387 | 388 | 389 | /* Clearing 390 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 391 | 392 | /* Self Clearing Goodness */ 393 | .container:after, 394 | .row:after, 395 | .u-cf { 396 | content: ""; 397 | display: table; 398 | clear: both; } 399 | 400 | 401 | /* Media Queries 402 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 403 | /* 404 | Note: The best way to structure the use of media queries is to create the queries 405 | near the relevant code. For example, if you wanted to change the styles for buttons 406 | on small devices, paste the mobile query code up in the buttons section and style it 407 | there. 408 | */ 409 | 410 | 411 | /* Larger than mobile */ 412 | @media (min-width: 400px) {} 413 | 414 | /* Larger than phablet (also point when grid becomes active) */ 415 | @media (min-width: 550px) {} 416 | 417 | /* Larger than tablet */ 418 | @media (min-width: 750px) {} 419 | 420 | /* Larger than desktop */ 421 | @media (min-width: 1000px) {} 422 | 423 | /* Larger than Desktop HD */ 424 | @media (min-width: 1200px) {} -------------------------------------------------------------------------------- /external_css/light.css: -------------------------------------------------------------------------------- 1 | @import url('https://fonts.googleapis.com/css2?family=Libre+Franklin:wght@300&display=swap'); 2 | 3 | .hljs { 4 | background: #ffffff; 5 | color: #2c3e50; 6 | } 7 | 8 | /* Links 9 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 10 | a { 11 | color: #ef5777; 12 | text-decoration: none; 13 | cursor: pointer;} 14 | a:hover { 15 | color: #ffa801; } 16 | 17 | /* Base Styles 18 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 19 | /* NOTE 20 | html is set to 62.5% so that all the REM measurements throughout Skeleton 21 | are based on 10px sizing. So basically 1.5rem = 15px :) */ 22 | html { 23 | font-size: 62.5%; } 24 | body { 25 | font-size: 1.5em; /* currently ems cause chrome bug misinterpreting rems on body element */ 26 | line-height: 1.6; 27 | font-weight: 400; 28 | font-family: "Libre Franklin", "HelveticaNeue", "Helvetica Neue", Helvetica, Arial, sans-serif !important; 29 | color: rgb(50, 50, 50); 30 | background: #FFFFFF; 31 | } 32 | 33 | /* Table of contents 34 | –––––––––––––––––––––––––––––––––––––––––––––––––– 35 | - Base Styles 36 | - Links 37 | - Plotly.js 38 | - Grid 39 | - Typography 40 | - Buttons 41 | - Forms 42 | - Lists 43 | - Code 44 | - Tables 45 | - Spacing 46 | - Utilities 47 | - Clearing 48 | - Media Queries 49 | */ 50 | 51 | /* PLotly.js 52 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 53 | /* plotly.js's modebar's z-index is 1001 by default 54 | * https://github.com/plotly/plotly.js/blob/7e4d8ab164258f6bd48be56589dacd9bdd7fded2/src/css/_modebar.scss#L5 55 | * In case a dropdown is above the graph, the dropdown's options 56 | * will be rendered below the modebar 57 | * Increase the select option's z-index 58 | */ 59 | 60 | /* This was actually not quite right - 61 | dropdowns were overlapping each other (edited October 26) 62 | 63 | .Select { 64 | z-index: 1002; 65 | }*/ 66 | 67 | 68 | /* Grid 69 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 70 | .container { 71 | position: relative; 72 | width: 100%; 73 | max-width: 960px; 74 | margin: 0 auto; 75 | padding: 0 20px; 76 | box-sizing: border-box; } 77 | .column, 78 | .columns { 79 | width: 100%; 80 | float: left; 81 | box-sizing: border-box; } 82 | 83 | /* For devices larger than 400px */ 84 | @media (min-width: 400px) { 85 | .container { 86 | width: 85%; 87 | padding: 0; } 88 | } 89 | 90 | /* For devices larger than 550px */ 91 | @media (min-width: 550px) { 92 | .container { 93 | width: 80%; } 94 | .column, 95 | .columns { 96 | margin-left: 4%; } 97 | .column:first-child, 98 | .columns:first-child { 99 | margin-left: 0; } 100 | 101 | .one.column, 102 | .one.columns { width: 4.66666666667%; } 103 | .two.columns { width: 13.3333333333%; } 104 | .three.columns { width: 22%; } 105 | .four.columns { width: 30.6666666667%; } 106 | .five.columns { width: 39.3333333333%; } 107 | .six.columns { width: 48%; } 108 | .seven.columns { width: 56.6666666667%; } 109 | .eight.columns { width: 65.3333333333%; } 110 | .nine.columns { width: 74.0%; } 111 | .ten.columns { width: 82.6666666667%; } 112 | .eleven.columns { width: 91.3333333333%; } 113 | .twelve.columns { width: 100%; margin-left: 0; } 114 | 115 | .one-third.column { width: 30.6666666667%; } 116 | .two-thirds.column { width: 65.3333333333%; } 117 | 118 | .one-half.column { width: 48%; } 119 | 120 | /* Offsets */ 121 | .offset-by-one.column, 122 | .offset-by-one.columns { margin-left: 8.66666666667%; } 123 | .offset-by-two.column, 124 | .offset-by-two.columns { margin-left: 17.3333333333%; } 125 | .offset-by-three.column, 126 | .offset-by-three.columns { margin-left: 26%; } 127 | .offset-by-four.column, 128 | .offset-by-four.columns { margin-left: 34.6666666667%; } 129 | .offset-by-five.column, 130 | .offset-by-five.columns { margin-left: 43.3333333333%; } 131 | .offset-by-six.column, 132 | .offset-by-six.columns { margin-left: 52%; } 133 | .offset-by-seven.column, 134 | .offset-by-seven.columns { margin-left: 60.6666666667%; } 135 | .offset-by-eight.column, 136 | .offset-by-eight.columns { margin-left: 69.3333333333%; } 137 | .offset-by-nine.column, 138 | .offset-by-nine.columns { margin-left: 78.0%; } 139 | .offset-by-ten.column, 140 | .offset-by-ten.columns { margin-left: 86.6666666667%; } 141 | .offset-by-eleven.column, 142 | .offset-by-eleven.columns { margin-left: 95.3333333333%; } 143 | 144 | .offset-by-one-third.column, 145 | .offset-by-one-third.columns { margin-left: 34.6666666667%; } 146 | .offset-by-two-thirds.column, 147 | .offset-by-two-thirds.columns { margin-left: 69.3333333333%; } 148 | 149 | .offset-by-one-half.column, 150 | .offset-by-one-half.columns { margin-left: 52%; } 151 | 152 | } 153 | 154 | /* Typography 155 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 156 | h1, h2, h3, h4, h5, h6 { 157 | margin-top: 0; 158 | margin-bottom: 0; 159 | font-weight: 300; } 160 | h1 { font-size: 4.5rem; line-height: 1.2; letter-spacing: -.1rem; margin-bottom: 2rem; } 161 | h2 { font-size: 3.6rem; line-height: 1.25; letter-spacing: -.1rem; margin-bottom: 1.8rem; margin-top: 1.8rem;} 162 | h3 { font-size: 3.0rem; line-height: 1.3; letter-spacing: -.1rem; margin-bottom: 1.5rem; margin-top: 1.5rem;} 163 | h4 { font-size: 2.6rem; line-height: 1.35; letter-spacing: -.08rem; margin-bottom: 1.2rem; margin-top: 1.2rem;} 164 | h5 { font-size: 2.2rem; line-height: 1.5; letter-spacing: -.05rem; margin-bottom: 0.6rem; margin-top: 0.6rem;} 165 | h6 { font-size: 2.0rem; line-height: 1.6; letter-spacing: 0; margin-bottom: 0.75rem; margin-top: 0.75rem;} 166 | 167 | p { 168 | margin-top: 0; } 169 | 170 | 171 | /* Blockquotes 172 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 173 | blockquote { 174 | border-left: 4px lightgrey solid; 175 | padding-left: 1rem; 176 | margin-top: 2rem; 177 | margin-bottom: 2rem; 178 | margin-left: 0rem; 179 | } 180 | 181 | 182 | 183 | 184 | 185 | /* Buttons 186 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 187 | .button, 188 | button, 189 | input[type="submit"], 190 | input[type="reset"], 191 | input[type="button"] { 192 | display: inline-block; 193 | height: 38px; 194 | padding: 0 30px; 195 | color: #555; 196 | text-align: center; 197 | font-size: 11px; 198 | font-weight: 600; 199 | line-height: 38px; 200 | letter-spacing: .1rem; 201 | text-transform: uppercase; 202 | text-decoration: none; 203 | white-space: nowrap; 204 | background-color: transparent; 205 | border-radius: 4px; 206 | border: 1px solid #bbb; 207 | cursor: pointer; 208 | box-sizing: border-box; } 209 | .button:hover, 210 | button:hover, 211 | input[type="submit"]:hover, 212 | input[type="reset"]:hover, 213 | input[type="button"]:hover, 214 | .button:focus, 215 | button:focus, 216 | input[type="submit"]:focus, 217 | input[type="reset"]:focus, 218 | input[type="button"]:focus { 219 | color: #333; 220 | border-color: #888; 221 | outline: 0; } 222 | .button.button-primary, 223 | button.button-primary, 224 | input[type="submit"].button-primary, 225 | input[type="reset"].button-primary, 226 | input[type="button"].button-primary { 227 | color: #FFF; 228 | background-color: #33C3F0; 229 | border-color: #33C3F0; } 230 | .button.button-primary:hover, 231 | button.button-primary:hover, 232 | input[type="submit"].button-primary:hover, 233 | input[type="reset"].button-primary:hover, 234 | input[type="button"].button-primary:hover, 235 | .button.button-primary:focus, 236 | button.button-primary:focus, 237 | input[type="submit"].button-primary:focus, 238 | input[type="reset"].button-primary:focus, 239 | input[type="button"].button-primary:focus { 240 | color: #FFF; 241 | background-color: #1EAEDB; 242 | border-color: #1EAEDB; } 243 | 244 | 245 | /* Forms 246 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 247 | input[type="email"], 248 | input[type="number"], 249 | input[type="search"], 250 | input[type="text"], 251 | input[type="tel"], 252 | input[type="url"], 253 | input[type="password"], 254 | textarea, 255 | select { 256 | height: 38px; 257 | padding: 6px 10px; /* The 6px vertically centers text on FF, ignored by Webkit */ 258 | background-color: #fff; 259 | border: 1px solid #D1D1D1; 260 | border-radius: 4px; 261 | box-shadow: none; 262 | box-sizing: border-box; 263 | font-family: inherit; 264 | font-size: inherit; /*https://stackoverflow.com/questions/6080413/why-doesnt-input-inherit-the-font-from-body*/} 265 | /* Removes awkward default styles on some inputs for iOS */ 266 | input[type="email"], 267 | input[type="number"], 268 | input[type="search"], 269 | input[type="text"], 270 | input[type="tel"], 271 | input[type="url"], 272 | input[type="password"], 273 | textarea { 274 | -webkit-appearance: none; 275 | -moz-appearance: none; 276 | appearance: none; } 277 | textarea { 278 | min-height: 65px; 279 | padding-top: 6px; 280 | padding-bottom: 6px; } 281 | input[type="email"]:focus, 282 | input[type="number"]:focus, 283 | input[type="search"]:focus, 284 | input[type="text"]:focus, 285 | input[type="tel"]:focus, 286 | input[type="url"]:focus, 287 | input[type="password"]:focus, 288 | textarea:focus, 289 | select:focus { 290 | border: 1px solid #33C3F0; 291 | outline: 0; } 292 | label, 293 | legend { 294 | display: block; 295 | margin-bottom: 0px; } 296 | fieldset { 297 | padding: 0; 298 | border-width: 0; } 299 | input[type="checkbox"], 300 | input[type="radio"] { 301 | display: inline; } 302 | label > .label-body { 303 | display: inline-block; 304 | margin-left: .5rem; 305 | font-weight: normal; } 306 | 307 | 308 | /* Lists 309 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 310 | ul { 311 | list-style: circle inside; } 312 | ol { 313 | list-style: decimal inside; } 314 | ol, ul { 315 | padding-left: 0; 316 | margin-top: 0; } 317 | ul ul, 318 | ul ol, 319 | ol ol, 320 | ol ul { 321 | margin: 1.5rem 0 1.5rem 3rem; 322 | font-size: 90%; } 323 | li { 324 | margin-bottom: 1rem; } 325 | 326 | 327 | /* Tables 328 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 329 | table { 330 | border-collapse: collapse; 331 | } 332 | th:not(.CalendarDay), 333 | td:not(.CalendarDay) { 334 | padding: 12px 15px; 335 | text-align: left; 336 | border-bottom: 1px solid #E1E1E1; } 337 | th:first-child:not(.CalendarDay), 338 | td:first-child:not(.CalendarDay) { 339 | padding-left: 0; } 340 | th:last-child:not(.CalendarDay), 341 | td:last-child:not(.CalendarDay) { 342 | padding-right: 0; } 343 | 344 | 345 | /* Spacing 346 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 347 | button, 348 | .button { 349 | margin-bottom: 0rem; } 350 | input, 351 | textarea, 352 | select, 353 | fieldset { 354 | margin-bottom: 0rem; } 355 | pre, 356 | dl, 357 | figure, 358 | table, 359 | form { 360 | margin-bottom: 0rem; } 361 | p, 362 | ul, 363 | ol { 364 | margin-bottom: 0.75rem; } 365 | 366 | /* Utilities 367 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 368 | .u-full-width { 369 | width: 100%; 370 | box-sizing: border-box; } 371 | .u-max-full-width { 372 | max-width: 100%; 373 | box-sizing: border-box; } 374 | .u-pull-right { 375 | float: right; } 376 | .u-pull-left { 377 | float: left; } 378 | 379 | 380 | /* Misc 381 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 382 | hr { 383 | margin-top: 3rem; 384 | margin-bottom: 3.5rem; 385 | border-width: 0; 386 | border-top: 1px solid #E1E1E1; } 387 | 388 | 389 | /* Clearing 390 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 391 | 392 | /* Self Clearing Goodness */ 393 | .container:after, 394 | .row:after, 395 | .u-cf { 396 | content: ""; 397 | display: table; 398 | clear: both; } 399 | 400 | 401 | /* Media Queries 402 | –––––––––––––––––––––––––––––––––––––––––––––––––– */ 403 | /* 404 | Note: The best way to structure the use of media queries is to create the queries 405 | near the relevant code. For example, if you wanted to change the styles for buttons 406 | on small devices, paste the mobile query code up in the buttons section and style it 407 | there. 408 | */ 409 | 410 | 411 | /* Larger than mobile */ 412 | @media (min-width: 400px) {} 413 | 414 | /* Larger than phablet (also point when grid becomes active) */ 415 | @media (min-width: 550px) {} 416 | 417 | /* Larger than tablet */ 418 | @media (min-width: 750px) {} 419 | 420 | /* Larger than desktop */ 421 | @media (min-width: 1000px) {} 422 | 423 | /* Larger than Desktop HD */ 424 | @media (min-width: 1200px) {} -------------------------------------------------------------------------------- /hexapod/models.py: -------------------------------------------------------------------------------- 1 | # This module contains the model of a hexapod 2 | # It's used to manipulate the pose of the hexapod 3 | from copy import deepcopy 4 | from pprint import pprint 5 | from math import atan2, degrees, isclose 6 | import json 7 | import numpy as np 8 | from settings import PRINT_MODEL_ON_UPDATE, ALPHA_MAX_ANGLE, BETA_MAX_ANGLE, GAMMA_MAX_ANGLE 9 | from hexapod.linkage import Linkage 10 | import hexapod.ground_contact_solver.ground_contact_solver as gc 11 | import hexapod.ground_contact_solver.ground_contact_solver2 as gc2 12 | 13 | from hexapod.templates.pose_template import HEXAPOD_POSE 14 | from hexapod.points import ( 15 | Vector, 16 | frame_to_align_vector_a_to_b, 17 | frame_rotxyz, 18 | rotz, 19 | ) 20 | 21 | 22 | # Dimensions f, s, and m 23 | # 24 | # |-f-| 25 | # *---*---*-------- 26 | # / | \ | 27 | # / | \ s 28 | # / | \ | 29 | # *------cog------* --- 30 | # \ | /| 31 | # \ | / | 32 | # \ | / | 33 | # *---*---* | 34 | # | | 35 | # |---m---| 36 | # 37 | # y axis 38 | # ^ 39 | # | 40 | # | 41 | # ----> x axis 42 | # cog (origin) 43 | # 44 | # 45 | # Relative x-axis, for each attached linkage 46 | # 47 | # x2 x1 48 | # \ / 49 | # *---*---* 50 | # / | \ 51 | # / | \ 52 | # / | \ 53 | # x3 --*------cog------*-- x0 54 | # \ | / 55 | # \ | / 56 | # \ | / 57 | # *---*---* 58 | # / \ 59 | # x4 x5 60 | # 61 | class Hexagon: 62 | VERTEX_NAMES = ( 63 | "right-middle", 64 | "right-front", 65 | "left-front", 66 | "left-middle", 67 | "left-back", 68 | "right-back", 69 | ) 70 | COXIA_AXES = (0, 45, 135, 180, 225, 315) 71 | __slots__ = ("f", "m", "s", "cog", "head", "vertices", "all_points") 72 | 73 | def __init__(self, f, m, s): 74 | self.f = f 75 | self.m = m 76 | self.s = s 77 | 78 | self.cog = Vector(0, 0, 0, name="center-of-gravity") 79 | self.head = Vector(0, s, 0, name="head") 80 | self.vertices = [ 81 | Vector(m, 0, 0, name=Hexagon.VERTEX_NAMES[0]), 82 | Vector(f, s, 0, name=Hexagon.VERTEX_NAMES[1]), 83 | Vector(-f, s, 0, name=Hexagon.VERTEX_NAMES[2]), 84 | Vector(-m, 0, 0, name=Hexagon.VERTEX_NAMES[3]), 85 | Vector(-f, -s, 0, name=Hexagon.VERTEX_NAMES[4]), 86 | Vector(f, -s, 0, name=Hexagon.VERTEX_NAMES[5]), 87 | ] 88 | 89 | self.all_points = self.vertices + [self.cog, self.head] 90 | 91 | 92 | # .......................................... 93 | # The hexapod model 94 | # .......................................... 95 | class VirtualHexapod: 96 | LEG_COUNT = 6 97 | __slots__ = ( 98 | "body", 99 | "legs", 100 | "dimensions", 101 | "coxia", 102 | "femur", 103 | "tibia", 104 | "front", 105 | "side", 106 | "mid", 107 | "body_rotation_frame", 108 | "ground_contacts", 109 | "x_axis", 110 | "y_axis", 111 | "z_axis", 112 | ) 113 | 114 | def __init__(self, dimensions): 115 | self._store_attributes(dimensions) 116 | self._init_legs() 117 | self._init_local_frame() 118 | 119 | def update(self, poses, assume_ground_targets=True): 120 | might_raise_poses_range_error(poses) 121 | 122 | self.body_rotation_frame = None 123 | might_twist = find_if_might_twist(self, poses) 124 | old_contacts = deepcopy(self.ground_contacts) 125 | 126 | # Update leg poses 127 | for pose in poses.values(): 128 | i = pose["id"] 129 | self.legs[i].change_pose(pose["coxia"], pose["femur"], pose["tibia"]) 130 | 131 | # Find new orientation of the body (new normal) 132 | # distance of cog from ground and which legs are on the ground 133 | if assume_ground_targets: 134 | # We are positive that our assumed target ground contact points 135 | # are correct then we don't have to test all possible cases 136 | legs, n_axis, height = gc.compute_orientation_properties(self.legs) 137 | else: 138 | legs, n_axis, height = gc2.compute_orientation_properties(self.legs) 139 | 140 | if n_axis is None: 141 | raise Exception("❗Pose Unstable. COG not inside support polygon.") 142 | 143 | # Tilt and shift the hexapod based on new normal 144 | frame = frame_to_align_vector_a_to_b(n_axis, Vector(0, 0, 1)) 145 | self.rotate_and_shift(frame, height) 146 | self._update_local_frame(frame) 147 | 148 | # Twist around the new normal if you have to 149 | self.ground_contacts = [leg.ground_contact() for leg in legs] 150 | 151 | if might_twist: 152 | twist_frame = find_twist_frame(old_contacts, self.ground_contacts) 153 | self.rotate_and_shift(twist_frame) 154 | 155 | might_print_hexapod(self, poses) 156 | 157 | def detach_body_rotate_and_translate(self, rx, ry, rz, tx, ty, tz): 158 | # Detach the body of the hexapod from the legs 159 | # then rotate and translate body as if a separate entity 160 | frame = frame_rotxyz(rx, ry, rz) 161 | self.body_rotation_frame = frame 162 | 163 | for point in self.body.all_points: 164 | point.update_point_wrt(frame) 165 | point.move_xyz(tx, ty, tz) 166 | 167 | self._update_local_frame(frame) 168 | 169 | def move_xyz(self, tx, ty, tz): 170 | for point in self.body.all_points: 171 | point.move_xyz(tx, ty, tz) 172 | 173 | for leg in self.legs: 174 | for point in leg.all_points: 175 | point.move_xyz(tx, ty, tz) 176 | 177 | def update_stance(self, hip_stance, leg_stance): 178 | pose = deepcopy(HEXAPOD_POSE) 179 | pose[1]["coxia"] = -hip_stance # right_front 180 | pose[2]["coxia"] = hip_stance # left_front 181 | pose[4]["coxia"] = -hip_stance # left_back 182 | pose[5]["coxia"] = hip_stance # right_back 183 | 184 | for leg in pose.values(): 185 | leg["femur"] = leg_stance 186 | leg["tibia"] = -leg_stance 187 | 188 | self.update(pose) 189 | 190 | def sum_of_dimensions(self): 191 | f, m, s = self.front, self.mid, self.side 192 | a, b, c = self.coxia, self.femur, self.tibia 193 | return f + m + s + a + b + c 194 | 195 | def _store_attributes(self, dimensions): 196 | self.body_rotation_frame = None 197 | self.dimensions = dimensions 198 | self.coxia = dimensions["coxia"] 199 | self.femur = dimensions["femur"] 200 | self.tibia = dimensions["tibia"] 201 | self.front = dimensions["front"] 202 | self.mid = dimensions["middle"] 203 | self.side = dimensions["side"] 204 | self.body = Hexagon(self.front, self.mid, self.side) 205 | 206 | def _init_legs(self): 207 | self.legs = [] 208 | for i in range(VirtualHexapod.LEG_COUNT): 209 | linkage = Linkage( 210 | self.coxia, 211 | self.femur, 212 | self.tibia, 213 | coxia_axis=Hexagon.COXIA_AXES[i], 214 | new_origin=self.body.vertices[i], 215 | name=Hexagon.VERTEX_NAMES[i], 216 | id_number=i, 217 | ) 218 | self.legs.append(linkage) 219 | 220 | self.ground_contacts = [leg.ground_contact() for leg in self.legs] 221 | 222 | def rotate_and_shift(self, frame, height=0): 223 | for vertex in self.body.all_points: 224 | vertex.update_point_wrt(frame, height) 225 | 226 | for leg in self.legs: 227 | leg.update_leg_wrt(frame, height) 228 | 229 | def _init_local_frame(self): 230 | self.x_axis = Vector(1, 0, 0, name="hexapod x axis") 231 | self.y_axis = Vector(0, 1, 0, name="hexapod y axis") 232 | self.z_axis = Vector(0, 0, 1, name="hexapod z axis") 233 | 234 | def _update_local_frame(self, frame): 235 | # Update the x, y, z axis centered at cog of hexapod 236 | self.x_axis.update_point_wrt(frame) 237 | self.y_axis.update_point_wrt(frame) 238 | self.z_axis.update_point_wrt(frame) 239 | 240 | 241 | # .......................................... 242 | # Helper functions 243 | # .......................................... 244 | 245 | def might_raise_poses_range_error(poses): 246 | angle_limits = { 247 | "coxia": ALPHA_MAX_ANGLE, 248 | "femur": BETA_MAX_ANGLE, 249 | "tibia": GAMMA_MAX_ANGLE, 250 | } 251 | 252 | def _within_range(angle, max_angle): 253 | return -max_angle <= angle <= max_angle 254 | 255 | def _raise_range_error(leg_name, joint_name, angle, max_angle): 256 | identifier = f"{leg_name} leg's {joint_name} angle is {angle}" 257 | msg = f"{identifier}. Must be within [-{max_angle}, {max_angle}]" 258 | raise Exception(msg) 259 | 260 | for pose in poses.values(): 261 | for joint_name in angle_limits: 262 | 263 | angle = pose[joint_name] 264 | max_angle = angle_limits[joint_name] 265 | 266 | if _within_range(angle, max_angle): 267 | continue 268 | 269 | _raise_range_error(pose["name"], joint_name, angle, max_angle) 270 | 271 | 272 | def get_hip_angle(leg_id, poses): 273 | if leg_id in poses: 274 | return poses[leg_id]["coxia"] 275 | 276 | if str(leg_id) in poses: 277 | return poses[str(leg_id)]["coxia"] 278 | 279 | # ❗Error will silently pass, is this ok? 280 | return 0.0 281 | 282 | 283 | def find_if_might_twist(hexapod, poses): 284 | # The hexapod will only definitely NOT twist 285 | # if only two of the legs that's currently on the ground 286 | # has twisted its hips/coxia 287 | # i.e. only 2 legs with ground contact points have changed their alpha angles 288 | # i.e. we don't care if the legs which are not on the ground twisted its hips 289 | def _find_leg_id(leg_point): 290 | right_or_left, front_mid_or_back, _ = leg_point.name.split("-") 291 | leg_placement = right_or_left + "-" + front_mid_or_back 292 | return Hexagon.VERTEX_NAMES.index(leg_placement) 293 | 294 | did_change_count = 0 295 | 296 | for leg_point in hexapod.ground_contacts: 297 | leg_id = _find_leg_id(leg_point) 298 | old_hip_angle = hexapod.legs[leg_id].coxia_angle() 299 | new_hip_angle = get_hip_angle(leg_id, poses) 300 | if not isclose(old_hip_angle, new_hip_angle): 301 | did_change_count += 1 302 | if did_change_count >= 3: 303 | return True 304 | 305 | return False 306 | 307 | 308 | def find_twist_frame(old_ground_contacts, new_ground_contacts): 309 | # This is the frame used to twist the model about the z axis 310 | 311 | def _make_contact_dict(contact_list): 312 | return {leg_point.name: leg_point for leg_point in contact_list} 313 | 314 | def _twist(v1, v2): 315 | # https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/ 316 | theta = atan2(v2.y, v2.x) - atan2(v1.y, v1.x) 317 | return rotz(degrees(theta)) 318 | 319 | # Make dictionary mapping contact point name and leg_contact_point 320 | old_contacts = _make_contact_dict(old_ground_contacts) 321 | new_contacts = _make_contact_dict(new_ground_contacts) 322 | 323 | # Find at least one point that's the same 324 | same_point_name = None 325 | for key in old_contacts: 326 | if key in new_contacts: 327 | same_point_name = key 328 | break 329 | 330 | # We don't know how to rotate if we don't 331 | # know at least one point that's on the ground 332 | # before and after the movement, 333 | # so we assume that the hexapod didn't move 334 | if same_point_name is None: 335 | return np.eye(4) 336 | 337 | old = old_contacts[same_point_name] 338 | new = new_contacts[same_point_name] 339 | 340 | # Get the projection of these points in the ground 341 | old_vector = Vector(old.x, old.y, 0) 342 | new_vector = Vector(new.x, new.y, 0) 343 | 344 | twist_frame = _twist(new_vector, old_vector) 345 | 346 | # ❗IMPORTANT: We are assuming that because the point 347 | # is on the ground before and after 348 | # They should be at the same point after movement 349 | # I can't think of a case that contradicts this as of this moment 350 | return twist_frame 351 | 352 | 353 | def might_print_hexapod(hexapod, poses): 354 | if not PRINT_MODEL_ON_UPDATE: 355 | return 356 | 357 | print("█████████████████████████████") 358 | print("█ start: Hexapod Model █") 359 | print("█████████████████████████████") 360 | 361 | print("............") 362 | print("...Dimensions") 363 | print("............") 364 | print(json.dumps(hexapod.dimensions, indent=4)) 365 | 366 | print("............") 367 | print("...Vertices") 368 | print("............") 369 | pprint(hexapod.body.all_points) 370 | 371 | print("............") 372 | print("...Legs") 373 | print("............") 374 | for i, leg in enumerate(hexapod.legs): 375 | print(f"\nleg{i}_points = ") 376 | pprint(leg.all_points) 377 | 378 | print("............") 379 | print("...Poses") 380 | print("............") 381 | print(json.dumps(poses, indent=4)) 382 | 383 | print("█████████████████████████████") 384 | print("█ end: Hexapod Model █") 385 | print("█████████████████████████████") 386 | -------------------------------------------------------------------------------- /.pylintrc: -------------------------------------------------------------------------------- 1 | [MASTER] 2 | 3 | # A comma-separated list of package or module names from where C extensions may 4 | # be loaded. Extensions are loading into the active Python interpreter and may 5 | # run arbitrary code. 6 | extension-pkg-whitelist= 7 | 8 | # Add files or directories to the blacklist. They should be base names, not 9 | # paths. 10 | #ignore= 11 | 12 | # Add files or directories matching the regex patterns to the blacklist. The 13 | # regex matches against base names, not paths. 14 | #ignore-patterns= 15 | 16 | # Python code to execute, usually for sys.path manipulation such as 17 | # pygtk.require(). 18 | #init-hook= 19 | 20 | # Use multiple processes to speed up Pylint. Specifying 0 will auto-detect the 21 | # number of processors available to use. 22 | jobs=1 23 | 24 | # Control the amount of potential inferred values when inferring a single 25 | # object. This can help the performance when dealing with large functions or 26 | # complex, nested conditions. 27 | limit-inference-results=100 28 | 29 | # List of plugins (as comma separated values of python module names) to load, 30 | # usually to register additional checkers. 31 | load-plugins= 32 | 33 | # Pickle collected data for later comparisons. 34 | persistent=yes 35 | 36 | # Specify a configuration file. 37 | #rcfile= 38 | 39 | # When enabled, pylint would attempt to guess common misconfiguration and emit 40 | # user-friendly hints instead of false-positive error messages. 41 | suggestion-mode=yes 42 | 43 | # Allow loading of arbitrary C extensions. Extensions are imported into the 44 | # active Python interpreter and may run arbitrary code. 45 | unsafe-load-any-extension=no 46 | 47 | 48 | [MESSAGES CONTROL] 49 | 50 | # Only show warnings with the listed confidence levels. Leave empty to show 51 | # all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED. 52 | confidence= 53 | 54 | # Disable the message, report, category or checker with the given id(s). You 55 | # can either give multiple identifiers separated by comma (,) or put this 56 | # option multiple times (only on the command line, not in the configuration 57 | # file where it should appear only once). You can also use "--disable=all" to 58 | # disable everything first and then reenable specific checks. For example, if 59 | # you want to run only the similarities checker, you can use "--disable=all 60 | # --enable=similarities". If you want to run only the classes checker, but have 61 | # no Warning level messages displayed, use "--disable=all --enable=classes 62 | # --disable=W". 63 | disable=print-statement, 64 | parameter-unpacking, 65 | unpacking-in-except, 66 | old-raise-syntax, 67 | backtick, 68 | long-suffix, 69 | old-ne-operator, 70 | old-octal-literal, 71 | import-star-module-level, 72 | non-ascii-bytes-literal, 73 | raw-checker-failed, 74 | bad-inline-option, 75 | locally-disabled, 76 | file-ignored, 77 | suppressed-message, 78 | useless-suppression, 79 | deprecated-pragma, 80 | use-symbolic-message-instead, 81 | apply-builtin, 82 | basestring-builtin, 83 | buffer-builtin, 84 | cmp-builtin, 85 | coerce-builtin, 86 | execfile-builtin, 87 | file-builtin, 88 | long-builtin, 89 | raw_input-builtin, 90 | reduce-builtin, 91 | standarderror-builtin, 92 | unicode-builtin, 93 | xrange-builtin, 94 | coerce-method, 95 | delslice-method, 96 | getslice-method, 97 | setslice-method, 98 | no-absolute-import, 99 | old-division, 100 | dict-iter-method, 101 | dict-view-method, 102 | next-method-called, 103 | metaclass-assignment, 104 | indexing-exception, 105 | raising-string, 106 | reload-builtin, 107 | oct-method, 108 | hex-method, 109 | nonzero-method, 110 | cmp-method, 111 | input-builtin, 112 | round-builtin, 113 | intern-builtin, 114 | unichr-builtin, 115 | map-builtin-not-iterating, 116 | zip-builtin-not-iterating, 117 | range-builtin-not-iterating, 118 | filter-builtin-not-iterating, 119 | using-cmp-argument, 120 | eq-without-hash, 121 | div-method, 122 | idiv-method, 123 | rdiv-method, 124 | exception-message-attribute, 125 | invalid-str-codec, 126 | sys-max-int, 127 | bad-python3-import, 128 | deprecated-string-function, 129 | deprecated-str-translate-call, 130 | deprecated-itertools-function, 131 | deprecated-types-field, 132 | next-method-defined, 133 | dict-items-not-iterating, 134 | dict-keys-not-iterating, 135 | dict-values-not-iterating, 136 | deprecated-operator-function, 137 | deprecated-urllib-function, 138 | xreadlines-attribute, 139 | deprecated-sys-function, 140 | exception-escape, 141 | comprehension-escape 142 | 143 | # Enable the message, report, category or checker with the given id(s). You can 144 | # either give multiple identifier separated by comma (,) or put this option 145 | # multiple time (only on the command line, not in the configuration file where 146 | # it should appear only once). See also the "--disable" option for examples. 147 | enable=c-extension-no-member 148 | 149 | 150 | [REPORTS] 151 | 152 | # Python expression which should return a score less than or equal to 10. You 153 | # have access to the variables 'error', 'warning', 'refactor', and 'convention' 154 | # which contain the number of messages in each category, as well as 'statement' 155 | # which is the total number of statements analyzed. This score is used by the 156 | # global evaluation report (RP0004). 157 | evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) 158 | 159 | # Template used to display messages. This is a python new-style format string 160 | # used to format the message information. See doc for all details. 161 | #msg-template= 162 | 163 | # Set the output format. Available formats are text, parseable, colorized, json 164 | # and msvs (visual studio). You can also give a reporter class, e.g. 165 | # mypackage.mymodule.MyReporterClass. 166 | output-format=text 167 | 168 | # Tells whether to display a full report or only the messages. 169 | reports=no 170 | 171 | # Activate the evaluation score. 172 | score=yes 173 | 174 | 175 | [REFACTORING] 176 | 177 | # Maximum number of nested blocks for function / method body 178 | max-nested-blocks=5 179 | 180 | # Complete name of functions that never returns. When checking for 181 | # inconsistent-return-statements if a never returning function is called then 182 | # it will be considered as an explicit return statement and no message will be 183 | # printed. 184 | never-returning-functions=sys.exit 185 | 186 | 187 | [LOGGING] 188 | 189 | # Format style used to check logging format string. `old` means using % 190 | # formatting, `new` is for `{}` formatting,and `fstr` is for f-strings. 191 | logging-format-style=old 192 | 193 | # Logging modules to check that the string format arguments are in logging 194 | # function parameter format. 195 | logging-modules=logging 196 | 197 | 198 | [SPELLING] 199 | 200 | # Limits count of emitted suggestions for spelling mistakes. 201 | max-spelling-suggestions=4 202 | 203 | # Spelling dictionary name. Available dictionaries: none. To make it work, 204 | # install the python-enchant package. 205 | spelling-dict= 206 | 207 | # List of comma separated words that should not be checked. 208 | spelling-ignore-words= 209 | 210 | # A path to a file that contains the private dictionary; one word per line. 211 | spelling-private-dict-file= 212 | 213 | # Tells whether to store unknown words to the private dictionary (see the 214 | # --spelling-private-dict-file option) instead of raising a message. 215 | spelling-store-unknown-words=no 216 | 217 | 218 | [MISCELLANEOUS] 219 | 220 | # List of note tags to take in consideration, separated by a comma. 221 | notes=FIXME, 222 | XXX, 223 | TODO 224 | 225 | 226 | [TYPECHECK] 227 | 228 | # List of decorators that produce context managers, such as 229 | # contextlib.contextmanager. Add to this list to register other decorators that 230 | # produce valid context managers. 231 | contextmanager-decorators=contextlib.contextmanager 232 | 233 | # List of members which are set dynamically and missed by pylint inference 234 | # system, and so shouldn't trigger E1101 when accessed. Python regular 235 | # expressions are accepted. 236 | generated-members= 237 | 238 | # Tells whether missing members accessed in mixin class should be ignored. A 239 | # mixin class is detected if its name ends with "mixin" (case insensitive). 240 | ignore-mixin-members=yes 241 | 242 | # Tells whether to warn about missing members when the owner of the attribute 243 | # is inferred to be None. 244 | ignore-none=yes 245 | 246 | # This flag controls whether pylint should warn about no-member and similar 247 | # checks whenever an opaque object is returned when inferring. The inference 248 | # can return multiple potential results while evaluating a Python object, but 249 | # some branches might not be evaluated, which results in partial inference. In 250 | # that case, it might be useful to still emit no-member and other checks for 251 | # the rest of the inferred objects. 252 | ignore-on-opaque-inference=yes 253 | 254 | # List of class names for which member attributes should not be checked (useful 255 | # for classes with dynamically set attributes). This supports the use of 256 | # qualified names. 257 | ignored-classes=optparse.Values,thread._local,_thread._local 258 | 259 | # List of module names for which member attributes should not be checked 260 | # (useful for modules/projects where namespaces are manipulated during runtime 261 | # and thus existing member attributes cannot be deduced by static analysis). It 262 | # supports qualified module names, as well as Unix pattern matching. 263 | ignored-modules= 264 | 265 | # Show a hint with possible names when a member name was not found. The aspect 266 | # of finding the hint is based on edit distance. 267 | missing-member-hint=yes 268 | 269 | # The minimum edit distance a name should have in order to be considered a 270 | # similar match for a missing member name. 271 | missing-member-hint-distance=1 272 | 273 | # The total number of similar names that should be taken in consideration when 274 | # showing a hint for a missing member. 275 | missing-member-max-choices=1 276 | 277 | # List of decorators that change the signature of a decorated function. 278 | signature-mutators= 279 | 280 | 281 | [VARIABLES] 282 | 283 | # List of additional names supposed to be defined in builtins. Remember that 284 | # you should avoid defining new builtins when possible. 285 | additional-builtins= 286 | 287 | # Tells whether unused global variables should be treated as a violation. 288 | allow-global-unused-variables=yes 289 | 290 | # List of strings which can identify a callback function by name. A callback 291 | # name must start or end with one of those strings. 292 | callbacks=cb_, 293 | _cb 294 | 295 | # A regular expression matching the name of dummy variables (i.e. expected to 296 | # not be used). 297 | dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ 298 | 299 | # Argument names that match this expression will be ignored. Default to name 300 | # with leading underscore. 301 | ignored-argument-names=_.*|^ignored_|^unused_ 302 | 303 | # Tells whether we should check for unused import in __init__ files. 304 | init-import=no 305 | 306 | # List of qualified module names which can have objects that can redefine 307 | # builtins. 308 | redefining-builtins-modules=six.moves,past.builtins,future.builtins,builtins,io 309 | 310 | 311 | [FORMAT] 312 | 313 | # Expected format of line ending, e.g. empty (any line ending), LF or CRLF. 314 | expected-line-ending-format= 315 | 316 | # Regexp for a line that is allowed to be longer than the limit. 317 | ignore-long-lines=^\s*(# )??$ 318 | 319 | # Number of spaces of indent required inside a hanging or continued line. 320 | indent-after-paren=4 321 | 322 | # String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 323 | # tab). 324 | indent-string=' ' 325 | 326 | # Maximum number of characters on a single line. 327 | max-line-length=100 328 | 329 | # Maximum number of lines in a module. 330 | max-module-lines=1000 331 | 332 | # List of optional constructs for which whitespace checking is disabled. `dict- 333 | # separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. 334 | # `trailing-comma` allows a space between comma and closing bracket: (a, ). 335 | # `empty-line` allows space-only lines. 336 | no-space-check=trailing-comma, 337 | dict-separator 338 | 339 | # Allow the body of a class to be on the same line as the declaration if body 340 | # contains single statement. 341 | single-line-class-stmt=no 342 | 343 | # Allow the body of an if to be on the same line as the test if there is no 344 | # else. 345 | single-line-if-stmt=no 346 | 347 | 348 | [SIMILARITIES] 349 | 350 | # Ignore comments when computing similarities. 351 | ignore-comments=yes 352 | 353 | # Ignore docstrings when computing similarities. 354 | ignore-docstrings=yes 355 | 356 | # Ignore imports when computing similarities. 357 | ignore-imports=no 358 | 359 | # Minimum lines number of a similarity. 360 | min-similarity-lines=4 361 | 362 | 363 | [BASIC] 364 | 365 | # Naming style matching correct argument names. 366 | argument-naming-style=snake_case 367 | 368 | # Regular expression matching correct argument names. Overrides argument- 369 | # naming-style. 370 | #argument-rgx= 371 | 372 | # Naming style matching correct attribute names. 373 | attr-naming-style=snake_case 374 | 375 | # Regular expression matching correct attribute names. Overrides attr-naming- 376 | # style. 377 | #attr-rgx= 378 | 379 | # Bad variable names which should always be refused, separated by a comma. 380 | bad-names=foo, 381 | bar, 382 | baz, 383 | toto, 384 | tutu, 385 | tata 386 | 387 | # Naming style matching correct class attribute names. 388 | class-attribute-naming-style=any 389 | 390 | # Regular expression matching correct class attribute names. Overrides class- 391 | # attribute-naming-style. 392 | #class-attribute-rgx= 393 | 394 | # Naming style matching correct class names. 395 | class-naming-style=PascalCase 396 | 397 | # Regular expression matching correct class names. Overrides class-naming- 398 | # style. 399 | #class-rgx= 400 | 401 | # Naming style matching correct constant names. 402 | const-naming-style=UPPER_CASE 403 | 404 | # Regular expression matching correct constant names. Overrides const-naming- 405 | # style. 406 | #const-rgx= 407 | 408 | # Minimum line length for functions/classes that require docstrings, shorter 409 | # ones are exempt. 410 | docstring-min-length=-1 411 | 412 | # Naming style matching correct function names. 413 | function-naming-style=snake_case 414 | 415 | # Regular expression matching correct function names. Overrides function- 416 | # naming-style. 417 | #function-rgx= 418 | 419 | # Good variable names which should always be accepted, separated by a comma. 420 | good-names=i, 421 | j, 422 | k, 423 | ex, 424 | Run, 425 | _ 426 | 427 | # Include a hint for the correct naming format with invalid-name. 428 | include-naming-hint=no 429 | 430 | # Naming style matching correct inline iteration names. 431 | inlinevar-naming-style=any 432 | 433 | # Regular expression matching correct inline iteration names. Overrides 434 | # inlinevar-naming-style. 435 | #inlinevar-rgx= 436 | 437 | # Naming style matching correct method names. 438 | method-naming-style=snake_case 439 | 440 | # Regular expression matching correct method names. Overrides method-naming- 441 | # style. 442 | #method-rgx= 443 | 444 | # Naming style matching correct module names. 445 | module-naming-style=snake_case 446 | 447 | # Regular expression matching correct module names. Overrides module-naming- 448 | # style. 449 | #module-rgx= 450 | 451 | # Colon-delimited sets of names that determine each other's naming style when 452 | # the name regexes allow several styles. 453 | name-group= 454 | 455 | # Regular expression which should only match function or class names that do 456 | # not require a docstring. 457 | no-docstring-rgx=^_ 458 | 459 | # List of decorators that produce properties, such as abc.abstractproperty. Add 460 | # to this list to register other decorators that produce valid properties. 461 | # These decorators are taken in consideration only for invalid-name. 462 | property-classes=abc.abstractproperty 463 | 464 | # Naming style matching correct variable names. 465 | variable-naming-style=snake_case 466 | 467 | # Regular expression matching correct variable names. Overrides variable- 468 | # naming-style. 469 | #variable-rgx= 470 | 471 | 472 | [STRING] 473 | 474 | # This flag controls whether the implicit-str-concat-in-sequence should 475 | # generate a warning on implicit string concatenation in sequences defined over 476 | # several lines. 477 | check-str-concat-over-line-jumps=no 478 | 479 | 480 | [IMPORTS] 481 | 482 | # List of modules that can be imported at any level, not just the top level 483 | # one. 484 | allow-any-import-level= 485 | 486 | # Allow wildcard imports from modules that define __all__. 487 | allow-wildcard-with-all=no 488 | 489 | # Analyse import fallback blocks. This can be used to support both Python 2 and 490 | # 3 compatible code, which means that the block might have code that exists 491 | # only in one or another interpreter, leading to false positives when analysed. 492 | analyse-fallback-blocks=no 493 | 494 | # Deprecated modules which should not be used, separated by a comma. 495 | deprecated-modules=optparse,tkinter.tix 496 | 497 | # Create a graph of external dependencies in the given file (report RP0402 must 498 | # not be disabled). 499 | ext-import-graph= 500 | 501 | # Create a graph of every (i.e. internal and external) dependencies in the 502 | # given file (report RP0402 must not be disabled). 503 | import-graph= 504 | 505 | # Create a graph of internal dependencies in the given file (report RP0402 must 506 | # not be disabled). 507 | int-import-graph= 508 | 509 | # Force import order to recognize a module as part of the standard 510 | # compatibility libraries. 511 | known-standard-library= 512 | 513 | # Force import order to recognize a module as part of a third party library. 514 | known-third-party=enchant 515 | 516 | # Couples of modules and preferred modules, separated by a comma. 517 | preferred-modules= 518 | 519 | 520 | [CLASSES] 521 | 522 | # List of method names used to declare (i.e. assign) instance attributes. 523 | defining-attr-methods=__init__, 524 | __new__, 525 | setUp, 526 | __post_init__ 527 | 528 | # List of member names, which should be excluded from the protected access 529 | # warning. 530 | exclude-protected=_asdict, 531 | _fields, 532 | _replace, 533 | _source, 534 | _make 535 | 536 | # List of valid names for the first argument in a class method. 537 | valid-classmethod-first-arg=cls 538 | 539 | # List of valid names for the first argument in a metaclass class method. 540 | valid-metaclass-classmethod-first-arg=cls 541 | 542 | 543 | [DESIGN] 544 | 545 | # Maximum number of arguments for function / method. 546 | max-args=5 547 | 548 | # Maximum number of attributes for a class (see R0902). 549 | max-attributes=7 550 | 551 | # Maximum number of boolean expressions in an if statement (see R0916). 552 | max-bool-expr=5 553 | 554 | # Maximum number of branch for function / method body. 555 | max-branches=12 556 | 557 | # Maximum number of locals for function / method body. 558 | max-locals=15 559 | 560 | # Maximum number of parents for a class (see R0901). 561 | max-parents=7 562 | 563 | # Maximum number of public methods for a class (see R0904). 564 | max-public-methods=20 565 | 566 | # Maximum number of return / yield for function / method body. 567 | max-returns=6 568 | 569 | # Maximum number of statements in function / method body. 570 | max-statements=50 571 | 572 | # Minimum number of public methods for a class (see R0903). 573 | min-public-methods=2 574 | 575 | 576 | [EXCEPTIONS] 577 | 578 | # Exceptions that will emit a warning when being caught. Defaults to 579 | # "BaseException, Exception". 580 | overgeneral-exceptions=BaseException, 581 | Exception 582 | --------------------------------------------------------------------------------