├── .bazelrc ├── .gitignore ├── .travis.yml ├── BOM.md ├── BUILD ├── LICENSE ├── README.md ├── WORKSPACE ├── base ├── BUILD ├── aspect_ratio.cc ├── aspect_ratio.h ├── async_i2c.h ├── bezier.h ├── circular_buffer.h ├── common.h ├── component_archives.h ├── context.cc ├── context.h ├── context_full.h ├── error_wrap.h ├── euler.h ├── fail_functor.h ├── fit_plane.cc ├── fit_plane.h ├── format_hex.cc ├── format_hex.h ├── git_info.cc ├── git_info.h ├── git_info_linkstamp.cc ├── handler_util.h ├── interpolate.h ├── kinematic_relation.h ├── leg_force.cc ├── leg_force.h ├── linux_input.cc ├── linux_input.h ├── logging.cc ├── logging.h ├── module_main.bzl ├── module_main.h ├── named_type.h ├── point3d.h ├── quaternion.cc ├── quaternion.h ├── reservoir_sampler.h ├── runfiles.h ├── saturate.h ├── signal_result.h ├── sophus.h ├── stringify.h ├── system_fd.cc ├── system_fd.h ├── system_mmap.h ├── telemetry_log_registrar.h ├── telemetry_registry.h ├── telemetry_remote_debug_registrar.h ├── telemetry_remote_debug_server.cc ├── telemetry_remote_debug_server.h ├── test │ ├── aspect_ratio_test.cc │ ├── bezier_test.cc │ ├── fit_plane_test.cc │ ├── leg_force_test.cc │ ├── linux_input_manual_test.cc │ ├── named_type_test.cc │ ├── quaternion_test.cc │ ├── se3d_test.cc │ ├── signal_result_test.cc │ ├── sophus_test.cc │ ├── telemetry_log_registrar_test.cc │ ├── telemetry_registry_test.cc │ ├── test_main.cc │ ├── udp_manual_test.h │ └── ukf_filter_test.cc ├── timestamped_log.cc ├── timestamped_log.h ├── tokenizer.h ├── udp_data_link.cc ├── udp_data_link.h ├── udp_socket.cc ├── udp_socket.h └── ukf_filter.h ├── configs ├── BUILD ├── hoverbot.cfg └── hoverbot.ini ├── docs ├── diagrams │ ├── 1d_swing.blend │ ├── 1d_swing.png │ ├── as5047_noise_plot.py │ ├── magnetic_saturation.odg │ ├── phase_current.py │ ├── support_line.blend │ ├── support_line.png │ └── torque_compensation.py ├── geometry.md ├── numbered-targets.odp └── quada1 │ └── README.md ├── hw ├── battery_pcb │ ├── bosch_pcb-r1.zip │ ├── bosch_pcb.kicad_pcb │ ├── bosch_pcb.kicad_prl │ ├── bosch_pcb.kicad_pro │ ├── bosch_pcb.kicad_sch │ └── r1 │ │ ├── bosch_pcb-B_Cu.gbr │ │ ├── bosch_pcb-B_Mask.gbr │ │ ├── bosch_pcb-B_Paste.gbr │ │ ├── bosch_pcb-B_Silkscreen.gbr │ │ ├── bosch_pcb-Edge_Cuts.gbr │ │ ├── bosch_pcb-F_Cu.gbr │ │ ├── bosch_pcb-F_Mask.gbr │ │ ├── bosch_pcb-F_Paste.gbr │ │ ├── bosch_pcb-F_Silkscreen.gbr │ │ ├── bosch_pcb-NPTH.drl │ │ ├── bosch_pcb-PTH.drl │ │ └── bosch_pcb-job.gbrjob ├── gopro_mount.3mf ├── gopro_mount.stl ├── hoverbot_base.3mf ├── hoverbot_base.stl ├── hoverbot_base_cover.stl ├── hoverbot_battery_cover.stl ├── hoverbot_battery_rail1.stl ├── hoverbot_battery_rail2.stl ├── hoverbot_chassis.3mf ├── hoverbot_chassis.stl ├── hoverbot_chassis_cover.3mf ├── hoverbot_chassis_cover.stl ├── hoverbot_motor_mount1.stl ├── hoverbot_motor_mount2.stl └── hoverbot_rails_base_cover.3mf ├── install-packages ├── mech ├── BUILD ├── attitude_data.h ├── camera_driver.cc ├── camera_driver.h ├── control_timing.h ├── hoverbot.cc ├── hoverbot.h ├── hoverbot_command.h ├── hoverbot_config.h ├── hoverbot_context.h ├── hoverbot_control.cc ├── hoverbot_control.h ├── hoverbot_state.h ├── imu_client.h ├── imu_data.h ├── mime_type.cc ├── mime_type.h ├── moteus.h ├── moteus_tool_main.cc ├── multiplex_tool_main.cc ├── pi3hat_interface.h ├── pi3hat_wrapper.cc ├── pi3hat_wrapper.h ├── system_info.cc ├── system_info.h ├── test │ └── test_main.cc ├── trajectory.h ├── web_control.h ├── web_control_assets │ ├── index.html │ ├── js │ │ └── app.js │ ├── mjbots.png │ └── styles.css ├── web_server.cc └── web_server.h ├── tools ├── bazel ├── workspace │ ├── BUILD │ ├── bazel_deps │ │ ├── BUILD │ │ └── repository.bzl │ ├── default.bzl │ ├── generate_file.bzl │ ├── github_archive.bzl │ ├── gst-rpicamsrc │ │ ├── BUILD │ │ ├── package.BUILD │ │ └── repository.bzl │ ├── i2c-tools │ │ ├── BUILD │ │ ├── package.BUILD │ │ └── repository.bzl │ ├── implot │ │ ├── BUILD │ │ ├── empty-legend.diff │ │ ├── package.BUILD │ │ └── repository.bzl │ ├── mjlib │ │ ├── BUILD │ │ └── repository.bzl │ ├── moteus │ │ ├── BUILD │ │ └── repository.bzl │ ├── pi3hat │ │ ├── BUILD │ │ └── repository.bzl │ ├── raspberrypi-firmware │ │ ├── BUILD │ │ ├── package.BUILD │ │ └── repository.bzl │ ├── raspicam │ │ ├── BUILD │ │ ├── package.BUILD │ │ ├── raspicam.diff │ │ └── repository.bzl │ ├── rpi_bazel │ │ ├── BUILD │ │ └── repository.bzl │ ├── rules_pkg │ │ ├── BUILD │ │ └── repository.bzl │ ├── sophus │ │ ├── BUILD │ │ ├── package.BUILD │ │ └── repository.bzl │ └── template_file.bzl └── workspace_status.sh ├── travis-ci.py └── utils ├── BUILD ├── config_servos.py ├── hoverbot-start.sh ├── hoverbot.cmd ├── hoverbot_screen.conf ├── performance_governor.sh ├── rpi3 └── setup-system.py ├── ssh ├── default.ssh └── default.ssh.pub └── start-robot.sh /.bazelrc: -------------------------------------------------------------------------------- 1 | build --crosstool_top=@rpi_bazel//tools/cc_toolchain:toolchain 2 | build --define CLANG=true 3 | 4 | # Sadly, Eigen/C++ still hasn't figured out how to reliably get 5 | # alignment correct. Lets just disable it for now. 6 | build --copt -DEIGEN_DONT_ALIGN_STATICALLY 7 | 8 | build:pi --cpu=armeabihf 9 | build:pi --define COM_GITHUB_MJBOTS_RASPBERRYPI=1 10 | 11 | build --strip=never 12 | build --compiler=clang 13 | 14 | build -c opt 15 | build --copt -D_FILE_OFFSET_BITS=64 16 | build --copt -D_LARGEFILE_SOURCE 17 | build --copt -D_LARGEFILE64_SOURCE 18 | 19 | test --test_output=errors 20 | 21 | build --workspace_status_command=tools/workspace_status.sh 22 | 23 | build --stamp 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *~ 3 | core 4 | *.so 5 | *.os 6 | .scons* 7 | work_plan.pdf 8 | mtool_main_window.py 9 | *.fcstd? 10 | *.blend? 11 | build-armv7l 12 | build-x86_64 13 | mjmech-data 14 | *.l#? 15 | *.s#? 16 | *.b#? 17 | *.pro 18 | logs 19 | #* 20 | .#* 21 | _bazelrc_generated 22 | /bazel-* 23 | imgui.ini 24 | tplot2.ini 25 | *-backups/ 26 | fp-info-cache 27 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - build0 3 | - build1 4 | - build2 5 | - build3 6 | - build4 7 | - build5 8 | jobs: 9 | include: 10 | - stage: build0 11 | script: ./travis-ci.py 0 12 | - stage: build1 13 | script: ./travis-ci.py 1 14 | - stage: build2 15 | script: ./travis-ci.py 2 16 | - stage: build3 17 | script: ./travis-ci.py 3 18 | - stage: build4 19 | script: ./travis-ci.py 4 20 | - stage: build5 21 | script: ./travis-ci.py 5 && rm -rf $HOME/.cache/bazel 22 | dist: bionic 23 | language: cpp 24 | cache: 25 | directories: 26 | - $HOME/.cache/bazel/ 27 | -------------------------------------------------------------------------------- /BOM.md: -------------------------------------------------------------------------------- 1 | # Hoverbot BOM 2 | 3 | * 3D printed parts (all designed for Prusament PETG with a Prusa MK3s and 0.6mm nozzle) 4 | * [Base](hw/hoverbot_base.3mf) 5 | * [Chassis](hw/hoverbot_chassis.3mf) 6 | * [Chassis cover](hw/hoverbot_chassis_cover.3mf) 7 | * [Base cover, battery rails, and battery cover](hw/hoverbot_rails_base_cover.3mf) 8 | * Electronics 9 | * 2x [moteus-c1](https://mjbots.com/products/moteus-c1) 10 | * [pi3hat](https://mjbots.com/products/mjbots-pi3hat-r4-5) 11 | * [power_dist](https://mjbots.com/products/mjbots-power-dist-r4-5b) 12 | * [Raspberry Pi 4 / 2GB](https://www.raspberrypi.com/products/raspberry-pi-4-model-b/) 13 | * [64GB Micro SD Card](https://www.amazon.com/gp/product/B09X7C7LL1) 14 | * Connectors and cabling 15 | * 3x [30cm PH3 cable](https://mjbots.com/products/jst-ph3-cable) 16 | * 3x XT30 power cables, each with 17 | * 1x [XT30 Male](https://mjbots.com/products/xt30u-m) 18 | * 1x [XT30 Female](https://mjbots.com/products/xt30u-f) 19 | * ~30cm [18 AWG silicone wire](https://www.amazon.com/gp/product/B01KCPL3Z8) 20 | * heat shrink tubing 21 | * 2x motor wiring, each with 22 | * 1x [MR30 connector pairs](https://www.amazon.com/gp/product/B0747MW9RX) 23 | * 1x [GH7 housings](https://mjbots.com/products/jst-gh7-housing) 24 | * 5x [GH pre-crimped wires](https://mjbots.com/products/jst-gh-wire) 25 | * small heat shrink tubing 26 | * Power cable 27 | * [XT90S](https://www.amazon.com/gp/product/B0893CGLP7) 28 | * ~20cm [16 AWG silicone wire](https://www.amazon.com/gp/product/B01MCRY7OT) 29 | * Battery mount 30 | * 2x [M3x4 bolt / 91292A109](https://www.mcmaster.com/91292A109/) 31 | * [Battery PCB](hw/battery_pcb/) 32 | * 2x [Battery Terminal / 1289](https://www.digikey.com/en/products/detail/keystone-electronics/1289) 33 | * Hardware 34 | * 28x [M3 heat set inserts / 94180A331](https://www.mcmaster.com/94180A331/) 35 | * 18x in base 36 | * 10x in chassis 37 | * 8x [M2.5 heat set inserts / 94180A321](https://www.mcmaster.com/94180A321/) 38 | * 8x in chassis 39 | * 16x [M2.5x5 bolts / 91292A009](https://www.mcmaster.com/91292A009/) 40 | * 8x for moteus-c1 41 | * 4x for power_dist 42 | * 4x for pi3hat 43 | * 26x [M3x8 bolt 91292A112](https://www.mcmaster.com/91292A112/) 44 | * 8x for motors 45 | * 4x for base cover 46 | * 4x for chassis cover 47 | * 4x for chassis 48 | * 6x for battery mounts 49 | -------------------------------------------------------------------------------- /BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | config_setting( 20 | name = "raspberrypi", 21 | values = { 22 | "cpu" : "armeabihf", 23 | }, 24 | ) 25 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2015, Josh Pieper jjp@pobox.com, Mikhail Afanasyev 2 | mafanasyev@gmail.com 3 | 4 | Licensed under the Apache License, Version 2.0 (the "License"); 5 | you may not use this file except in compliance with the License. 6 | You may obtain a copy of the License at 7 | 8 | http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | Unless required by applicable law or agreed to in writing, software 11 | distributed under the License is distributed on an "AS IS" BASIS, 12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | See the License for the specific language governing permissions and 14 | limitations under the License. 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mjbots hoverbot 2 | =============== 3 | 4 | Source and design files for the mjbots hoverbot, its controlling 5 | interfaces, and utilities for developing and operating it. 6 | 7 | * GitHub https://github.com/mjbots/hoverbot 8 | * Most files are free hardware and software: Apache 2.0 License 9 | * F360 assembly: https://a360.co/3Fpwtx1 10 | * [BOM](BOM.md) 11 | 12 | Directory structure 13 | ------------------- 14 | 15 | * **base/** - C++ source files common to many applications. 16 | * **mech/** - C++ source files specific to walking robots. 17 | * **utils/** - Utilities for development and data analysis. 18 | * **configs/** - Configuration files for different robots and applications. 19 | * **hw/** - Hardware design files 20 | * **docs/** - Documentation. 21 | 22 | 23 | First Time Setup 24 | ---------------- 25 | 26 | The following should work on Ubuntu 22.04 27 | 28 | ``` 29 | ./install-packages 30 | ``` 31 | 32 | Building for host 33 | ----------------- 34 | 35 | ``` 36 | tools/bazel test //... 37 | ``` 38 | 39 | Building for the target 40 | ----------------------- 41 | 42 | ``` 43 | tools/bazel test --config=pi //mech:hoverbot_deploy.tar 44 | ``` 45 | -------------------------------------------------------------------------------- /WORKSPACE: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | workspace(name = "com_github_mjbots_mech") 18 | 19 | BAZEL_VERSION = "5.4.1" 20 | BAZEL_VERSION_SHA = "5d90515f84b5ee1fd6ec22ee9e83103e77ed1a907ee5eec198fef3a5b45abf13" 21 | 22 | load("//tools/workspace:default.bzl", "add_default_repositories") 23 | 24 | add_default_repositories() 25 | 26 | load("@rpi_bazel//tools/workspace:default.bzl", 27 | rpi_bazel_add = "add_default_repositories") 28 | rpi_bazel_add() 29 | 30 | load("@com_github_mjbots_bazel_deps//tools/workspace:default.bzl", 31 | bazel_deps_add = "add_default_repositories") 32 | bazel_deps_add() 33 | 34 | load("@com_github_mjbots_mjlib//tools/workspace:default.bzl", 35 | mjlib_add = "add_default_repositories") 36 | mjlib_add() 37 | 38 | load("@moteus//tools/workspace:default.bzl", 39 | moteus_add = "add_default_repositories") 40 | moteus_add() 41 | -------------------------------------------------------------------------------- /base/aspect_ratio.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/aspect_ratio.h" 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | Eigen::AlignedBox2i MaintainAspectRatio(Eigen::Vector2i source, 21 | Eigen::Vector2i dest) { 22 | int x = 0; 23 | int y = 0; 24 | int display_w = dest.x(); 25 | int display_h = dest.y(); 26 | 27 | // Enforce an aspect ratio. 28 | const double desired_aspect_ratio = 29 | static_cast(std::abs(source.x())) / 30 | static_cast(std::abs(source.y())); 31 | const double actual_ratio = 32 | static_cast(display_w) / 33 | static_cast(display_h); 34 | if (actual_ratio > desired_aspect_ratio) { 35 | const int w = display_h * desired_aspect_ratio; 36 | const int remaining = display_w - w; 37 | x = remaining / 2; 38 | display_w = w; 39 | } else if (actual_ratio < desired_aspect_ratio) { 40 | const int h = display_w / desired_aspect_ratio; 41 | const int remaining = display_h - h; 42 | y = remaining / 2; 43 | display_h = h; 44 | } 45 | 46 | return { 47 | Eigen::Vector2i(x, y), 48 | Eigen::Vector2i(x + display_w, y + display_h) 49 | }; 50 | } 51 | 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /base/aspect_ratio.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace mjmech { 21 | namespace base { 22 | 23 | /// Given a source image and a destination region, return a draw 24 | /// location that maximizes the usable size while maintaining the 25 | /// aspect ratio of the source. 26 | Eigen::AlignedBox2i MaintainAspectRatio(Eigen::Vector2i source, 27 | Eigen::Vector2i dest); 28 | 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /base/async_i2c.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "mjlib/io/async_types.h" 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | class AsyncI2C : boost::noncopyable { 25 | public: 26 | virtual ~AsyncI2C() {} 27 | virtual boost::asio::any_io_executor get_executor() = 0; 28 | 29 | virtual void AsyncRead( 30 | uint8_t device, uint8_t address, 31 | mjlib::io::MutableBufferSequence buffers, mjlib::io::ReadHandler) = 0; 32 | 33 | virtual void AsyncWrite( 34 | uint8_t device, uint8_t address, 35 | mjlib::io::ConstBufferSequence buffers, mjlib::io::WriteHandler) = 0; 36 | }; 37 | 38 | typedef std::shared_ptr SharedI2C; 39 | typedef std::function SharedI2CHandler; 40 | 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /base/bezier.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | /// Interpolate over a cubic bezier function with fixed control points 21 | /// such that velocity is 0 at start and end and the acceleration is 22 | /// continuous within that range. 23 | template 24 | class Bezier { 25 | public: 26 | Bezier(T start, T end) 27 | : start_(start), 28 | end_(end) {} 29 | 30 | T position(double phase) const { 31 | const double bezier = 32 | phase * phase * phase + 3.0 * (phase * phase * (1.0 - phase)); 33 | return start_ + bezier * delta_; 34 | } 35 | 36 | T velocity(double phase) const { 37 | const double bezier = 6 * phase * (1.0 - phase); 38 | return bezier * delta_; 39 | } 40 | 41 | T acceleration(double phase) const { 42 | const double bezier = 6 - 12 * phase; 43 | return bezier * delta_; 44 | } 45 | 46 | private: 47 | T start_; 48 | T end_; 49 | T delta_{end_ - start_}; 50 | }; 51 | 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /base/circular_buffer.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | /// A dumb circular buffer which is based on a std::vector. Thus it 20 | /// has the property that in steady state no allocations are required, 21 | /// unlike std::list, but actually works with move-only objects, 22 | /// unlike boost::circular buffer as of boost 1.55. 23 | template 24 | class circular_buffer { 25 | public: 26 | circular_buffer() { data_.resize(2); } 27 | 28 | void push_back(T&& value) { 29 | if (full()) { resize(data_.size() * 2); } 30 | data_[insert_] = std::move(value); 31 | insert_ = (insert_ + 1) % data_.size(); 32 | } 33 | 34 | void pop_front() { 35 | remove_ = (remove_ + 1) % data_.size(); 36 | } 37 | 38 | T& front() { return data_[remove_]; } 39 | const T& front() const { return data_[remove_]; } 40 | 41 | T& back() { return data_[insert_]; } 42 | const T& back() const { return data_[insert_]; } 43 | 44 | bool empty() const { return insert_ == remove_; } 45 | bool full() const { 46 | return ((insert_ + 1) % data_.size()) == remove_; 47 | } 48 | 49 | size_t capacity() const { return data_.size() - 1; } 50 | 51 | private: 52 | void resize(size_t size) { 53 | std::vector new_data(size); 54 | size_t new_offset = 0; 55 | while (!empty()) { 56 | new_data[new_offset] = std::move(front()); 57 | pop_front(); 58 | new_offset++; 59 | } 60 | data_.swap(new_data); 61 | remove_ = 0; 62 | insert_ = new_offset; 63 | } 64 | 65 | std::vector data_; 66 | size_t insert_ = 0; 67 | size_t remove_ = 0; 68 | size_t size_ = 0; 69 | }; 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /base/context.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "context_full.h" 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | Context::Context() 21 | : telemetry_log(std::make_unique([]() { 22 | mjlib::telemetry::FileWriter::Options options; 23 | options.blocking = false; 24 | return options; 25 | }())), 26 | remote_debug(std::make_unique(executor)), 27 | telemetry_registry(std::make_unique( 28 | context, telemetry_log.get(), remote_debug.get())), 29 | factory(std::make_unique(executor)) 30 | { 31 | } 32 | 33 | Context::~Context() {} 34 | 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /base/context.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "mjlib/io/realtime_executor.h" 24 | #include "mjlib/io/stream_factory.h" 25 | #include "mjlib/telemetry/file_writer.h" 26 | 27 | namespace mjmech { 28 | namespace base { 29 | 30 | class TelemetryRemoteDebugServer; 31 | class TelemetryRegistry; 32 | 33 | struct Context : boost::noncopyable { 34 | Context(); 35 | ~Context(); 36 | 37 | boost::asio::io_context context; 38 | mjlib::io::RealtimeExecutor rt_executor{context.get_executor()}; 39 | boost::asio::any_io_executor executor{rt_executor}; 40 | std::unique_ptr telemetry_log; 41 | std::unique_ptr remote_debug; 42 | std::unique_ptr telemetry_registry; 43 | std::unique_ptr factory; 44 | }; 45 | 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /base/context_full.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // Copyright 2015-2016 Mikhail Afanasyev. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #pragma once 17 | 18 | #include "base/context.h" 19 | 20 | #include "mjlib/io/stream_factory.h" 21 | 22 | #include "telemetry_registry.h" 23 | #include "telemetry_remote_debug_server.h" 24 | -------------------------------------------------------------------------------- /base/error_wrap.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2018 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include "stringify.h" 25 | 26 | namespace mjmech { 27 | namespace base { 28 | 29 | /// The following routine can be used to wrap coroutines such that 30 | /// boost::system_error information is captured. The default 31 | /// boost::exception_ptr ignores this exception, making it challenging 32 | /// to even report what happened. 33 | template 34 | auto ErrorWrap(Coroutine coro) { 35 | return [=](boost::asio::yield_context yield) { 36 | try { 37 | return coro(yield); 38 | } catch (boost::system::system_error& e) { 39 | std::throw_with_nested( 40 | std::runtime_error( 41 | fmt::format("system_error: {}: {}", 42 | e.what(), Stringify(e.code())))); 43 | } catch (std::runtime_error& e) { 44 | std::throw_with_nested(std::runtime_error(e.what())); 45 | } 46 | }; 47 | } 48 | 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /base/euler.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2016 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "mjlib/base/visitor.h" 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | /// Euler angles are in roll, pitch, then yaw. 23 | /// +roll -> right side down 24 | /// +pitch -> forward edge up 25 | /// +yaw -> clockwise looking down 26 | struct Euler { 27 | double roll = 0.0; 28 | double pitch = 0.0; 29 | double yaw = 0.0; 30 | 31 | template 32 | void Serialize(Archive* a) { 33 | a->Visit(MJ_NVP(roll)); 34 | a->Visit(MJ_NVP(pitch)); 35 | a->Visit(MJ_NVP(yaw)); 36 | } 37 | }; 38 | 39 | inline Euler operator*(const Euler& lhs, double s) { 40 | return Euler{lhs.roll * s, lhs.pitch * s, lhs.yaw * s}; 41 | } 42 | 43 | inline Euler operator*(double s, const Euler& lhs) { 44 | return Euler{lhs.roll * s, lhs.pitch * s, lhs.yaw * s}; 45 | } 46 | 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /base/fail_functor.h: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "mjlib/base/error_code.h" 18 | #include "mjlib/base/fail.h" 19 | 20 | namespace mjmech { 21 | namespace base { 22 | 23 | class FailFunctor { 24 | public: 25 | template 26 | void operator()(const mjlib::base::error_code& ec, Args...) { 27 | mjlib::base::FailIf(ec); 28 | } 29 | }; 30 | 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /base/fit_plane.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/fit_plane.h" 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | Plane FitPlane(const std::vector& points) { 23 | Eigen::MatrixXd A(points.size(), 3); 24 | Eigen::MatrixXd B(points.size(), 1); 25 | 26 | for (size_t i = 0; i < points.size(); i++) { 27 | A(i, 0) = points[i].x(); 28 | A(i, 1) = points[i].y(); 29 | A(i, 2) = 1.0; 30 | B(i) = points[i].z(); 31 | } 32 | 33 | Eigen::MatrixXd result = A.bdcSvd( 34 | Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B); 35 | return Plane{result(0), result(1), result(2)}; 36 | } 37 | 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /base/fit_plane.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | /// For a plane defined as a*x + b*y + c = z 25 | /// 26 | /// Yes, this only works for planes that are mostly level. 27 | struct Plane { 28 | double a = 0.0; 29 | double b = 0.0; 30 | double c = 0.0; 31 | }; 32 | 33 | Plane FitPlane(const std::vector& points); 34 | 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /base/format_hex.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/format_hex.h" 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | std::string FormatHex(std::string_view data) { 25 | std::ostringstream ostr; 26 | for (char c : data) { 27 | ostr << fmt::format("{:02x}", static_cast(static_cast(c))); 28 | } 29 | return ostr.str(); 30 | } 31 | 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /base/format_hex.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace mjmech { 21 | namespace base { 22 | 23 | std::string FormatHex(std::string_view); 24 | 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /base/git_info.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/git_info.h" 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | namespace { 23 | 24 | uint8_t ParseHexNibble(uint8_t c) { 25 | if (c >= '0' && c <= '9') { return c - '0'; } 26 | if (c >= 'A' && c <= 'F') { return c - 'A' + 10; } 27 | if (c >= 'a' && c <= 'f') { return c - 'a' + 10; } 28 | return 0; 29 | } 30 | 31 | uint8_t ParseHexByte(const char* data) { 32 | return (ParseHexNibble(data[0]) << 4) | ParseHexNibble(data[1]); 33 | } 34 | 35 | } 36 | 37 | GitInfo::GitInfo() { 38 | if (std::strlen(kGitHash) != 40) { 39 | dirty = true; 40 | } else { 41 | for (size_t i = 0; i <= 20; i++) { 42 | hash[i] = ParseHexByte(&kGitHash[i * 2]); 43 | } 44 | 45 | dirty = kGitDirty[0] != '0'; 46 | } 47 | } 48 | 49 | char kGitHash[41] __attribute__((weak)) = {}; 50 | char kGitDirty[10] __attribute__((weak)) = {}; 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /base/git_info.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include "mjlib/base/visitor.h" 21 | 22 | namespace mjmech { 23 | namespace base { 24 | 25 | struct GitInfo { 26 | GitInfo(); 27 | 28 | std::array hash = {{}}; 29 | bool dirty = false; 30 | 31 | template 32 | void Serialize(Archive* a) { 33 | a->Visit(MJ_NVP(hash)); 34 | a->Visit(MJ_NVP(dirty)); 35 | } 36 | }; 37 | 38 | extern char kGitHash[41]; 39 | extern char kGitDirty[10]; 40 | 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /base/git_info_linkstamp.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | namespace mjmech { 16 | namespace base { 17 | 18 | char kGitHash[41] = BUILD_SCM_REVISION; 19 | char kGitDirty[10] = BUILD_SCM_STATUS; 20 | 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /base/handler_util.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include "mjlib/base/error_code.h" 21 | #include "mjlib/io/async_types.h" 22 | 23 | #include "logging.h" 24 | 25 | namespace mjmech { 26 | namespace base { 27 | /// Join one or more ErrorHandler callbacks into a single callback. 28 | /// The first failure will cause the target to be emitted, and it will 29 | /// be called with success after all registered handlers are called. 30 | class ErrorHandlerJoiner 31 | : public std::enable_shared_from_this { 32 | public: 33 | ErrorHandlerJoiner(mjlib::io::ErrorCallback handler) 34 | : handler_(std::move(handler)), 35 | log_(GetUniqueLogInstance("joiner")) { 36 | } 37 | 38 | mjlib::io::ErrorCallback Wrap(const std::string& message) { 39 | BOOST_ASSERT(!done_); 40 | outstanding_.push_front(true); 41 | log_.debugStream() 42 | << "Scheduling [" << message << "], " 43 | << outstanding_.size() << " pending"; 44 | auto it = outstanding_.begin(); 45 | auto me = shared_from_this(); 46 | return [me, it, message](mjlib::base::error_code ec) { 47 | if (me->done_) { 48 | me->log_.warnStream() 49 | << "Complete [" << message << "]: result " << (!!ec) 50 | << ", but we are already done"; 51 | // Another callback already finished early, eat this handler 52 | // and return. 53 | return; 54 | } 55 | 56 | me->outstanding_.erase(it); 57 | me->log_.debugStream() 58 | << "Complete [" << message << "]: result " << (!!ec) 59 | << ", " << me->outstanding_.size() << " pending"; 60 | if (ec || me->outstanding_.empty()) { 61 | ec.Append(message); 62 | me->log_.debug("All complete"); 63 | me->handler_(ec); 64 | me->done_ = true; 65 | } 66 | }; 67 | } 68 | 69 | bool done_ = false; 70 | mjlib::io::ErrorCallback handler_; 71 | std::list outstanding_; 72 | LogRef log_; 73 | }; 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /base/interpolate.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | template 21 | T Interpolate(T start, T end, double scale) { 22 | return (end - start) * scale + start; 23 | } 24 | 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /base/kinematic_relation.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | /// Describes the kinematic relation between two rigid body frames. 21 | struct KinematicRelation { 22 | Sophus::SE3d pose; // Absolute position and orientation 23 | base::Point3D v; // velocity 24 | base::Point3D w; // angular rate 25 | 26 | template 27 | void Serialize(Archive* a) { 28 | a->Visit(MJ_NVP(pose)); 29 | a->Visit(MJ_NVP(v)); 30 | a->Visit(MJ_NVP(w)); 31 | } 32 | }; 33 | 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /base/leg_force.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/leg_force.h" 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "mjlib/base/assert.h" 23 | #include "mjlib/base/system_error.h" 24 | 25 | namespace mjmech { 26 | namespace base { 27 | 28 | namespace { 29 | struct LegFunctor : public Eigen::DenseFunctor { 30 | LegFunctor(const std::vector& legs) 31 | : Eigen::DenseFunctor(legs.size(), 5 + legs.size()), 32 | legs_(legs) {} 33 | 34 | int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec) const { 35 | MJ_ASSERT(fvec.size() == 5 + legs_.size()); 36 | MJ_ASSERT(x.size() == legs_.size()); 37 | fvec(0) = 0.0; 38 | fvec(1) = 0.0; 39 | fvec(2) = -1.0; // This is the total sum of all legs. 40 | fvec(3) = 0.0; 41 | fvec(4) = 0.0; 42 | 43 | for (size_t i = 0; i < legs_.size(); i++) { 44 | fvec(0) += legs_[i].x() * x(i); 45 | fvec(1) += 10 * legs_[i].y() * x(i); 46 | fvec(2) += x(i); 47 | if (x(i) < 0.0) { fvec(3) = x(i); } 48 | if (x(i) > 1.0) { fvec(4) = x(i); } 49 | 50 | fvec(5 + i) = balance_ratio_ * (x(i) - (1.0 / legs_.size())); 51 | } 52 | 53 | fvec(2) *= 1e6; 54 | 55 | return 0; 56 | } 57 | 58 | std::vector legs_; 59 | // Just some small value. 60 | const double balance_ratio_ = 0.001; 61 | }; 62 | } 63 | 64 | std::vector OptimizeLegForce(const std::vector& legs) { 65 | if (legs.size() == 0) { return {}; } 66 | if (legs.size() == 1) { return { 1.0 }; } 67 | 68 | Eigen::VectorXd solution(legs.size()); 69 | 70 | LegFunctor lf{legs}; 71 | Eigen::NumericalDiff nf{lf}; 72 | Eigen::LevenbergMarquardt> lm{nf}; 73 | lm.minimize(solution); 74 | 75 | std::vector result; 76 | for (size_t i = 0; i < legs.size(); i++) { 77 | result.push_back(solution(i)); 78 | } 79 | 80 | return result; 81 | } 82 | 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /base/leg_force.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | /// Given the leg X/Y positions in the M frame, return a ratio of 25 | /// force to apply which minimizes the amount of angular acceleration 26 | /// incurred. 27 | std::vector OptimizeLegForce(const std::vector&); 28 | 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /base/linux_input.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "mjlib/io/async_types.h" 23 | 24 | namespace mjmech { 25 | namespace base { 26 | /// Support reading events from a linux input device. 27 | class LinuxInput : boost::noncopyable { 28 | public: 29 | explicit LinuxInput(const boost::asio::any_io_executor&); 30 | LinuxInput(const boost::asio::any_io_executor&, const std::string& device); 31 | void Open(const std::string& device); 32 | 33 | ~LinuxInput(); 34 | 35 | boost::asio::any_io_executor get_executor(); 36 | 37 | /// @return the underlying file descriptor for this device. 38 | int fileno() const; 39 | 40 | /// @return the human readable name the kernel provides for this 41 | /// device 42 | std::string name() const; 43 | 44 | struct AbsInfo { 45 | int axis = -1; 46 | int value = 0; 47 | int minimum = 0; 48 | int maximum = 0; 49 | int fuzz = 0; 50 | int flat = 0; 51 | int resolution = 0; 52 | 53 | double scaled() const { 54 | double center = 0.5 * (minimum + maximum); 55 | return (value - center) / (0.5 * (maximum - minimum)); 56 | } 57 | }; 58 | 59 | /// @return the absolute value associated with a given joystick axis. 60 | /// 61 | /// @param axis is one of the ABS_* defines from linux/input.h 62 | AbsInfo abs_info(int axis) const; 63 | 64 | struct Features { 65 | int ev_type = -1; 66 | 67 | /// This bitset contains bits set as defined in the relevant type 68 | /// specific defines. 69 | boost::dynamic_bitset<> capabilities; 70 | }; 71 | 72 | /// @return an object describing the capabilities of this input 73 | /// device 74 | /// 75 | /// @param ev_type should be one of the EV_* defines. 76 | Features features(int ev_type) const; 77 | 78 | struct Event { 79 | boost::posix_time::ptime time; 80 | int ev_type = 0; 81 | int code = 0; 82 | int value = 0; 83 | }; 84 | 85 | /// Read one event asynchronously. This function returns 86 | /// immediately, @p handler will be invoked using boost::asio::post 87 | /// and @p event must be valid until the handler is invoked. 88 | void AsyncRead(Event* event, mjlib::io::ErrorCallback handler); 89 | 90 | /// Cancel all asynchronous operations associated with this device. 91 | void cancel(); 92 | 93 | private: 94 | class Impl; 95 | std::unique_ptr impl_; 96 | }; 97 | 98 | // Overloads for diagnostic output. 99 | std::ostream& operator<<(std::ostream&, const LinuxInput&); 100 | std::ostream& operator<<(std::ostream&, const LinuxInput::AbsInfo&); 101 | std::ostream& operator<<(std::ostream&, const LinuxInput::Features&); 102 | std::ostream& operator<<(std::ostream&, const LinuxInput::Event&); 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /base/logging.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // Copyright 2015 Mikhail Afanasyev. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #pragma once 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include "mjlib/base/visitor.h" 26 | 27 | namespace mjmech { 28 | namespace base { 29 | 30 | clipp::group MakeLoggingOptions(); 31 | 32 | // Call after program_options has been notified, or if you do not support 33 | // program options. 34 | void InitLogging(); 35 | 36 | typedef log4cpp::Category& LogRef; 37 | // Get a LogRef with a given name. 38 | // - there is a .-separated hierachy -- '-t cd' will enable 'cd.stats' logger 39 | // - set the first element to C++ file name (lowercase, _-separated) 40 | LogRef GetLogInstance(const std::string& name); 41 | 42 | // Same as GetLogInstance, but appends a unique suffix to a name. 43 | LogRef GetUniqueLogInstance(const std::string& name); 44 | 45 | LogRef GetSubLogger(LogRef parent, const std::string& name); 46 | 47 | 48 | //class TelemetryRegistry; 49 | //void WriteLogToTelemetryLog(TelemetryRegistry*); 50 | 51 | 52 | // TODO theamk: ideally, all of the stuff below should be hidden, and a single 53 | // function (with signature above) should be exposed. However, 54 | // TelemetryRegsitry is way too templated for this to work, so we have to 55 | // expose much more than we wanted to. 56 | struct TextLogMessage { 57 | boost::posix_time::ptime timestamp; 58 | std::string thread; 59 | std::string priority; 60 | int priority_int; 61 | std::string ndc; 62 | std::string category; 63 | std::string message; 64 | 65 | template 66 | void Serialize(Archive* a) { 67 | a->Visit(MJ_NVP(timestamp)); 68 | a->Visit(MJ_NVP(thread)); 69 | a->Visit(MJ_NVP(priority)); 70 | a->Visit(MJ_NVP(priority_int)); 71 | a->Visit(MJ_NVP(ndc)); 72 | a->Visit(MJ_NVP(category)); 73 | a->Visit(MJ_NVP(message)); 74 | } 75 | }; 76 | typedef boost::signals2::signal TextLogMessageSignal; 78 | 79 | TextLogMessageSignal* GetLogMessageSignal(); 80 | 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /base/module_main.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2014-2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | def module_main(name, prefix, cname, deps): 18 | native.genrule( 19 | name = "{}_main".format(name), 20 | outs = ["{}_main.cc".format(name)], 21 | cmd = """cat > $(location {name}_main.cc) << EOF 22 | #include "{prefix}/{name}.h" 23 | 24 | #include "base/module_main.h" 25 | 26 | extern "C" {{ 27 | int main(int argc, char**argv) {{ 28 | return mjmech::base::main<{cname}>(argc, argv); 29 | }} 30 | }} 31 | EOF 32 | """.format(name=name, cname=cname, prefix=prefix), 33 | ) 34 | 35 | native.cc_binary( 36 | name = "{}".format(name), 37 | srcs = [ 38 | "{}_main.cc".format(name), 39 | name + ".h", 40 | ], 41 | deps = deps + [ 42 | "@boost//:filesystem", 43 | "@boost//:date_time", 44 | "@org_llvm_libcxx//:libcxx", 45 | ], 46 | ) 47 | -------------------------------------------------------------------------------- /base/named_type.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | template 23 | class NamedType 24 | { 25 | public: 26 | explicit NamedType(T const& value) : value_(value) {} 27 | explicit NamedType(T&& value) : value_(std::move(value)) {} 28 | T& get() { return value_; } 29 | T const& get() const {return value_; } 30 | private: 31 | T value_; 32 | }; 33 | 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /base/point3d.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "mjlib/base/eigen.h" 22 | 23 | #include "base/common.h" 24 | 25 | namespace mjmech { 26 | namespace base { 27 | 28 | using Point3D = Eigen::Vector3d; 29 | 30 | inline double Point3DHeadingDeg(const Point3D& p) { 31 | return Degrees(WrapNegPiToPi(0.5 * M_PI - std::atan2(p.y(), p.x()))); 32 | } 33 | 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /base/quaternion.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2015 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "quaternion.h" 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | std::string Quaternion::str() const { 23 | return fmt::format("%f %f %f %f", w_, x_, y_, z_); 24 | } 25 | 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /base/reservoir_sampler.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | /// Recursively estimate a sample of a large data set. 23 | /// 24 | /// The algorithm used here is "R" from "Random Sampling with a 25 | /// Reservoir", J. Vitter. 26 | template 27 | class ReservoirSampler { 28 | public: 29 | ReservoirSampler(std::size_t size) 30 | : size_(size) {} 31 | 32 | using Container = std::vector; 33 | using iterator = Container::iterator; 34 | using const_iterator = Container::const_iterator; 35 | 36 | void Add(T value) { 37 | count_++; 38 | 39 | if (samples_.size() < size_) { 40 | samples.push_back(std::move(value)); 41 | } else { 42 | const std::size_t M = static_cast( 43 | std::uniform_int<>(0, count_ - 1)(rng_)); 44 | if (M < size_) { 45 | samples_[M] = value; 46 | } 47 | } 48 | } 49 | 50 | iterator begin() { return samples_.begin(); } 51 | iterator end() { return samples_.end(); } 52 | 53 | const_iterator begin() const { return samples_.begin(); } 54 | const_iterator end() const { return samples_.end(); } 55 | 56 | private: 57 | Container samples_; 58 | std::size_t size_; 59 | 60 | std::mt19937 rng_; 61 | std::size_t count_ = 0; 62 | } 63 | 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /base/runfiles.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include "tools/cpp/runfiles/runfiles.h" 23 | 24 | #include "mjlib/base/system_error.h" 25 | 26 | namespace mjmech { 27 | namespace base { 28 | class Runfiles { 29 | public: 30 | Runfiles() { 31 | std::string error; 32 | runfiles_.reset( 33 | bazel::tools::cpp::runfiles::Runfiles::Create( 34 | boost::filesystem::canonical("/proc/self/exe").native(), &error)); 35 | if (!runfiles_) { 36 | throw mjlib::base::system_error::einval("Error creating runfiles: " + error); 37 | } 38 | } 39 | 40 | std::string Rlocation(std::string_view path) const { 41 | return runfiles_->Rlocation("com_github_mjbots_mech/" + std::string(path)); 42 | } 43 | 44 | private: 45 | std::unique_ptr runfiles_; 46 | }; 47 | 48 | class TestRunfiles { 49 | public: 50 | TestRunfiles() { 51 | std::string error; 52 | runfiles_.reset(bazel::tools::cpp::runfiles::Runfiles::CreateForTest(&error)); 53 | if (!runfiles_) { 54 | throw mjlib::base::system_error::einval("Error creating runfiles: " + error); 55 | } 56 | } 57 | 58 | std::string Rlocation(std::string_view path) const { 59 | return runfiles_->Rlocation("com_github_mjbots_mech/" + std::string(path)); 60 | } 61 | 62 | private: 63 | std::unique_ptr runfiles_; 64 | }; 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /base/saturate.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | template 21 | T Saturate(InType value) { 22 | if (value >= static_cast(std::numeric_limits::max())) { 23 | return std::numeric_limits::max(); 24 | } 25 | if (value <= static_cast(std::numeric_limits::min())) { 26 | return std::numeric_limits::min(); 27 | } 28 | return static_cast(value); 29 | } 30 | 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /base/signal_result.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "mjlib/io/deadline_timer.h" 22 | 23 | #include "common.h" 24 | 25 | namespace mjmech { 26 | namespace base { 27 | class TimeoutError : public std::runtime_error, public boost::exception { 28 | public: 29 | TimeoutError() : std::runtime_error("timeout") {} 30 | virtual ~TimeoutError() {} 31 | }; 32 | 33 | /// This class presents a future which is satisfied when the given 34 | /// signal emits, or when a timeout occurs, whichever occurs first. 35 | class SignalResult : boost::noncopyable { 36 | public: 37 | 38 | template 39 | static void Wait(const boost::asio::any_io_executor& executor, 40 | boost::signals2::signal* signal, 41 | double timeout_s, 42 | Handler handler) { 43 | struct Context { 44 | mjlib::io::DeadlineTimer timer; 45 | bool active = true; 46 | boost::signals2::connection connection; 47 | Handler handler; 48 | 49 | Context(const boost::asio::any_io_executor& executor, Handler handler_in) 50 | : timer(executor), 51 | handler(std::move(handler_in)) {} 52 | }; 53 | 54 | // TODO jpieper: It would be nice to implement this in a way that 55 | // didn't require a shared_ptr heap allocation for every wait. 56 | // However, the fact that asio's timer's have a two-stage enqueue, 57 | // then call semantics, means that it isn't possibly to reliably 58 | // cancel one. Since this coroutine may go out of scope entirely, 59 | // along with its stack by the time the canceled timer callback is 60 | // invoked, this is the only way I can think of to ensure it 61 | // doesn't access anything it shouldn't. 62 | std::shared_ptr context(new Context(executor, std::move(handler))); 63 | 64 | context->timer.expires_from_now(ConvertSecondsToDuration(timeout_s)); 65 | context->timer.async_wait( 66 | [context](boost::system::error_code ec) mutable -> void { 67 | if (ec == boost::asio::error::operation_aborted) { return; } 68 | if (!context->active) { return; } 69 | context->active = false; 70 | context->connection.disconnect(); 71 | T result{}; 72 | context->handler(boost::asio::error::operation_aborted, result); 73 | }); 74 | 75 | context->connection = 76 | signal->connect([context](const T* value) mutable -> void { 77 | if (!context->active) { return; } 78 | context->active = false; 79 | context->connection.disconnect(); 80 | context->handler(boost::system::error_code(), *value); 81 | }); 82 | } 83 | }; 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /base/sophus.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "mjlib/base/eigen.h" 20 | #include "mjlib/base/visitor.h" 21 | 22 | namespace mjlib { 23 | namespace base { 24 | 25 | template 26 | struct ExternalSerializer> { 27 | struct Wrapper { 28 | Wrapper(Sophus::SO3* wrapped = nullptr) 29 | : wrapped_(wrapped ? wrapped : &g_static_wrapped_) {} 30 | 31 | template 32 | void Serialize(Archive* a) { 33 | auto* data = wrapped_->data(); 34 | a->Visit(mjlib::base::MakeNameValuePair(&data[0], "x")); 35 | a->Visit(mjlib::base::MakeNameValuePair(&data[1], "y")); 36 | a->Visit(mjlib::base::MakeNameValuePair(&data[2], "z")); 37 | a->Visit(mjlib::base::MakeNameValuePair(&data[3], "w")); 38 | } 39 | 40 | Sophus::SO3* wrapped_; 41 | static inline Sophus::SO3 g_static_wrapped_; 42 | }; 43 | 44 | template 45 | void Serialize(Sophus::SO3* value, PairReceiver receiver) { 46 | Wrapper wrapper(value); 47 | receiver(mjlib::base::MakeNameValuePair(&wrapper, "")); 48 | } 49 | }; 50 | 51 | template 52 | struct ExternalSerializer> { 53 | struct Wrapper { 54 | Wrapper(Sophus::SE3* wrapped = nullptr) 55 | : wrapped_(wrapped ? wrapped : &g_static_wrapped_) {} 56 | 57 | template 58 | void Serialize(Archive* a) { 59 | a->Visit(mjlib::base::MakeNameValuePair( 60 | &wrapped_->so3(), "so3")); 61 | a->Visit(mjlib::base::MakeNameValuePair( 62 | &wrapped_->translation(), "translation")); 63 | } 64 | 65 | Sophus::SE3* wrapped_; 66 | static inline Sophus::SE3 g_static_wrapped_; 67 | }; 68 | 69 | template 70 | void Serialize(Sophus::SE3* value, PairReceiver receiver) { 71 | Wrapper wrapper(value); 72 | receiver(mjlib::base::MakeNameValuePair(&wrapper, "")); 73 | } 74 | }; 75 | 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /base/stringify.h: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2018 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace mjmech { 21 | namespace base { 22 | 23 | template 24 | std::string Stringify(const T& rhs) { 25 | std::ostringstream ostr; 26 | ostr << rhs; 27 | return ostr.str(); 28 | } 29 | 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /base/system_fd.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/system_fd.h" 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace base { 21 | 22 | SystemFd::~SystemFd() { 23 | if (fd_ >= 0) ::close(fd_); 24 | } 25 | 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /base/system_fd.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | namespace mjmech { 18 | namespace base { 19 | 20 | /// Manages ownership of a system file descriptor. 21 | class SystemFd { 22 | public: 23 | SystemFd() : fd_(-1) {} 24 | SystemFd(int fd) : fd_(fd) {} 25 | 26 | SystemFd(SystemFd&& rhs) { 27 | fd_ = rhs.fd_; 28 | rhs.fd_ = -1; 29 | } 30 | 31 | SystemFd& operator=(SystemFd&& rhs) { 32 | fd_ = rhs.fd_; 33 | rhs.fd_ = -1; 34 | return *this; 35 | } 36 | 37 | ~SystemFd(); 38 | 39 | SystemFd(const SystemFd&) = delete; 40 | SystemFd& operator=(const SystemFd&) = delete; 41 | 42 | operator int() { return fd_; } 43 | 44 | private: 45 | int fd_ = -1; 46 | }; 47 | 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /base/system_mmap.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "mjlib/base/system_error.h" 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | /// Manages ownership of an mmap'ed region of a given file descriptor. 25 | class SystemMmap { 26 | public: 27 | SystemMmap() {} 28 | 29 | SystemMmap(int fd, size_t size, uint64_t offset) { 30 | ptr_ = ::mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, offset); 31 | size_ = size; 32 | mjlib::base::system_error::throw_if(ptr_ == MAP_FAILED); 33 | } 34 | 35 | ~SystemMmap() { 36 | if (ptr_ != MAP_FAILED) { 37 | mjlib::base::system_error::throw_if(::munmap(ptr_, size_) < 0); 38 | } 39 | } 40 | 41 | SystemMmap(SystemMmap&& rhs) { 42 | std::swap(ptr_, rhs.ptr_); 43 | std::swap(size_, rhs.size_); 44 | } 45 | 46 | SystemMmap& operator=(SystemMmap&& rhs) { 47 | std::swap(ptr_, rhs.ptr_); 48 | std::swap(size_, rhs.size_); 49 | return *this; 50 | } 51 | 52 | SystemMmap(const SystemMmap&) = delete; 53 | SystemMmap& operator=(const SystemMmap&) = delete; 54 | 55 | void* ptr() { return ptr_; } 56 | 57 | // Since this is intended to be whatever, we just allow it to be 58 | // converted to any old pointer at will without extra hoops. 59 | template 60 | operator T*() { return ptr_; } 61 | 62 | template 63 | operator const T*() const { return ptr_; } 64 | 65 | private: 66 | void* ptr_ = MAP_FAILED; 67 | size_t size_ = 0; 68 | }; 69 | 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /base/telemetry_log_registrar.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include "mjlib/io/now.h" 21 | #include "mjlib/telemetry/binary_write_archive.h" 22 | #include "mjlib/telemetry/file_writer.h" 23 | 24 | 25 | namespace mjmech { 26 | namespace base { 27 | /// A registrar which emits every instance of every record to a 28 | /// TelemetryLog instance using the TelemetryArchive for 29 | /// serialization. 30 | /// 31 | /// NOTE: Ideally this would be noncopyable, but std::tuple doesn't 32 | /// currently allow construction of noncopyable members. 33 | /// 34 | /// NOTE: In the future, this could have policies around which records 35 | /// are written to the log and at what rate. 36 | class TelemetryLogRegistrar { 37 | public: 38 | TelemetryLogRegistrar(boost::asio::io_context& context, 39 | mjlib::telemetry::FileWriter* telemetry_log) 40 | : context_(context), 41 | telemetry_log_(telemetry_log) {} 42 | 43 | template 44 | void Register(const std::string& name, 45 | boost::signals2::signal* signal) { 46 | const auto identifier = telemetry_log_->AllocateIdentifier(name); 47 | telemetry_log_->WriteSchema( 48 | identifier, 49 | mjlib::telemetry::BinarySchemaArchive::template schema()); 50 | signal->connect(std::bind(&TelemetryLogRegistrar::HandleData, 51 | this, identifier, 52 | std::placeholders::_1)); 53 | } 54 | 55 | template 56 | void HandleData(mjlib::telemetry::FileWriter::Identifier identifier, 57 | const T* data) { 58 | // If the log isn't open, don't even bother serializing things. 59 | if (!telemetry_log_->IsOpen()) { return; } 60 | 61 | auto buffer = telemetry_log_->GetBuffer(); 62 | mjlib::telemetry::BinaryWriteArchive(*buffer).Accept(data); 63 | telemetry_log_->WriteData( 64 | mjlib::io::Now(context_), identifier, std::move(buffer)); 65 | } 66 | 67 | boost::asio::io_context& context_; 68 | mjlib::telemetry::FileWriter* const telemetry_log_; 69 | }; 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /base/telemetry_registry.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include "mjlib/telemetry/file_writer.h" 23 | 24 | #include "base/telemetry_log_registrar.h" 25 | #include "base/telemetry_remote_debug_registrar.h" 26 | 27 | namespace mjmech { 28 | namespace base { 29 | /// Maintain a local publish-subscribe model used for data objects. 30 | class TelemetryRegistry : boost::noncopyable { 31 | public: 32 | TelemetryRegistry(boost::asio::io_context& context, 33 | mjlib::telemetry::FileWriter* log, 34 | TelemetryRemoteDebugServer* debug) 35 | : log_(context, log), debug_(debug) {} 36 | 37 | /// Register a serializable object, and return a function object 38 | /// which when called will disseminate the 39 | /// object to any observers. 40 | template 41 | std::function 42 | Register(const std::string& record_name) { 43 | // NOTE jpieper: Ideally this would return auto, and just let the 44 | // compiler sort out what type the lambda is. But since C++11 45 | // doesn't support that yet, we return the type erasing 46 | // std::function. 47 | 48 | auto* ptr = new Concrete(); 49 | auto result = [ptr](const DataObject* object) { ptr->signal(object); }; 50 | 51 | log_.Register(record_name, &ptr->signal); 52 | debug_.Register(record_name, &ptr->signal); 53 | 54 | records_.insert( 55 | std::make_pair( 56 | record_name, std::unique_ptr(ptr))); 57 | 58 | return result; 59 | }; 60 | 61 | template 62 | void Register(const std::string& record_name, 63 | boost::signals2::signal* signal) { 64 | signal->connect(Register(record_name)); 65 | } 66 | 67 | private: 68 | struct Base { 69 | virtual ~Base() {} 70 | }; 71 | 72 | template 73 | struct Concrete : public Base { 74 | virtual ~Concrete() {} 75 | 76 | boost::signals2::signal signal; 77 | }; 78 | 79 | std::map > records_; 80 | 81 | TelemetryLogRegistrar log_; 82 | TelemetryRemoteDebugRegistrar debug_; 83 | }; 84 | 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /base/telemetry_remote_debug_registrar.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "telemetry_remote_debug_server.h" 20 | 21 | namespace mjmech { 22 | namespace base { 23 | class TelemetryRemoteDebugServer; 24 | 25 | /// A registrar which supports emitting data over the network in a 26 | /// human readable format for online diagnostics and debugging. 27 | /// 28 | /// The implementation of this class is largely a pass-through, since 29 | /// tuple's can't hold noncopyable things, we delegate to a shared_ptr 30 | /// to something that is non-copyable. 31 | class TelemetryRemoteDebugRegistrar { 32 | public: 33 | TelemetryRemoteDebugRegistrar(TelemetryRemoteDebugServer* server) 34 | : server_(server) {} 35 | 36 | template 37 | void Register(const std::string& name, 38 | boost::signals2::signal* signal) { 39 | server_->Register(name, signal); 40 | } 41 | 42 | TelemetryRemoteDebugServer* const server_; 43 | }; 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /base/test/aspect_ratio_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/aspect_ratio.h" 16 | 17 | #include 18 | 19 | using mjmech::base::MaintainAspectRatio; 20 | 21 | BOOST_AUTO_TEST_CASE(AspectRatio) { 22 | { 23 | auto result = MaintainAspectRatio({4, 4}, {4, 4}); 24 | BOOST_TEST(result.min() == Eigen::Vector2i(0, 0)); 25 | BOOST_TEST(result.max() == Eigen::Vector2i(4, 4)); 26 | BOOST_TEST(result.sizes() == Eigen::Vector2i(4, 4)); 27 | } 28 | 29 | { 30 | auto result = MaintainAspectRatio({4, 4}, {10, 4}); 31 | BOOST_TEST(result.min() == Eigen::Vector2i(3, 0)); 32 | BOOST_TEST(result.sizes() == Eigen::Vector2i(4, 4)); 33 | } 34 | 35 | { 36 | auto result = MaintainAspectRatio({4, 4}, {20, 8}); 37 | BOOST_TEST(result.min() == Eigen::Vector2i(6, 0)); 38 | BOOST_TEST(result.sizes() == Eigen::Vector2i(8, 8)); 39 | } 40 | 41 | { 42 | auto result = MaintainAspectRatio({4, 4}, {8, 20}); 43 | BOOST_TEST(result.min() == Eigen::Vector2i(0, 6)); 44 | BOOST_TEST(result.sizes() == Eigen::Vector2i(8, 8)); 45 | } 46 | 47 | { 48 | auto result = MaintainAspectRatio({5, 4}, {10, 10}); 49 | BOOST_TEST(result.min() == Eigen::Vector2i(0, 1)); 50 | BOOST_TEST(result.sizes() == Eigen::Vector2i(10, 8)); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /base/test/bezier_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/bezier.h" 16 | 17 | #include 18 | 19 | #include 20 | 21 | template 22 | using DUT = mjmech::base::Bezier; 23 | 24 | BOOST_AUTO_TEST_CASE(BezierTest) { 25 | { 26 | DUT dut(0.0, 1.0); 27 | BOOST_TEST(dut.position(0.0) == 0.0); 28 | BOOST_TEST(dut.velocity(0.0) == 0.0); 29 | BOOST_TEST(dut.acceleration(0.0) == 6.0); 30 | 31 | BOOST_TEST(dut.position(0.5) == 0.5); 32 | BOOST_TEST(dut.velocity(0.5) == 1.5); 33 | BOOST_TEST(dut.acceleration(0.5) == 0.0); 34 | 35 | BOOST_TEST(dut.position(1.0) == 1.0); 36 | BOOST_TEST(dut.velocity(1.0) == 0.0); 37 | BOOST_TEST(dut.acceleration(1.0) == -6.0); 38 | } 39 | 40 | { 41 | DUT dut(-2, 8); 42 | BOOST_TEST(dut.position(0.0) == -2); 43 | BOOST_TEST(dut.velocity(0.0) == 0.0); 44 | BOOST_TEST(dut.acceleration(0.0) == 60.0); 45 | 46 | BOOST_TEST(dut.position(0.5) == 3.0); 47 | BOOST_TEST(dut.velocity(0.5) == 15); 48 | BOOST_TEST(dut.acceleration(0.5) == 0.0); 49 | 50 | BOOST_TEST(dut.position(1.0) == 8.0); 51 | BOOST_TEST(dut.velocity(1.0) == 0.0); 52 | BOOST_TEST(dut.acceleration(1.0) == -60.0); 53 | } 54 | } 55 | 56 | BOOST_AUTO_TEST_CASE(BezierEigen) { 57 | DUT dut(Eigen::Vector3d(0, 5, 10), 58 | Eigen::Vector3d(1, 10, 20)); 59 | BOOST_TEST(dut.position(0.0) == Eigen::Vector3d(0, 5, 10)); 60 | BOOST_TEST(dut.velocity(0.0) == Eigen::Vector3d(0, 0, 0)); 61 | BOOST_TEST(dut.acceleration(0.0) == Eigen::Vector3d(6, 30, 60)); 62 | 63 | BOOST_TEST(dut.position(0.5) == Eigen::Vector3d(0.5, 7.5, 15)); 64 | BOOST_TEST(dut.velocity(0.5) == Eigen::Vector3d(1.5, 7.5, 15)); 65 | BOOST_TEST(dut.acceleration(0.5) == Eigen::Vector3d(0., 0., 0.)); 66 | 67 | BOOST_TEST(dut.position(1.0) == Eigen::Vector3d(1, 10, 20)); 68 | BOOST_TEST(dut.velocity(1.0) == Eigen::Vector3d(0, 0, 0)); 69 | BOOST_TEST(dut.acceleration(1.0) == Eigen::Vector3d(-6, -30, -60)); 70 | } 71 | -------------------------------------------------------------------------------- /base/test/fit_plane_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/fit_plane.h" 16 | 17 | #include 18 | 19 | using mjmech::base::FitPlane; 20 | 21 | BOOST_AUTO_TEST_CASE(BasicFitPlane, * boost::unit_test::tolerance(1e-4)) { 22 | { 23 | std::vector points = { 24 | { -2, -2, -1 }, 25 | { -2, 2, -1 }, 26 | { 2, -2, 1 }, 27 | { 2, 2, 1 }, 28 | }; 29 | 30 | const auto result = FitPlane(points); 31 | BOOST_TEST(std::abs(result.c - 0.0) < 0.001); 32 | BOOST_TEST(result.a == 0.5); 33 | BOOST_TEST(result.b == 0.0); 34 | } 35 | 36 | { 37 | std::vector points = { 38 | { -2, -2, 0 }, 39 | { -2, 2, 2 }, 40 | { 2, -2, 0 }, 41 | { 2, 2, 2 }, 42 | }; 43 | 44 | const auto result = FitPlane(points); 45 | BOOST_TEST(result.c == 1.0); 46 | BOOST_TEST(result.a == 0.0); 47 | BOOST_TEST(result.b == 0.5); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /base/test/leg_force_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/leg_force.h" 16 | 17 | #include 18 | 19 | using mjmech::base::OptimizeLegForce; 20 | 21 | BOOST_AUTO_TEST_CASE(LegForceTest, * boost::unit_test::tolerance(1e-3)) { 22 | { 23 | const auto result = OptimizeLegForce({ 24 | {-1., -1.}, 25 | {-1., 1.}, 26 | {1., -1.}, 27 | {1., 1.}, 28 | }); 29 | 30 | BOOST_TEST_REQUIRE(result.size() == 4); 31 | BOOST_TEST(result[0] == 0.25); 32 | BOOST_TEST(result[1] == 0.25); 33 | BOOST_TEST(result[2] == 0.25); 34 | BOOST_TEST(result[3] == 0.25); 35 | } 36 | 37 | { 38 | const auto result = OptimizeLegForce({{-4, 0}, {2, 0}}); 39 | BOOST_TEST_REQUIRE(result.size() == 2); 40 | BOOST_TEST(result[0] == 0.3333); 41 | BOOST_TEST(result[1] == 0.6667); 42 | } 43 | { 44 | // Something that requires a simple balance. 45 | const auto result = OptimizeLegForce({{-4, 1}, {2, 1}}); 46 | BOOST_TEST_REQUIRE(result.size() == 2); 47 | BOOST_TEST(result[0] == 0.3333); 48 | BOOST_TEST(result[1] == 0.6667); 49 | } 50 | 51 | { 52 | // No matter what we do, the Y axis will be off balance. 53 | const auto result = OptimizeLegForce({{-4, 1}, {2, 0}}); 54 | BOOST_TEST_REQUIRE(result.size() == 2); 55 | BOOST_TEST(result[0] == 0.088235); 56 | BOOST_TEST(result[1] == 0.911764); 57 | } 58 | 59 | { 60 | // No matter what we do, the Y axis will be even more off balance. 61 | const auto result = OptimizeLegForce({{-4, 4}, {2, 0}}); 62 | BOOST_TEST_REQUIRE(result.size() == 2); 63 | BOOST_TEST(result[0] == 0.007335); 64 | BOOST_TEST(result[1] == 0.992665); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /base/test/linux_input_manual_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/linux_input.h" 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "mjlib/base/clipp.h" 26 | #include "mjlib/base/fail.h" 27 | 28 | namespace { 29 | using namespace mjmech::base; 30 | 31 | class Reader { 32 | public: 33 | Reader(LinuxInput* input, bool absinfo) 34 | : input_(input), absinfo_(absinfo) {} 35 | 36 | void StartRead() { 37 | input_->AsyncRead( 38 | &event_, std::bind(&Reader::HandleRead, this, 39 | std::placeholders::_1)); 40 | } 41 | 42 | void HandleRead(mjlib::base::error_code ec) { 43 | mjlib::base::FailIf(ec); 44 | 45 | std::cout << event_ << "\n"; 46 | if (event_.ev_type == EV_ABS && absinfo_) { 47 | std::cout << input_->abs_info(event_.code) << "\n"; 48 | } 49 | 50 | StartRead(); 51 | } 52 | 53 | LinuxInput* const input_; 54 | const bool absinfo_; 55 | LinuxInput::Event event_; 56 | }; 57 | 58 | int work(int argc, char** argv) { 59 | std::string device; 60 | bool wait = false; 61 | bool absinfo = false; 62 | auto group = clipp::group( 63 | (clipp::option("d", "device") & clipp::value("", device)) % "input device", 64 | (clipp::option("w", "wait").set(wait)) % "wait for events", 65 | (clipp::option("a", "absinfo").set(absinfo)) % "display absinfo" 66 | ); 67 | 68 | mjlib::base::ClippParse(argc, argv, group); 69 | 70 | boost::asio::io_context context; 71 | LinuxInput linux_input(context.get_executor()); 72 | linux_input.Open(device); 73 | 74 | std::cout << linux_input << "\n"; 75 | std::cout << linux_input.features(EV_REL) << "\n"; 76 | std::cout << linux_input.features(EV_ABS) << "\n"; 77 | std::cout << linux_input.features(EV_KEY) << "\n"; 78 | 79 | auto abs_features = linux_input.features(EV_ABS); 80 | for (size_t i = 0; i < abs_features.capabilities.size(); i++) { 81 | if (!abs_features.capabilities.test(i)) { continue; } 82 | std::cout << linux_input.abs_info(i) << "\n"; 83 | } 84 | 85 | if (wait) { 86 | Reader reader(&linux_input, absinfo); 87 | reader.StartRead(); 88 | context.run(); 89 | } 90 | return 0; 91 | } 92 | } 93 | 94 | extern "C" { 95 | int main(int argc, char** argv) { 96 | try { 97 | return work(argc, argv); 98 | } catch (std::exception& e) { 99 | std::cerr << "error: " << e.what() << "\n"; 100 | return 1; 101 | } 102 | return 0; 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /base/test/named_type_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/named_type.h" 16 | 17 | #include 18 | 19 | namespace { 20 | using SimpleType = mjmech::base::NamedType; 21 | 22 | int SimpleTransformer(SimpleType v) { 23 | return v.get(); 24 | } 25 | } 26 | 27 | BOOST_AUTO_TEST_CASE(NamedTypeTest) { 28 | BOOST_TEST(SimpleTransformer(SimpleType(3)) == 3); 29 | } 30 | -------------------------------------------------------------------------------- /base/test/se3d_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "sophus/se3.hpp" 16 | 17 | #include 18 | 19 | BOOST_AUTO_TEST_CASE(DoIUnderstandFrames) { 20 | // I think that this means, that point (0, 0, 0) in frame B, should 21 | // be the same as point (1, 0, 0) in frame A. 22 | Sophus::SE3d pose_AB(Sophus::SO3d(), Eigen::Vector3d(1.0, 0, 0)); 23 | 24 | const Eigen::Vector3d p_B = Eigen::Vector3d(0, 0, 0); 25 | const Eigen::Vector3d p_A = pose_AB * p_B; 26 | BOOST_TEST(p_A.x() == 1.0); 27 | } 28 | -------------------------------------------------------------------------------- /base/test/signal_result_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/signal_result.h" 16 | 17 | #include 18 | 19 | using namespace mjmech::base; 20 | 21 | BOOST_AUTO_TEST_CASE(TestSignalResultCallback1) { 22 | boost::asio::io_context context; 23 | auto executor = context.get_executor(); 24 | 25 | boost::signals2::signal signal; 26 | int value = 0; 27 | bool done = false; 28 | int count = 0; 29 | 30 | SignalResult::Wait(executor, &signal, 1.0, 31 | [&](const boost::system::error_code& ec, 32 | int value_in) { 33 | BOOST_CHECK(!ec); 34 | done = true; 35 | value = value_in; 36 | count++; 37 | }); 38 | 39 | // We haven't even started the io_context yet, so nothing should be 40 | // running. 41 | BOOST_CHECK_EQUAL(done, false); 42 | BOOST_CHECK_EQUAL(value, 0); 43 | 44 | mjlib::io::DeadlineTimer timer1(executor); 45 | timer1.expires_from_now(boost::posix_time::milliseconds(500)); 46 | timer1.async_wait([&](const boost::system::error_code& ec) { 47 | BOOST_CHECK(!ec); 48 | BOOST_CHECK_EQUAL(done, false); 49 | 50 | int send = 5; 51 | signal(&send); 52 | 53 | BOOST_CHECK_EQUAL(done, true); 54 | BOOST_CHECK_EQUAL(value, 5); 55 | BOOST_CHECK_EQUAL(count, 1); 56 | }); 57 | 58 | context.run(); 59 | BOOST_CHECK_EQUAL(done, true); 60 | BOOST_CHECK_EQUAL(count, 1); 61 | } 62 | 63 | BOOST_AUTO_TEST_CASE(TestSignalResultCallback2) { 64 | boost::asio::io_context context; 65 | auto executor = context.get_executor(); 66 | 67 | boost::signals2::signal signal; 68 | bool done = false; 69 | int count = 0; 70 | 71 | SignalResult::Wait(executor, &signal, 1.0, 72 | [&](const boost::system::error_code& ec, 73 | int) { 74 | BOOST_CHECK(ec); 75 | BOOST_CHECK_EQUAL(ec, boost::asio::error::operation_aborted); 76 | done = true; 77 | count++; 78 | }); 79 | 80 | mjlib::io::DeadlineTimer timer1(executor); 81 | timer1.expires_from_now(boost::posix_time::milliseconds(1500)); 82 | timer1.async_wait([&](const boost::system::error_code& ec) { 83 | BOOST_CHECK(!ec); 84 | BOOST_CHECK_EQUAL(done, true); 85 | BOOST_CHECK_EQUAL(count, 1); 86 | }); 87 | 88 | context.run(); 89 | BOOST_CHECK_EQUAL(count, 1); 90 | } 91 | -------------------------------------------------------------------------------- /base/test/sophus_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/sophus.h" 16 | 17 | #include 18 | 19 | #include "mjlib/base/json5_write_archive.h" 20 | #include "mjlib/base/json5_read_archive.h" 21 | 22 | BOOST_AUTO_TEST_CASE(BasicSophusTest) { 23 | Sophus::SE3d zero; 24 | const auto actual = mjlib::base::Json5WriteArchive::Write(zero); 25 | const std::string expected = R"XXX({ 26 | "so3" : { 27 | "x" : 0, 28 | "y" : 0, 29 | "z" : 0, 30 | "w" : 1 31 | }, 32 | "translation" : [ 33 | 0, 34 | 0, 35 | 0 36 | ] 37 | })XXX"; 38 | BOOST_TEST(actual == expected); 39 | } 40 | -------------------------------------------------------------------------------- /base/test/telemetry_log_registrar_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/telemetry_log_registrar.h" 16 | 17 | #include 18 | 19 | #include "mjlib/base/visitor.h" 20 | 21 | #include "base/telemetry_registry.h" 22 | 23 | namespace { 24 | struct TestData { 25 | uint16_t value = 0; 26 | 27 | template 28 | void Serialize(Archive* a) { 29 | a->Visit(MJ_NVP(value)); 30 | } 31 | }; 32 | 33 | using namespace mjmech::base; 34 | } 35 | 36 | BOOST_AUTO_TEST_CASE(TelemetryLogRegistrarTest) { 37 | // TelemetryLog log; 38 | // TelemetryRegistry registry(&log); 39 | // auto callable = registry.Register("test1"); 40 | 41 | // // This should have resulted in a schema being written. 42 | 43 | } 44 | -------------------------------------------------------------------------------- /base/test/telemetry_registry_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/telemetry_registry.h" 16 | 17 | #include 18 | 19 | #include "mjlib/base/visitor.h" 20 | 21 | using namespace mjmech::base; 22 | 23 | namespace { 24 | struct TestData { 25 | int8_t foo = 0; 26 | 27 | template 28 | void Serialize(Archive* a) { 29 | a->Visit(MJ_NVP(foo)); 30 | } 31 | }; 32 | } 33 | 34 | BOOST_AUTO_TEST_CASE(TelemetryBasicTest) { 35 | } 36 | -------------------------------------------------------------------------------- /base/test/test_main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #define BOOST_TEST_MODULE mech 16 | #include 17 | -------------------------------------------------------------------------------- /base/test/ukf_filter_test.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/ukf_filter.h" 16 | 17 | #include 18 | 19 | namespace { 20 | typedef mjmech::base::UkfFilter UkfFilter; 21 | } 22 | 23 | BOOST_AUTO_TEST_CASE(BasicUkfFilter) { 24 | auto test_process = [](const UkfFilter::State& s, 25 | double dt_s) -> UkfFilter::State { 26 | UkfFilter::State delta; 27 | delta(0, 0) = 0.; 28 | delta(1, 0) = s(0) * dt_s; 29 | delta(2, 0) = s(1) * dt_s + 0.5 * s(0) * dt_s * dt_s; 30 | return s + delta; 31 | }; 32 | 33 | auto test_measurement = [](const UkfFilter::State& s) { 34 | Eigen::Matrix r; 35 | r(0, 0) = s(2); 36 | return r; 37 | }; 38 | 39 | UkfFilter::Covariance cov = UkfFilter::Covariance::Zero(); 40 | cov(0, 0) = 1.0; 41 | cov(1, 1) = 2.0; 42 | cov(2, 2) = 3.0; 43 | 44 | UkfFilter::Covariance proc = UkfFilter::Covariance::Zero(); 45 | proc(0, 0) = 0.1; 46 | proc(1, 1) = 0.1; 47 | proc(2, 2) = 0.1; 48 | UkfFilter dut(UkfFilter::State(0.2, 0.0, 0.0), 49 | cov, 50 | proc); 51 | 52 | Eigen::Matrix meas; 53 | meas(0, 0) = 0.5; 54 | Eigen::Matrix meas_noise; 55 | meas_noise(0, 0) = 2.0; 56 | 57 | for (int i = 0; i < 200; i++) { 58 | meas(0, 0) += 0.5; 59 | dut.UpdateState(0.1, test_process); 60 | dut.UpdateMeasurement(test_measurement, meas, meas_noise); 61 | } 62 | 63 | BOOST_CHECK_SMALL(dut.state()(2) - meas(0), 1e-2); 64 | BOOST_CHECK_SMALL(dut.state()(1) - 0.5 / 0.1, 1e-2); 65 | BOOST_CHECK(dut.covariance()(0, 0) > 0.0); 66 | } 67 | -------------------------------------------------------------------------------- /base/timestamped_log.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "base/timestamped_log.h" 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | 23 | namespace mjmech { 24 | namespace base { 25 | 26 | void OpenMaybeTimestampedLog(mjlib::telemetry::FileWriter* writer, 27 | std::string_view filename, 28 | TimestampMode mode) { 29 | // Make sure that the log file has a date and timestamp somewhere 30 | // in the name. 31 | namespace fs = boost::filesystem; 32 | fs::path log_file_path{std::string(filename)}; 33 | std::string extension = log_file_path.extension().native(); 34 | 35 | const auto now = boost::posix_time::microsec_clock::universal_time(); 36 | std::string datestamp = 37 | fmt::format("{}-{:02d}{:02d}{:02d}", 38 | to_iso_string(now.date()), 39 | now.time_of_day().hours(), 40 | now.time_of_day().minutes(), 41 | now.time_of_day().seconds()); 42 | 43 | const std::string stem = log_file_path.stem().native(); 44 | 45 | fs::path stamped_path = log_file_path.parent_path() / 46 | fmt::format("{}-{}{}", stem, datestamp, extension); 47 | 48 | switch (mode) { 49 | case kShort: { 50 | writer->Open(filename); 51 | break; 52 | } 53 | case kTimestamped: { 54 | writer->Open(stamped_path.native()); 55 | break; 56 | } 57 | } 58 | } 59 | 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /base/timestamped_log.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "mjlib/telemetry/file_writer.h" 20 | 21 | namespace mjmech { 22 | namespace base { 23 | 24 | enum TimestampMode { 25 | kTimestamped, 26 | kShort, 27 | }; 28 | 29 | void OpenMaybeTimestampedLog(mjlib::telemetry::FileWriter* writer, 30 | std::string_view filename, 31 | TimestampMode); 32 | 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /base/tokenizer.h: -------------------------------------------------------------------------------- 1 | // Copyright 2015-2016 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | /// A simple tokenizer. It can split on multiple delimiters, and 20 | /// reports multiple consecutive delimiters as empty tokens. 21 | class Tokenizer { 22 | public: 23 | Tokenizer(const std::string& source, const char* delimiters) 24 | : source_(source), 25 | delimiters_(delimiters), 26 | position_(source_.cbegin()) {} 27 | 28 | std::string next() { 29 | if (position_ == source_.end()) { return std::string(); } 30 | const auto start = position_; 31 | auto next = position_; 32 | bool found = false; 33 | for (; next != source_.end(); ++next) { 34 | if (std::strchr(delimiters_, *next) != nullptr) { 35 | position_ = next; 36 | ++position_; 37 | found = true; 38 | break; 39 | } 40 | } 41 | if (!found) { position_ = next; } 42 | return std::string(start, next); 43 | } 44 | 45 | std::string remaining() const { 46 | return std::string(position_, source_.end()); 47 | } 48 | 49 | private: 50 | const std::string source_; 51 | const char* const delimiters_; 52 | std::string::const_iterator position_; 53 | }; 54 | -------------------------------------------------------------------------------- /configs/BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | filegroup( 20 | name = "configs", 21 | srcs = [ 22 | "hoverbot.ini", 23 | "hoverbot.cfg", 24 | ], 25 | ) 26 | -------------------------------------------------------------------------------- /configs/hoverbot.cfg: -------------------------------------------------------------------------------- 1 | // Configuration for the "mjbots hoverbot". 2 | { 3 | "joints" : [ 4 | { "id" : 1,}, 5 | { "id" : 2, "sign" : -1 }, 6 | ], 7 | "stand_up" : { 8 | "pitch_rate_dps" : 360.0, 9 | }, 10 | "pitch" : { 11 | "pitch_offset_deg" : 5.0, 12 | "pitch_pid" : { 13 | "kp" : 0.05, 14 | "kd" : 0.005, 15 | "ilimit" : 0.0, 16 | }, 17 | "yaw_pid" : { 18 | "kp" : 0.02, 19 | "kd" : 0.002, 20 | "ilimit" : 0.0, 21 | }, 22 | }, 23 | 24 | "drive" : { 25 | "pitch_limit_deg" : 45.0, 26 | "drive_pid" : { 27 | "kp" : 20.0, 28 | "kd" : 0.0, 29 | "ki" : 20.0, 30 | "ilimit" : 10.0, 31 | }, 32 | }, 33 | } 34 | -------------------------------------------------------------------------------- /configs/hoverbot.ini: -------------------------------------------------------------------------------- 1 | # CPUS 1, 2, and 3 are set up as isolcpu's, leaving just 0 for linux 2 | rt.cpu_affinity=2 3 | pi3hat.cpu_affinity=3 4 | 5 | [hoverbot_control] 6 | 7 | config=configs/hoverbot.cfg 8 | max_torque_Nm=3 9 | 10 | [pi3hat] 11 | 12 | mounting.yaw_deg = 90 13 | mounting.pitch_deg = 0 14 | mounting.roll_deg = 90 15 | 16 | [multiplex_client] 17 | -------------------------------------------------------------------------------- /docs/diagrams/1d_swing.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/diagrams/1d_swing.blend -------------------------------------------------------------------------------- /docs/diagrams/1d_swing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/diagrams/1d_swing.png -------------------------------------------------------------------------------- /docs/diagrams/as5047_noise_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright 2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | 18 | import matplotlib 19 | import matplotlib.pyplot as plt 20 | import sys 21 | 22 | data = [[float(y) for y in line.strip().split(',')] 23 | for line in (open(sys.argv[1]).readlines()[1:]) 24 | if line.strip() != ''] 25 | 26 | ax = plt.subplot() 27 | ax.plot([1.0 / x[0] for x in data], [x[1]/4. for x in data]) 28 | ax.set_xlim(40000, 10) 29 | ax.set_xscale("log") 30 | ax.set_xlabel("Frequency Hz") 31 | ax.set_ylabel("LSB") 32 | ax.set_title("AS5047U Position Noise vs Frequency") 33 | 34 | plt.show() 35 | -------------------------------------------------------------------------------- /docs/diagrams/magnetic_saturation.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/diagrams/magnetic_saturation.odg -------------------------------------------------------------------------------- /docs/diagrams/phase_current.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import matplotlib 4 | import matplotlib.pylab as pylab 5 | import matplotlib.pyplot as plt 6 | 7 | data_105kv = [ 8 | # Phase A, Input Power W, Output Torque Nm 9 | (0., 1.5, 0.), 10 | (2.29, 2.79, 1.06), 11 | (4.63, 5.81, 2.13), 12 | (6.42, 8.59, 2.71), 13 | (7.19, 11.36, 2.98), 14 | (8.99, 16.49, 3.7), 15 | (12.57, 29.02, 4.92), 16 | (15, 42.8, 5.86), 17 | (17.4, 57.77, 6.75), 18 | (20.02, 78.21, 7.65), 19 | (24.36, 114.26, 8.8), 20 | (26.58, 144.1, 9.45), 21 | (29.64, 187.44, 10.2), 22 | (33.75, 262.16, 11.49), 23 | (38.65, 383.14, 12.9), 24 | (47.17, 484.63, 15.2), 25 | (52.2, None, 15.87), 26 | (58.8, None, 18.2), 27 | ] 28 | 29 | data_135kv = [ 30 | (0., 1.5, 0.), 31 | (3.18, 2.64, 0.95), 32 | (7.43, 7.32, 2.61), 33 | (13.3, 21.68, 4.2), 34 | (21.76, 54.71, 6.92), 35 | (26.38, 78.85, 7.81), 36 | (36.17, 154.28, 10.7), 37 | (44.61, 241.68, 11.77), 38 | (55.63, 425.5, 14.35), 39 | (58.98, 420.0, 14.72), 40 | (61.63, 401.76, 14.99), 41 | (79.7, None, 16.82), 42 | (99.89, None, 18.05), 43 | ] 44 | 45 | def show_plot(index, data, title, xlabel=None): 46 | ax1 = plt.subplot(index) 47 | ax1.plot([x[0] for x in data], [x[2] for x in data], 48 | color='b', label='output torque') 49 | ax1.set_ylabel('Nm') 50 | 51 | if xlabel: 52 | ax1.set_xlabel('phase current A') 53 | ax1.legend() 54 | 55 | ax1.grid(which='major') 56 | ax1.set_yticks(ticks=[0, 3, 6, 9, 12, 15, 18]) 57 | ax1.set_title(title) 58 | 59 | 60 | ax2 = ax1.twinx() 61 | ax2.plot([x[0] for x in data], [x[1] for x in data], 62 | color='r', label='input power') 63 | ax2.set_ylabel('W') 64 | ax2.set_yscale('log') 65 | ax2.legend(loc='lower right') 66 | 67 | 68 | def plot_torques(): 69 | show_plot("212", data_105kv, "105 Kv stator", xlabel=True) 70 | show_plot("211", data_135kv, "135 Kv stator", xlabel=False) 71 | 72 | plt.show() 73 | 74 | 75 | def plot_power_torque(): 76 | ax1 = plt.subplot() 77 | ax1.plot([x[1] for x in data_135kv], [x[2] for x in data_135kv], label='135Kv') 78 | ax1.plot([x[1] for x in data_105kv], [x[2] for x in data_105kv], label='105Kv') 79 | 80 | ax1.set_xscale("log") 81 | ax1.set_xlabel("input power W") 82 | ax1.set_ylabel("output torque Nm") 83 | ax1.grid() 84 | ax1.legend() 85 | ax1.set_title("Output Torque vs Input Power") 86 | 87 | plt.show() 88 | 89 | 90 | plot_power_torque() 91 | -------------------------------------------------------------------------------- /docs/diagrams/support_line.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/diagrams/support_line.blend -------------------------------------------------------------------------------- /docs/diagrams/support_line.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/diagrams/support_line.png -------------------------------------------------------------------------------- /docs/diagrams/torque_compensation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import matplotlib 4 | import matplotlib.pyplot as plt 5 | 6 | 7 | DATA = [ 8 | (1.012, 1.033, 3.166), 9 | (2.496, 2.619, 7.831), 10 | (3.844, 3.698, 12.144), 11 | (4.924, 4.976, 15.53), 12 | (5.733, 5.506, 17.985), 13 | (6.812, 6.452, 20.556), 14 | (7.757, 7.425, 24.214), 15 | (8.836, 8.592, 27.998), 16 | (9.375, 9.125, 30.32), 17 | (10.455, 9.835, 35.043), 18 | (11.669, 11.417, 41.542), 19 | (12.883, 12.956, 48.286), 20 | (13.693, 13.656, 53.627), 21 | (15.311, 15.407, 65.86), 22 | (15.986, 16.071, 71.987), 23 | (16.93, 16.49, 81.1), 24 | (17.2, 16.89, 83.73), 25 | ] 26 | 27 | 28 | ax1 = plt.subplot() 29 | 30 | ax1.plot([x[0] for x in DATA], [x[1] for x in DATA], color='r', 31 | label="Measured Torque") 32 | ax1.plot([x[0] for x in DATA], [x[0] for x in DATA], color='b', 33 | linewidth=1, linestyle='dashed', 34 | label="Ideal Torque") 35 | ax1.set_ylabel("Nm") 36 | ax1.set_xlabel("Command Torque Nm") 37 | ax1.set_ylim(0, 26) 38 | ax1.grid() 39 | ax1.legend(loc='lower right') 40 | 41 | ax2 = ax1.twinx() 42 | ax2.plot([x[0] for x in DATA], [x[2] for x in DATA], color='g', 43 | label="Phase Current") 44 | ax2.set_ylabel("A") 45 | ax2.legend(loc='upper right') 46 | 47 | 48 | plt.show() 49 | -------------------------------------------------------------------------------- /docs/geometry.md: -------------------------------------------------------------------------------- 1 | # mjmech geometry # 2 | 3 | ## Unit naming conventions ## 4 | 5 | Quantities expressed in a certain unit should have that unit included 6 | as a suffix that variable name. 7 | 8 | ## Frame naming conventions ## 9 | 10 | Quantities which are expressed relative to some frame or frames have those frames suffixed on the variable name using the simbody conventions :https://simtk.org/docman/view.php/47/231/SimbodyTheoryManual.pdf 11 | 12 | As there, common frames may be abbreviated with a single uppercase 13 | letter. 14 | 15 | ## Frame conventions ## 16 | 17 | Coordinate systems are right handed, if there is a human associated 18 | direction, they should be interpreted as: 19 | 20 | * x is positive forward 21 | * y is positive right 22 | * z is positive down 23 | 24 | ## Common frames ## 25 | 26 | Leg Frame (G): The leg frame is located at the center of the shoulder 27 | joint, with +x facing forward, +y facing right, and +z facing down. 28 | In this case, "forward" is oriented away from the center of the robot 29 | in the longitudinal direction, and "right" is measured relative to 30 | that. 31 | 32 | Body Frame (B): This frame is oriented at the center of the quadruped 33 | body and fixed to the chassis. 34 | 35 | CoM Frame (M): This frame is fixed to the chassis at the center of 36 | mass as measured with the legs in idle position. It is oriented to be 37 | level with the ground, so +Z is always down w.r.t. gravity. The frame 38 | has a yaw such that +X is pointed to same heading as Body +X. 39 | 40 | Attitude Frame (A): This frame is fixed to the chassis center of mass. 41 | +Z is down w.r.t. gravity. The rotation of +X/+Y is arbitrary at 42 | power up, and follows the rotation of the B/L transform. 43 | 44 | Robot Frame (R): This frame is offset from the Body Frame (B) and 45 | referenced in the same manner, but may be offset or rotated to apply 46 | cosmetic kinematic changes. 47 | 48 | Terrain Frame (T): This frame is located at the center of mass, 49 | projected along the body Z direction at the ground location. It is 50 | oriented parallel to the local ground terrain and rotated to be 51 | consistent with the A frame. 52 | 53 | Local Frame (L): This frame is oriented such that down is parallel to 54 | gravity. Other degrees of freedom are initialized with the quadrupeds 55 | body center at turn on, and have no absolute correlation with the 56 | outside world. 57 | -------------------------------------------------------------------------------- /docs/numbered-targets.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/docs/numbered-targets.odp -------------------------------------------------------------------------------- /hw/battery_pcb/bosch_pcb-r1.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/battery_pcb/bosch_pcb-r1.zip -------------------------------------------------------------------------------- /hw/battery_pcb/bosch_pcb.kicad_prl: -------------------------------------------------------------------------------- 1 | { 2 | "board": { 3 | "active_layer": 39, 4 | "active_layer_preset": "All Layers", 5 | "auto_track_width": true, 6 | "hidden_netclasses": [], 7 | "hidden_nets": [], 8 | "high_contrast_mode": 0, 9 | "net_color_mode": 1, 10 | "opacity": { 11 | "images": 0.6, 12 | "pads": 1.0, 13 | "tracks": 1.0, 14 | "vias": 1.0, 15 | "zones": 0.6 16 | }, 17 | "selection_filter": { 18 | "dimensions": true, 19 | "footprints": true, 20 | "graphics": true, 21 | "keepouts": true, 22 | "lockedItems": false, 23 | "otherItems": true, 24 | "pads": true, 25 | "text": true, 26 | "tracks": true, 27 | "vias": true, 28 | "zones": true 29 | }, 30 | "visible_items": [ 31 | 0, 32 | 1, 33 | 2, 34 | 3, 35 | 4, 36 | 5, 37 | 8, 38 | 9, 39 | 10, 40 | 11, 41 | 12, 42 | 13, 43 | 15, 44 | 16, 45 | 17, 46 | 18, 47 | 19, 48 | 20, 49 | 21, 50 | 22, 51 | 23, 52 | 24, 53 | 25, 54 | 26, 55 | 27, 56 | 28, 57 | 29, 58 | 30, 59 | 32, 60 | 33, 61 | 34, 62 | 35, 63 | 36, 64 | 39, 65 | 40 66 | ], 67 | "visible_layers": "fffffff_ffffffff", 68 | "zone_display_mode": 0 69 | }, 70 | "meta": { 71 | "filename": "bosch_pcb.kicad_prl", 72 | "version": 3 73 | }, 74 | "project": { 75 | "files": [] 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /hw/battery_pcb/bosch_pcb.kicad_sch: -------------------------------------------------------------------------------- 1 | (kicad_sch (version 20230121) (generator eeschema) 2 | (paper "A4") 3 | (lib_symbols) 4 | (symbol_instances) 5 | ) 6 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-B_Paste.gbr: -------------------------------------------------------------------------------- 1 | %TF.GenerationSoftware,KiCad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1*% 2 | %TF.CreationDate,2024-04-19T08:37:49-04:00*% 3 | %TF.ProjectId,bosch_pcb,626f7363-685f-4706-9362-2e6b69636164,rev?*% 4 | %TF.SameCoordinates,Original*% 5 | %TF.FileFunction,Paste,Bot*% 6 | %TF.FilePolarity,Positive*% 7 | %FSLAX46Y46*% 8 | G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)* 9 | G04 Created by KiCad (PCBNEW 7.0.10-7.0.10~ubuntu22.04.1) date 2024-04-19 08:37:49* 10 | %MOMM*% 11 | %LPD*% 12 | G01* 13 | G04 APERTURE LIST* 14 | G04 APERTURE END LIST* 15 | M02* 16 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-B_Silkscreen.gbr: -------------------------------------------------------------------------------- 1 | %TF.GenerationSoftware,KiCad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1*% 2 | %TF.CreationDate,2024-04-19T08:37:49-04:00*% 3 | %TF.ProjectId,bosch_pcb,626f7363-685f-4706-9362-2e6b69636164,rev?*% 4 | %TF.SameCoordinates,Original*% 5 | %TF.FileFunction,Legend,Bot*% 6 | %TF.FilePolarity,Positive*% 7 | %FSLAX46Y46*% 8 | G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)* 9 | G04 Created by KiCad (PCBNEW 7.0.10-7.0.10~ubuntu22.04.1) date 2024-04-19 08:37:49* 10 | %MOMM*% 11 | %LPD*% 12 | G01* 13 | G04 APERTURE LIST* 14 | G04 APERTURE END LIST* 15 | M02* 16 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-Edge_Cuts.gbr: -------------------------------------------------------------------------------- 1 | %TF.GenerationSoftware,KiCad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1*% 2 | %TF.CreationDate,2024-04-19T08:37:49-04:00*% 3 | %TF.ProjectId,bosch_pcb,626f7363-685f-4706-9362-2e6b69636164,rev?*% 4 | %TF.SameCoordinates,Original*% 5 | %TF.FileFunction,Profile,NP*% 6 | %FSLAX46Y46*% 7 | G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)* 8 | G04 Created by KiCad (PCBNEW 7.0.10-7.0.10~ubuntu22.04.1) date 2024-04-19 08:37:49* 9 | %MOMM*% 10 | %LPD*% 11 | G01* 12 | G04 APERTURE LIST* 13 | %TA.AperFunction,Profile*% 14 | %ADD10C,0.100000*% 15 | %TD*% 16 | G04 APERTURE END LIST* 17 | D10* 18 | X163000000Y-90000000D02* 19 | G75* 20 | G03* 21 | X164000000Y-89000000I0J1000000D01* 22 | G01* 23 | X163000000Y-90000000D02* 24 | X101000000Y-90000000D01* 25 | X100000000Y-89000000D02* 26 | X100000000Y-81000000D01* 27 | X164000000Y-81000000D02* 28 | G75* 29 | G03* 30 | X163000000Y-80000000I-1000000J0D01* 31 | G01* 32 | X101000000Y-80000000D02* 33 | G75* 34 | G03* 35 | X100000000Y-81000000I0J-1000000D01* 36 | G01* 37 | X101000000Y-80000000D02* 38 | X163000000Y-80000000D01* 39 | X100000000Y-89000000D02* 40 | G75* 41 | G03* 42 | X101000000Y-90000000I1000000J0D01* 43 | G01* 44 | X164000000Y-81000000D02* 45 | X164000000Y-89000000D01* 46 | M02* 47 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-F_Paste.gbr: -------------------------------------------------------------------------------- 1 | %TF.GenerationSoftware,KiCad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1*% 2 | %TF.CreationDate,2024-04-19T08:37:49-04:00*% 3 | %TF.ProjectId,bosch_pcb,626f7363-685f-4706-9362-2e6b69636164,rev?*% 4 | %TF.SameCoordinates,Original*% 5 | %TF.FileFunction,Paste,Top*% 6 | %TF.FilePolarity,Positive*% 7 | %FSLAX46Y46*% 8 | G04 Gerber Fmt 4.6, Leading zero omitted, Abs format (unit mm)* 9 | G04 Created by KiCad (PCBNEW 7.0.10-7.0.10~ubuntu22.04.1) date 2024-04-19 08:37:49* 10 | %MOMM*% 11 | %LPD*% 12 | G01* 13 | G04 APERTURE LIST* 14 | G04 APERTURE END LIST* 15 | M02* 16 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-NPTH.drl: -------------------------------------------------------------------------------- 1 | M48 2 | ; DRILL file {KiCad 7.0.10-7.0.10~ubuntu22.04.1} date 2024-04-19T08:35:24 EDT 3 | ; FORMAT={-:-/ absolute / inch / decimal} 4 | ; #@! TF.CreationDate,2024-04-19T08:35:24-04:00 5 | ; #@! TF.GenerationSoftware,Kicad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1 6 | ; #@! TF.FileFunction,NonPlated,1,2,NPTH 7 | FMAT,2 8 | INCH 9 | % 10 | G90 11 | G05 12 | M30 13 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-PTH.drl: -------------------------------------------------------------------------------- 1 | M48 2 | ; DRILL file {KiCad 7.0.10-7.0.10~ubuntu22.04.1} date 2024-04-19T08:35:24 EDT 3 | ; FORMAT={-:-/ absolute / inch / decimal} 4 | ; #@! TF.CreationDate,2024-04-19T08:35:24-04:00 5 | ; #@! TF.GenerationSoftware,Kicad,Pcbnew,7.0.10-7.0.10~ubuntu22.04.1 6 | ; #@! TF.FileFunction,Plated,1,2,PTH 7 | FMAT,2 8 | INCH 9 | ; #@! TA.AperFunction,Plated,PTH,ViaDrill 10 | T1C0.0598 11 | ; #@! TA.AperFunction,Plated,PTH,ViaDrill 12 | T2C0.1260 13 | % 14 | G90 15 | G05 16 | T1 17 | X4.4055Y-3.2465 18 | X4.4055Y-3.4465 19 | X5.9882Y-3.2465 20 | X5.9882Y-3.4465 21 | T2 22 | X4.0945Y-3.3465 23 | X6.2992Y-3.3465 24 | M30 25 | -------------------------------------------------------------------------------- /hw/battery_pcb/r1/bosch_pcb-job.gbrjob: -------------------------------------------------------------------------------- 1 | { 2 | "Header": { 3 | "GenerationSoftware": { 4 | "Vendor": "KiCad", 5 | "Application": "Pcbnew", 6 | "Version": "7.0.10-7.0.10~ubuntu22.04.1" 7 | }, 8 | "CreationDate": "2024-04-19T08:37:49-04:00" 9 | }, 10 | "GeneralSpecs": { 11 | "ProjectId": { 12 | "Name": "bosch_pcb", 13 | "GUID": "626f7363-685f-4706-9362-2e6b69636164", 14 | "Revision": "rev?" 15 | }, 16 | "Size": { 17 | "X": 64.1, 18 | "Y": 10.1 19 | }, 20 | "LayerNumber": 2, 21 | "BoardThickness": 1.6, 22 | "Finish": "None" 23 | }, 24 | "DesignRules": [ 25 | { 26 | "Layers": "Outer", 27 | "PadToPad": 0.2, 28 | "PadToTrack": 0.2, 29 | "TrackToTrack": 0.2, 30 | "TrackToRegion": 0.5, 31 | "RegionToRegion": 0.5 32 | } 33 | ], 34 | "FilesAttributes": [ 35 | { 36 | "Path": "bosch_pcb-F_Cu.gbr", 37 | "FileFunction": "Copper,L1,Top", 38 | "FilePolarity": "Positive" 39 | }, 40 | { 41 | "Path": "bosch_pcb-B_Cu.gbr", 42 | "FileFunction": "Copper,L2,Bot", 43 | "FilePolarity": "Positive" 44 | }, 45 | { 46 | "Path": "bosch_pcb-F_Paste.gbr", 47 | "FileFunction": "SolderPaste,Top", 48 | "FilePolarity": "Positive" 49 | }, 50 | { 51 | "Path": "bosch_pcb-B_Paste.gbr", 52 | "FileFunction": "SolderPaste,Bot", 53 | "FilePolarity": "Positive" 54 | }, 55 | { 56 | "Path": "bosch_pcb-F_Silkscreen.gbr", 57 | "FileFunction": "Legend,Top", 58 | "FilePolarity": "Positive" 59 | }, 60 | { 61 | "Path": "bosch_pcb-B_Silkscreen.gbr", 62 | "FileFunction": "Legend,Bot", 63 | "FilePolarity": "Positive" 64 | }, 65 | { 66 | "Path": "bosch_pcb-F_Mask.gbr", 67 | "FileFunction": "SolderMask,Top", 68 | "FilePolarity": "Negative" 69 | }, 70 | { 71 | "Path": "bosch_pcb-B_Mask.gbr", 72 | "FileFunction": "SolderMask,Bot", 73 | "FilePolarity": "Negative" 74 | }, 75 | { 76 | "Path": "bosch_pcb-Edge_Cuts.gbr", 77 | "FileFunction": "Profile", 78 | "FilePolarity": "Positive" 79 | } 80 | ], 81 | "MaterialStackup": [ 82 | { 83 | "Type": "Legend", 84 | "Name": "Top Silk Screen" 85 | }, 86 | { 87 | "Type": "SolderPaste", 88 | "Name": "Top Solder Paste" 89 | }, 90 | { 91 | "Type": "SolderMask", 92 | "Name": "Top Solder Mask" 93 | }, 94 | { 95 | "Type": "Copper", 96 | "Name": "F.Cu" 97 | }, 98 | { 99 | "Type": "Dielectric", 100 | "Material": "FR4", 101 | "Name": "F.Cu/B.Cu", 102 | "Notes": "Type: dielectric layer 1 (from F.Cu to B.Cu)" 103 | }, 104 | { 105 | "Type": "Copper", 106 | "Name": "B.Cu" 107 | }, 108 | { 109 | "Type": "SolderMask", 110 | "Name": "Bottom Solder Mask" 111 | }, 112 | { 113 | "Type": "SolderPaste", 114 | "Name": "Bottom Solder Paste" 115 | }, 116 | { 117 | "Type": "Legend", 118 | "Name": "Bottom Silk Screen" 119 | } 120 | ] 121 | } 122 | -------------------------------------------------------------------------------- /hw/gopro_mount.3mf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/gopro_mount.3mf -------------------------------------------------------------------------------- /hw/gopro_mount.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/gopro_mount.stl -------------------------------------------------------------------------------- /hw/hoverbot_base.3mf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_base.3mf -------------------------------------------------------------------------------- /hw/hoverbot_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_base.stl -------------------------------------------------------------------------------- /hw/hoverbot_base_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_base_cover.stl -------------------------------------------------------------------------------- /hw/hoverbot_battery_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_battery_cover.stl -------------------------------------------------------------------------------- /hw/hoverbot_battery_rail1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_battery_rail1.stl -------------------------------------------------------------------------------- /hw/hoverbot_battery_rail2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_battery_rail2.stl -------------------------------------------------------------------------------- /hw/hoverbot_chassis.3mf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_chassis.3mf -------------------------------------------------------------------------------- /hw/hoverbot_chassis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_chassis.stl -------------------------------------------------------------------------------- /hw/hoverbot_chassis_cover.3mf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_chassis_cover.3mf -------------------------------------------------------------------------------- /hw/hoverbot_chassis_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_chassis_cover.stl -------------------------------------------------------------------------------- /hw/hoverbot_motor_mount1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_motor_mount1.stl -------------------------------------------------------------------------------- /hw/hoverbot_motor_mount2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_motor_mount2.stl -------------------------------------------------------------------------------- /hw/hoverbot_rails_base_cover.3mf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/hw/hoverbot_rails_base_cover.3mf -------------------------------------------------------------------------------- /mech/attitude_data.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "mjlib/base/visitor.h" 18 | 19 | #include "base/euler.h" 20 | #include "base/point3d.h" 21 | #include "base/quaternion.h" 22 | 23 | namespace mjmech { 24 | namespace mech { 25 | 26 | struct AttitudeData { 27 | boost::posix_time::ptime timestamp; 28 | 29 | base::Quaternion attitude; 30 | base::Point3D rate_dps; 31 | base::Euler euler_deg; 32 | base::Point3D accel_mps2; 33 | 34 | base::Point3D bias_dps; 35 | base::Quaternion attitude_uncertainty; 36 | base::Point3D bias_uncertainty_dps; 37 | 38 | template 39 | void Serialize(Archive* a) { 40 | a->Visit(MJ_NVP(timestamp)); 41 | a->Visit(MJ_NVP(attitude)); 42 | a->Visit(MJ_NVP(rate_dps)); 43 | a->Visit(MJ_NVP(euler_deg)); 44 | a->Visit(MJ_NVP(accel_mps2)); 45 | a->Visit(MJ_NVP(bias_dps)); 46 | a->Visit(MJ_NVP(attitude_uncertainty)); 47 | a->Visit(MJ_NVP(bias_uncertainty_dps)); 48 | } 49 | }; 50 | 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /mech/camera_driver.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mech/camera_driver.h" 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #ifdef COM_GITHUB_MJBOTS_RASPBERRYPI 26 | #include 27 | #endif 28 | 29 | namespace mjmech { 30 | namespace mech { 31 | 32 | #ifdef COM_GITHUB_MJBOTS_RASPBERRYPI 33 | class CameraDriver::Impl { 34 | public: 35 | Impl(const Options& options) 36 | : thread_(std::bind(&Impl::Run, this, options)) { 37 | } 38 | 39 | ~Impl() { 40 | done_.store(true); 41 | thread_.join(); 42 | } 43 | 44 | void Run(Options options) { 45 | raspicam::RaspiCam_Cv camera; 46 | camera.set(cv::CAP_PROP_FRAME_WIDTH, options.width); 47 | camera.set(cv::CAP_PROP_FRAME_HEIGHT, options.height); 48 | camera.set(cv::CAP_PROP_MODE, options.mode); 49 | camera.set(cv::CAP_PROP_FPS, options.fps); 50 | camera.setRotation(options.rotation); 51 | camera.set(cv::CAP_PROP_FORMAT, CV_8UC3); 52 | camera.open(); 53 | 54 | cv::Mat image; 55 | uint16_t count = 0; 56 | while (!done_.load()) { 57 | camera.grab(); 58 | camera.retrieve(image); 59 | if (options.record_every >= 1) { 60 | if (count == 0) { 61 | cv::imwrite(MakePath(options), image); 62 | count = options.record_every - 1; 63 | } else { 64 | count--; 65 | } 66 | } 67 | image_signal_(image); 68 | } 69 | } 70 | 71 | std::string MakePath(const Options& options) const { 72 | const auto now = boost::posix_time::microsec_clock::universal_time(); 73 | return options.record_path + to_iso_string(now) + ".png"; 74 | } 75 | 76 | std::thread thread_; 77 | std::atomic done_{false}; 78 | ImageSignal image_signal_; 79 | }; 80 | #else 81 | class CameraDriver::Impl { 82 | public: 83 | Impl(const Options&) {} 84 | 85 | ImageSignal image_signal_; 86 | }; 87 | #endif 88 | 89 | CameraDriver::CameraDriver(const Options& options) 90 | : impl_(std::make_unique(options)) {} 91 | 92 | CameraDriver::~CameraDriver() {} 93 | 94 | CameraDriver::ImageSignal* CameraDriver::image_signal() { 95 | return &impl_->image_signal_; 96 | } 97 | 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /mech/camera_driver.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "mjlib/base/visitor.h" 22 | #include "mjlib/io/async_types.h" 23 | 24 | namespace mjmech { 25 | namespace mech { 26 | 27 | /// Read frames from a camera. 28 | class CameraDriver { 29 | public: 30 | struct Options { 31 | int width = 1280; 32 | int height = 720; 33 | int mode = 6; 34 | int fps = 60; 35 | int rotation = 270; 36 | 37 | std::string record_path = "/tmp/mjbots-camera"; 38 | int record_every = -1; 39 | 40 | template 41 | void Serialize(Archive* a) { 42 | a->Visit(MJ_NVP(width)); 43 | a->Visit(MJ_NVP(height)); 44 | a->Visit(MJ_NVP(mode)); 45 | a->Visit(MJ_NVP(fps)); 46 | a->Visit(MJ_NVP(rotation)); 47 | a->Visit(MJ_NVP(record_path)); 48 | a->Visit(MJ_NVP(record_every)); 49 | } 50 | }; 51 | 52 | CameraDriver(const Options& options); 53 | ~CameraDriver(); 54 | 55 | using ImageSignal = boost::signals2::signal; 56 | // This will be emitted from a background thread. 57 | ImageSignal* image_signal(); 58 | 59 | private: 60 | class Impl; 61 | std::unique_ptr impl_; 62 | }; 63 | 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /mech/control_timing.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include "mjlib/base/time_conversions.h" 21 | #include "mjlib/base/visitor.h" 22 | #include "mjlib/io/now.h" 23 | 24 | namespace mjmech { 25 | namespace mech { 26 | 27 | class ControlTiming { 28 | public: 29 | ControlTiming(const boost::asio::any_io_executor& executor, 30 | boost::posix_time::ptime last_cycle_start) 31 | : executor_(executor) { 32 | timestamps_.last_cycle_start = last_cycle_start; 33 | timestamps_.cycle_start = Now(); 34 | timestamps_.delta_s = mjlib::base::ConvertDurationToSeconds( 35 | timestamps_.cycle_start - timestamps_.last_cycle_start); 36 | } 37 | 38 | struct Status { 39 | double query_s = 0.0; 40 | double status_s = 0.0; 41 | double control_s = 0.0; 42 | double command_s = 0.0; 43 | double cycle_s = 0.0; 44 | double delta_s = 0.0; 45 | 46 | template 47 | void Serialize(Archive* a) { 48 | a->Visit(MJ_NVP(query_s)); 49 | a->Visit(MJ_NVP(status_s)); 50 | a->Visit(MJ_NVP(control_s)); 51 | a->Visit(MJ_NVP(command_s)); 52 | a->Visit(MJ_NVP(cycle_s)); 53 | a->Visit(MJ_NVP(delta_s)); 54 | } 55 | }; 56 | 57 | Status status() const { 58 | Status result; 59 | 60 | result.query_s = mjlib::base::ConvertDurationToSeconds( 61 | timestamps_.query_done - timestamps_.cycle_start); 62 | result.status_s = mjlib::base::ConvertDurationToSeconds( 63 | timestamps_.status_done - timestamps_.query_done); 64 | result.control_s = mjlib::base::ConvertDurationToSeconds( 65 | timestamps_.control_done - timestamps_.status_done); 66 | result.command_s = mjlib::base::ConvertDurationToSeconds( 67 | timestamps_.command_done - timestamps_.control_done); 68 | result.cycle_s = mjlib::base::ConvertDurationToSeconds( 69 | timestamps_.command_done - timestamps_.cycle_start); 70 | result.delta_s = timestamps_.delta_s; 71 | 72 | return result; 73 | } 74 | 75 | boost::posix_time::ptime cycle_start() const { return timestamps_.cycle_start; } 76 | 77 | void finish_query() { timestamps_.query_done = Now(); } 78 | void finish_status() { timestamps_.status_done = Now(); } 79 | void finish_control() { timestamps_.control_done = Now(); } 80 | void finish_command() { timestamps_.command_done = Now(); } 81 | 82 | private: 83 | struct Timestamps { 84 | boost::posix_time::ptime last_cycle_start; 85 | double delta_s = 0.0; 86 | 87 | boost::posix_time::ptime cycle_start; 88 | boost::posix_time::ptime query_done; 89 | boost::posix_time::ptime status_done; 90 | boost::posix_time::ptime control_done; 91 | boost::posix_time::ptime command_done; 92 | }; 93 | 94 | boost::posix_time::ptime Now() const { 95 | return mjlib::io::Now(executor_.context()); 96 | } 97 | 98 | boost::asio::any_io_executor executor_; 99 | Timestamps timestamps_; 100 | }; 101 | 102 | } 103 | } 104 | -------------------------------------------------------------------------------- /mech/hoverbot.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mech/hoverbot.h" 16 | 17 | #include 18 | 19 | #include "base/logging.h" 20 | #include "base/telemetry_registry.h" 21 | #include "mech/pi3hat_wrapper.h" 22 | 23 | namespace pl = std::placeholders; 24 | 25 | namespace mjmech { 26 | namespace mech { 27 | 28 | class Hoverbot::Impl { 29 | public: 30 | Impl(base::Context& context) 31 | : executor_(context.executor), 32 | factory_(context.factory.get()), 33 | telemetry_registry_(context.telemetry_registry.get()) { 34 | m_.pi3hat = std::make_unique< 35 | mjlib::io::Selector>(executor_, "type"); 36 | m_.pi3hat->Register("pi3hat"); 37 | m_.pi3hat->set_default("pi3hat"); 38 | 39 | m_.hoverbot_control = std::make_unique( 40 | context, 41 | [&]() { return m_.pi3hat->selected(); } ); 42 | m_.web_control = std::make_unique( 43 | context.executor, 44 | [q=m_.hoverbot_control.get()](const auto& cmd) { 45 | q->Command(cmd); 46 | }, 47 | [q=m_.hoverbot_control.get()]() { 48 | return q->status(); 49 | }, 50 | []() { 51 | HoverbotWebControl::Options options; 52 | options.asset_path = "web_control_assets"; 53 | return options; 54 | }()); 55 | m_.system_info = std::make_unique(context); 56 | } 57 | 58 | void AsyncStart(mjlib::io::ErrorCallback callback) { 59 | base::StartArchive::Start( 60 | &m_, [this, callback=std::move(callback)]( 61 | const mjlib::base::error_code& ec) mutable { 62 | // pi3hat should be initialized by now 63 | Pi3hatWrapper* const pi3hat = 64 | dynamic_cast(m_.pi3hat->selected()); 65 | if (pi3hat) { 66 | log_.warn("Registering power"); 67 | telemetry_registry_->Register("power", pi3hat->power_signal()); 68 | } 69 | std::move(callback)(ec); 70 | }); 71 | } 72 | 73 | boost::asio::any_io_executor executor_; 74 | mjlib::io::StreamFactory* const factory_; 75 | base::TelemetryRegistry* telemetry_registry_; 76 | 77 | base::LogRef log_ = base::GetLogInstance("Hoverbot"); 78 | 79 | Members m_; 80 | Parameters p_; 81 | }; 82 | 83 | Hoverbot::Hoverbot(base::Context& context) 84 | : impl_(std::make_unique(context)) {} 85 | 86 | Hoverbot::~Hoverbot() {} 87 | 88 | void Hoverbot::AsyncStart(mjlib::io::ErrorCallback callback) { 89 | impl_->AsyncStart(std::move(callback)); 90 | } 91 | 92 | Hoverbot::Members* Hoverbot::m() { return &impl_->m_; } 93 | 94 | clipp::group Hoverbot::program_options() { 95 | return ( 96 | mjlib::base::ClippArchive().Accept(&impl_->p_).release(), 97 | base::ClippComponentArchive().Accept(&impl_->m_).release() 98 | ); 99 | } 100 | 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /mech/hoverbot.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include "mjlib/io/selector.h" 24 | 25 | #include "base/component_archives.h" 26 | #include "base/context.h" 27 | 28 | #include "mech/pi3hat_interface.h" 29 | #include "mech/hoverbot_control.h" 30 | #include "mech/system_info.h" 31 | #include "mech/web_control.h" 32 | 33 | namespace mjmech { 34 | namespace mech { 35 | 36 | class Hoverbot : boost::noncopyable { 37 | public: 38 | Hoverbot(base::Context& context); 39 | ~Hoverbot(); 40 | 41 | void AsyncStart(mjlib::io::ErrorCallback); 42 | 43 | using HoverbotWebControl = 44 | WebControl; 45 | 46 | struct Members { 47 | std::unique_ptr< 48 | mjlib::io::Selector> pi3hat; 49 | std::unique_ptr hoverbot_control; 50 | std::unique_ptr web_control; 51 | std::unique_ptr system_info; 52 | 53 | template 54 | void Serialize(Archive* a) { 55 | a->Visit(MJ_NVP(pi3hat)); 56 | a->Visit(MJ_NVP(hoverbot_control)); 57 | a->Visit(MJ_NVP(web_control)); 58 | a->Visit(MJ_NVP(system_info)); 59 | } 60 | }; 61 | 62 | Members* m(); 63 | 64 | struct Parameters { 65 | template 66 | void Serialize(Archive* a) { 67 | } 68 | }; 69 | 70 | clipp::group program_options(); 71 | 72 | private: 73 | class Impl; 74 | std::unique_ptr impl_; 75 | }; 76 | 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /mech/hoverbot_config.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include "mjlib/base/pid.h" 20 | #include "mjlib/base/visitor.h" 21 | 22 | #include "base/point3d.h" 23 | #include "base/sophus.h" 24 | 25 | namespace mjmech { 26 | namespace mech { 27 | 28 | // This represents the JSON used to configure the geometry of the 29 | // robot. 30 | struct HoverbotConfig { 31 | double period_s = 0.0025; 32 | double min_voltage = 16.0; 33 | 34 | double wheel_diameter_m = 0.163; 35 | 36 | struct Joint { 37 | int id = 0; 38 | double sign = 1.0; 39 | double rezero_pos_deg = 0.0; 40 | 41 | template 42 | void Serialize(Archive* a) { 43 | a->Visit(MJ_NVP(id)); 44 | a->Visit(MJ_NVP(sign)); 45 | a->Visit(MJ_NVP(rezero_pos_deg)); 46 | } 47 | }; 48 | 49 | std::vector joints; 50 | 51 | struct StandUp { 52 | double pitch_rate_dps = 180.0; 53 | 54 | template 55 | void Serialize(Archive* a) { 56 | a->Visit(MJ_NVP(pitch_rate_dps)); 57 | } 58 | }; 59 | 60 | StandUp stand_up; 61 | 62 | struct Pitch { 63 | double pitch_offset_deg = 0.0; 64 | mjlib::base::PID::Config pitch_pid; 65 | mjlib::base::PID::Config yaw_pid; 66 | 67 | template 68 | void Serialize(Archive* a) { 69 | a->Visit(MJ_NVP(pitch_offset_deg)); 70 | a->Visit(MJ_NVP(pitch_pid)); 71 | a->Visit(MJ_NVP(yaw_pid)); 72 | } 73 | }; 74 | 75 | Pitch pitch; 76 | 77 | struct Drive { 78 | double pitch_limit_deg = 20.0; 79 | mjlib::base::PID::Config drive_pid; 80 | 81 | template 82 | void Serialize(Archive* a) { 83 | a->Visit(MJ_NVP(pitch_limit_deg)); 84 | a->Visit(MJ_NVP(drive_pid)); 85 | } 86 | }; 87 | 88 | Drive drive; 89 | 90 | double max_tip_deg = 65; 91 | double tip_filter_s = 0.1; 92 | double voltage_filter_s = 1.0; 93 | 94 | template 95 | void Serialize(Archive* a) { 96 | a->Visit(MJ_NVP(period_s)); 97 | a->Visit(MJ_NVP(min_voltage)); 98 | a->Visit(MJ_NVP(wheel_diameter_m)); 99 | a->Visit(MJ_NVP(joints)); 100 | a->Visit(MJ_NVP(stand_up)); 101 | a->Visit(MJ_NVP(pitch)); 102 | a->Visit(MJ_NVP(drive)); 103 | a->Visit(MJ_NVP(max_tip_deg)); 104 | a->Visit(MJ_NVP(tip_filter_s)); 105 | a->Visit(MJ_NVP(voltage_filter_s)); 106 | } 107 | }; 108 | 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /mech/hoverbot_context.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "mjlib/base/assert.h" 22 | 23 | #include "mech/hoverbot_command.h" 24 | #include "mech/hoverbot_config.h" 25 | #include "mech/hoverbot_state.h" 26 | 27 | namespace mjmech { 28 | namespace mech { 29 | 30 | struct HoverbotContext : boost::noncopyable { 31 | using Config = HoverbotConfig; 32 | using HC = HoverbotCommand; 33 | 34 | HoverbotContext(const HoverbotConfig& config_in, 35 | const HoverbotCommand* command_in, 36 | HoverbotState* state_in) 37 | : config(config_in), 38 | command(command_in), 39 | state(state_in) { 40 | } 41 | 42 | 43 | const HoverbotConfig& config; 44 | const HoverbotCommand* const command; 45 | HoverbotState* const state; 46 | }; 47 | 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /mech/imu_client.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "mjlib/io/async_types.h" 18 | 19 | #include "mech/attitude_data.h" 20 | 21 | namespace mjmech { 22 | namespace mech { 23 | 24 | class ImuClient { 25 | public: 26 | virtual ~ImuClient() {} 27 | 28 | virtual void ReadImu(AttitudeData*, mjlib::io::ErrorCallback) = 0; 29 | }; 30 | 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /mech/imu_data.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mjlib/base/visitor.h" 16 | 17 | #include "base/point3d.h" 18 | 19 | namespace mjmech { 20 | namespace mech { 21 | 22 | struct ImuData { 23 | boost::posix_time::ptime timestamp; 24 | 25 | base::Point3D rate_dps; 26 | base::Point3D accel_mps2; 27 | 28 | template 29 | void Serialize(Archive* a) { 30 | a->Visit(MJ_NVP(timestamp)); 31 | a->Visit(MJ_NVP(rate_dps)); 32 | a->Visit(MJ_NVP(accel_mps2)); 33 | } 34 | }; 35 | 36 | } 37 | } 38 | -------------------------------------------------------------------------------- /mech/mime_type.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mech/mime_type.h" 16 | 17 | namespace mjmech { 18 | namespace mech { 19 | 20 | std::string_view GetMimeType(std::string_view path) { 21 | const auto ext = [&]() { 22 | const auto pos = path.rfind("."); 23 | if (pos == std::string_view::npos) { return std::string_view(); } 24 | return path.substr(pos); 25 | }(); 26 | 27 | struct Mapping { 28 | std::string_view extension; 29 | std::string_view mime_type; 30 | }; 31 | constexpr Mapping mappings[] = { 32 | { ".htm", "text/html" }, 33 | { ".html", "text/html" }, 34 | { ".css", "text/css" }, 35 | { ".txt", "text/plain" }, 36 | { ".js", "application/javascript" }, 37 | { ".json", "application/json" }, 38 | { ".xml", "application/xml" }, 39 | { ".png", "image/png" }, 40 | { ".jpeg", "image/jpeg" }, 41 | { ".jpg", "image/jpg" }, 42 | { ".gif", "image/gif" }, 43 | { ".ico", "image/vnd.microsoft.icon" }, 44 | { ".svg", "image/svg+xml" }, 45 | }; 46 | for (const auto& mapping : mappings) { 47 | if (ext == mapping.extension) { 48 | return mapping.mime_type; 49 | } 50 | } 51 | return "application/text"; 52 | } 53 | 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /mech/mime_type.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace mjmech { 20 | namespace mech { 21 | 22 | std::string_view GetMimeType(std::string_view path); 23 | 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /mech/moteus_tool_main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "moteus/tool/moteus_tool.h" 16 | 17 | #include "mjlib/io/selector.h" 18 | #include "mjlib/multiplex/asio_client.h" 19 | 20 | #include "mech/pi3hat_wrapper.h" 21 | 22 | int main(int argc, char** argv) { 23 | boost::asio::io_context context; 24 | mjlib::io::Selector client_selector{ 25 | context.get_executor(), "client_type"}; 26 | client_selector.Register("pi3"); 27 | client_selector.set_default("pi3"); 28 | return moteus::tool::moteus_tool_main(context, argc, argv, &client_selector); 29 | } 30 | -------------------------------------------------------------------------------- /mech/multiplex_tool_main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "mjlib/io/selector.h" 18 | #include "mjlib/multiplex/multiplex_tool.h" 19 | #include "mjlib/multiplex/asio_client.h" 20 | 21 | #include "mech/pi3hat_wrapper.h" 22 | 23 | extern "C" { 24 | int main(int argc, char** argv) { 25 | boost::asio::io_context context; 26 | mjlib::io::Selector client_selector{ 27 | context.get_executor(), "client_type"}; 28 | client_selector.Register("pi3"); 29 | client_selector.set_default("pi3"); 30 | return mjlib::multiplex::multiplex_main(context, argc, argv, &client_selector); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /mech/pi3hat_interface.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "mjlib/multiplex/asio_client.h" 18 | 19 | #include "mech/imu_client.h" 20 | 21 | namespace mjmech { 22 | namespace mech { 23 | 24 | /// The pi3hat has multiple functions. This is just an interface that 25 | /// combines all of them for use in an mjlib::io::Selector. 26 | class Pi3hatInterface : public ImuClient, 27 | public mjlib::multiplex::AsioClient { 28 | public: 29 | ~Pi3hatInterface() override {} 30 | 31 | virtual void Cycle( 32 | AttitudeData*, 33 | const Request*, Reply*, 34 | mjlib::io::ErrorCallback callback) = 0; 35 | }; 36 | 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /mech/system_info.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "mech/system_info.h" 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "mjlib/base/time_conversions.h" 25 | #include "mjlib/io/now.h" 26 | #include "mjlib/io/repeating_timer.h" 27 | 28 | #include "base/telemetry_registry.h" 29 | 30 | namespace pl = std::placeholders; 31 | 32 | namespace mjmech { 33 | namespace mech { 34 | 35 | namespace { 36 | struct Data { 37 | boost::posix_time::ptime timestamp; 38 | double temp_C = 0.0; 39 | 40 | template 41 | void Serialize(Archive* a) { 42 | a->Visit(MJ_NVP(timestamp)); 43 | a->Visit(MJ_NVP(temp_C)); 44 | } 45 | }; 46 | 47 | #ifdef COM_GITHUB_MJBOTS_RASPBERRYPI 48 | void PopulateData(Data* data) { 49 | { 50 | std::ifstream inf("/sys/class/thermal/thermal_zone0/temp"); 51 | int temp_milliC = 0; 52 | inf >> temp_milliC; 53 | data->temp_C = temp_milliC * 0.001; 54 | } 55 | } 56 | #else 57 | void PopulateData(Data* data) { 58 | // Nothing for now. 59 | } 60 | #endif 61 | } 62 | 63 | class SystemInfo::Impl { 64 | public: 65 | Impl(base::Context& context) 66 | : executor_(context.executor) { 67 | context.telemetry_registry->Register("system_info", &data_signal_); 68 | } 69 | 70 | void AsyncStart(mjlib::io::ErrorCallback callback) { 71 | timer_.start( 72 | mjlib::base::ConvertSecondsToDuration(period_s_), 73 | std::bind(&Impl::HandleTimer, this, pl::_1)); 74 | boost::asio::post( 75 | executor_, 76 | std::bind(std::move(callback), mjlib::base::error_code())); 77 | } 78 | 79 | void HandleTimer(const mjlib::base::error_code& ec) { 80 | mjlib::base::FailIf(ec); 81 | 82 | Data data; 83 | data.timestamp = mjlib::io::Now(executor_.context()); 84 | 85 | PopulateData(&data); 86 | 87 | data_signal_(&data); 88 | } 89 | 90 | clipp::group program_options() { 91 | clipp::group result; 92 | result.push_back( 93 | clipp::option("period_s") & clipp::value("", period_s_)); 94 | return result; 95 | } 96 | 97 | boost::asio::any_io_executor executor_; 98 | mjlib::io::RepeatingTimer timer_{executor_}; 99 | 100 | double period_s_ = 1.0; 101 | boost::signals2::signal data_signal_; 102 | }; 103 | 104 | SystemInfo::SystemInfo(base::Context& context) 105 | : impl_(std::make_unique(context)) {} 106 | 107 | SystemInfo::~SystemInfo() {} 108 | 109 | void SystemInfo::AsyncStart(mjlib::io::ErrorCallback callback) { 110 | impl_->AsyncStart(std::move(callback)); 111 | } 112 | 113 | clipp::group SystemInfo::program_options() { 114 | return impl_->program_options(); 115 | } 116 | 117 | } 118 | } 119 | -------------------------------------------------------------------------------- /mech/system_info.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include "base/context.h" 22 | 23 | namespace mjmech { 24 | namespace mech { 25 | 26 | /// Record information about the system on a periodic basis. 27 | class SystemInfo : boost::noncopyable { 28 | public: 29 | SystemInfo(base::Context&); 30 | ~SystemInfo(); 31 | 32 | void AsyncStart(mjlib::io::ErrorCallback); 33 | 34 | clipp::group program_options(); 35 | 36 | private: 37 | class Impl; 38 | std::unique_ptr impl_; 39 | }; 40 | 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /mech/test/test_main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2014-2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #define BOOST_TEST_MODULE mech 16 | #include 17 | -------------------------------------------------------------------------------- /mech/trajectory.h: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include "base/point3d.h" 18 | 19 | namespace mjmech { 20 | namespace mech { 21 | 22 | struct TrajectoryState { 23 | base::Point3D pose_l; 24 | base::Point3D velocity_l_s; 25 | base::Point3D acceleration_l_s2; 26 | }; 27 | 28 | TrajectoryState CalculateAccelerationLimitedTrajectory( 29 | const TrajectoryState& start, 30 | const base::Point3D& target_l, 31 | double target_velocity_l_s, 32 | double max_acceleration_l_s2, 33 | double delta_s); 34 | 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /mech/web_control_assets/mjbots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/mech/web_control_assets/mjbots.png -------------------------------------------------------------------------------- /mech/web_server.h: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Josh Pieper, jjp@pobox.com. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "mjlib/io/async_types.h" 28 | 29 | namespace mjmech { 30 | namespace mech { 31 | 32 | /// An embedded web server that can serve static pages, as well as 33 | /// websocket connections. The server itself is run in a background 34 | /// thread. 35 | class WebServer { 36 | public: 37 | using WebsocketStream = 38 | boost::beast::websocket::stream; 39 | using WebsocketHandler = std::function; 40 | 41 | struct Options { 42 | std::string address = "0.0.0.0"; 43 | int port = -1; 44 | 45 | struct Root { 46 | /// The URL prefix which is used for this root. 47 | std::string prefix; 48 | 49 | /// The location in the filesystem to serve these files. 50 | std::string root; 51 | }; 52 | 53 | /// One or more document roots. Resources that match multiple 54 | /// prefixes are tried in order. 55 | std::vector document_roots; 56 | 57 | /// The following URL prefixes will be treated as exclusively 58 | /// websocket endpoints. No websocket "sub-protocols" are 59 | /// supported. Once the websocket connection has been upgraded, 60 | /// the given handler will be invoked. These websockets are given 61 | /// an executor that runs in a background thread, and may only 62 | /// execute in that thread. 63 | struct Websocket { 64 | std::string endpoint; 65 | 66 | WebsocketHandler handler; 67 | }; 68 | 69 | std::vector websocket_handlers; 70 | }; 71 | 72 | WebServer(const boost::asio::any_io_executor&, const Options&); 73 | ~WebServer(); 74 | 75 | void AsyncStart(mjlib::io::ErrorCallback); 76 | 77 | private: 78 | class Impl; 79 | std::unique_ptr impl_; 80 | }; 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /tools/bazel: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 -B 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import re 19 | import shutil 20 | import subprocess 21 | import sys 22 | import tempfile 23 | 24 | SCRIPT_PATH = os.path.basename(__file__) 25 | 26 | def find_workspace_dir(): 27 | path = SCRIPT_PATH 28 | while True: 29 | maybe_workspace = os.path.join(path, 'WORKSPACE') 30 | if os.path.exists(maybe_workspace): 31 | return path 32 | 33 | assert path != '' 34 | 35 | old_path = path 36 | path = os.path.dirname(path) 37 | assert path != old_path 38 | 39 | 40 | WORKSPACE_DIR = find_workspace_dir() 41 | 42 | def maybe_copy(maybe_filename, dest): 43 | if not os.path.exists(maybe_filename): 44 | return 45 | 46 | with open(maybe_filename, 'rb') as f: 47 | dest.write(f.read()) 48 | 49 | 50 | def get_workspace_var(var): 51 | search = re.compile(r'^{}\s*=\s*"(.*)"'.format(var)) 52 | 53 | with open(os.path.join(WORKSPACE_DIR, 'WORKSPACE'), 'r') as f: 54 | for line in f: 55 | match = re.search(search, line) 56 | if not match: 57 | continue 58 | 59 | return match.group(1) 60 | 61 | raise RuntimeError('Must supply a {} in the WORKSPACE'.format(var)) 62 | 63 | 64 | BAZEL_REMOTE_SOURCE = 'https://github.com/bazelbuild/bazel/releases/download' 65 | 66 | BAZEL_BIN_CACHE = os.path.expanduser('~/.cache/bazel') 67 | BAZEL_VERSION = get_workspace_var('BAZEL_VERSION') 68 | BAZEL_VERSION_SHA = get_workspace_var('BAZEL_VERSION_SHA') 69 | 70 | 71 | def download_bazel(dest): 72 | dist_tempfile = tempfile.NamedTemporaryFile(prefix='bazeldl-', delete=False) 73 | 74 | url = '{}/{}/bazel-{}-linux-x86_64'.format( 75 | BAZEL_REMOTE_SOURCE, BAZEL_VERSION, BAZEL_VERSION) 76 | 77 | print('Downloading bazel {} from {}'.format( 78 | BAZEL_VERSION, url), 79 | file=sys.stderr, flush=True) 80 | 81 | subprocess.check_call(['curl', '-L', url, '-o', dist_tempfile.name]) 82 | 83 | actual_sha256 = subprocess.check_output([ 84 | 'sha256sum', dist_tempfile.name]).decode('utf-8').split(' ')[0] 85 | 86 | if actual_sha256 != BAZEL_VERSION_SHA.lower(): 87 | raise RuntimeError( 88 | 'bazel sha256 does not match, expected {} got {}'.format( 89 | BAZEL_VERSION_SHA, actual_sha256)) 90 | 91 | subprocess.check_call(["chmod", "+x", dist_tempfile.name]) 92 | shutil.move(dist_tempfile.name, dest) 93 | 94 | 95 | def main(): 96 | bazel_bin_loc = os.path.join(BAZEL_BIN_CACHE, BAZEL_VERSION) 97 | 98 | if not os.path.exists(bazel_bin_loc): 99 | os.makedirs(os.path.dirname(bazel_bin_loc), exist_ok=True) 100 | download_bazel(bazel_bin_loc) 101 | 102 | os.execv(bazel_bin_loc, ['bazel', ] + sys.argv[1:]) 103 | assert False 104 | 105 | 106 | if __name__ == '__main__': 107 | main() 108 | -------------------------------------------------------------------------------- /tools/workspace/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/BUILD -------------------------------------------------------------------------------- /tools/workspace/bazel_deps/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/bazel_deps/BUILD -------------------------------------------------------------------------------- /tools/workspace/bazel_deps/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def bazel_deps_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "mjbots/bazel_deps", 23 | commit = "15ac6df1c4513d0fcce947ab4f46c5ffdd6fa0f2", 24 | sha256 = "7582c8a89470c835de65b7509320dda4069eea50d628774556841fcf1e312d8e", 25 | ) 26 | -------------------------------------------------------------------------------- /tools/workspace/default.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace/bazel_deps:repository.bzl", "bazel_deps_repository") 18 | load("//tools/workspace/gst-rpicamsrc:repository.bzl", "gst_rpicamsrc_repository") 19 | load("//tools/workspace/i2c-tools:repository.bzl", "i2c_tools_repository") 20 | load("//tools/workspace/implot:repository.bzl", "implot_repository") 21 | load("//tools/workspace/mjlib:repository.bzl", "mjlib_repository") 22 | load("//tools/workspace/moteus:repository.bzl", "moteus_repository") 23 | load("//tools/workspace/pi3hat:repository.bzl", "pi3hat_repository") 24 | load("//tools/workspace/raspicam:repository.bzl", "raspicam_repository") 25 | load("//tools/workspace/raspberrypi-firmware:repository.bzl", "raspberrypi_firmware_repository") 26 | load("//tools/workspace/rpi_bazel:repository.bzl", "rpi_bazel_repository") 27 | load("//tools/workspace/rules_pkg:repository.bzl", "rules_pkg_repository") 28 | load("//tools/workspace/sophus:repository.bzl", "sophus_repository") 29 | 30 | def add_default_repositories(excludes = []): 31 | if not native.existing_rule("com_github_mjbots_bazel_deps"): 32 | bazel_deps_repository(name = "com_github_mjbots_bazel_deps") 33 | if not native.existing_rule("gst-rpicamsrc"): 34 | gst_rpicamsrc_repository(name = "gst-rpicamsrc") 35 | if not native.existing_rule("i2c-tools"): 36 | i2c_tools_repository(name = "i2c-tools") 37 | if not native.existing_rule("implot"): 38 | implot_repository(name = "implot") 39 | if not native.existing_rule("com_github_mjbots_mjlib"): 40 | mjlib_repository(name = "com_github_mjbots_mjlib") 41 | if not native.existing_rule("moteus"): 42 | moteus_repository(name = "moteus") 43 | if not native.existing_rule("pi3hat"): 44 | pi3hat_repository(name = "pi3hat") 45 | if not native.existing_rule("raspicam"): 46 | raspicam_repository(name = "raspicam") 47 | if not native.existing_rule("raspberrypi-firmware"): 48 | raspberrypi_firmware_repository(name = "raspberrypi-firmware") 49 | if not native.existing_rule("rpi_bazel"): 50 | rpi_bazel_repository(name = "rpi_bazel") 51 | if not native.existing_rule("rules_pkg"): 52 | rules_pkg_repository(name = "rules_pkg") 53 | if not native.existing_rule("sophus"): 54 | sophus_repository(name = "sophus") 55 | -------------------------------------------------------------------------------- /tools/workspace/generate_file.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | def _generate_file_impl(ctx): 4 | out = ctx.actions.declare_file(ctx.label.name) 5 | ctx.actions.write(out, ctx.attr.content, ctx.attr.is_executable) 6 | return [DefaultInfo( 7 | files = depset([out]), 8 | data_runfiles = ctx.runfiles(files = [out]), 9 | )] 10 | 11 | generate_file = rule( 12 | attrs = { 13 | "content": attr.string(mandatory = True), 14 | "is_executable": attr.bool(default = False), 15 | }, 16 | output_to_genfiles = True, 17 | implementation = _generate_file_impl, 18 | ) 19 | 20 | """Generate a file with specified content. 21 | 22 | This creates a rule to generate a file with specified content (which is either 23 | static or has been previously computed). 24 | 25 | Args: 26 | content (:obj:`str`): Desired content of the generated file. 27 | """ 28 | -------------------------------------------------------------------------------- /tools/workspace/github_archive.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 18 | 19 | def github_archive(name, repo, commit, local_override=None, 20 | sha256=None, **kwargs): 21 | """Like 'http_archive', but for github repositories. 22 | 23 | If 'local_override' is set, then reference a local repository at 24 | the given path instead of github.com. 25 | """ 26 | 27 | if local_override: 28 | if 'build_file' in kwargs: 29 | build_file = kwargs['build_file'] 30 | build_path = build_file.package + '/' + build_file.name 31 | native.new_local_repository( 32 | name = name, 33 | build_file = build_path, 34 | path = local_override, 35 | ) 36 | else: 37 | native.local_repository( 38 | name = name, 39 | path = local_override, 40 | ) 41 | else: 42 | http_archive( 43 | name = name, 44 | url = "https://github.com/{repo}/archive/{commit}.zip".format( 45 | repo=repo, commit=commit), 46 | strip_prefix = "{}-{}".format(repo.rsplit('/', 1)[-1], commit), 47 | sha256 = sha256 or "0000000000000000000000000000000000000000000000000000000000000000", 48 | **kwargs) 49 | -------------------------------------------------------------------------------- /tools/workspace/gst-rpicamsrc/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/gst-rpicamsrc/BUILD -------------------------------------------------------------------------------- /tools/workspace/gst-rpicamsrc/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 18 | 19 | 20 | def gst_rpicamsrc_repository(name): 21 | commit = "a181cd8d4b284d09b5f0e23d9ddb4f0a94e1dc8c" 22 | http_archive( 23 | name = name, 24 | url = "https://github.com/thaytan/gst-rpicamsrc/archive/{}.zip".format(commit), 25 | sha256 = "8ebbd6736b8f396e9cef362d4e0a927f15fc97a133cadf7f8acb6cf06ad310a7", 26 | strip_prefix = "gst-rpicamsrc-{}".format(commit), 27 | build_file = Label("//tools/workspace/gst-rpicamsrc:package.BUILD"), 28 | ) 29 | -------------------------------------------------------------------------------- /tools/workspace/i2c-tools/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/i2c-tools/BUILD -------------------------------------------------------------------------------- /tools/workspace/i2c-tools/package.BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | cc_library( 20 | name = "i2c-tools", 21 | hdrs = glob(['include/**/*.h']), 22 | strip_include_prefix = "include", 23 | srcs = ["lib/smbus.c"], 24 | ) 25 | -------------------------------------------------------------------------------- /tools/workspace/i2c-tools/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 18 | 19 | 20 | def i2c_tools_repository(name): 21 | http_archive( 22 | name = name, 23 | urls = [ 24 | "https://mirrors.edge.kernel.org/pub/software/utils/i2c-tools/i2c-tools-4.0.tar.xz", 25 | ], 26 | sha256 = "d900ca1c11c51ea20caa50b096f948008b8a7ad832311b23353e21baa7af28d6", 27 | strip_prefix = "i2c-tools-4.0", 28 | build_file = Label("//tools/workspace/i2c-tools:package.BUILD"), 29 | ) 30 | -------------------------------------------------------------------------------- /tools/workspace/implot/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/implot/BUILD -------------------------------------------------------------------------------- /tools/workspace/implot/empty-legend.diff: -------------------------------------------------------------------------------- 1 | diff --git a/implot.cpp b/implot.cpp 2 | index 21bd171..c02f47f 100644 3 | --- a/implot.cpp 4 | +++ b/implot.cpp 5 | @@ -628,9 +628,11 @@ ImPlotItem* RegisterItem(const char* label_id) { 6 | item->SeenThisFrame = true; 7 | int idx = gp.CurrentPlot->Items.GetIndex(item); 8 | item->ID = id; 9 | - gp.LegendIndices.push_back(idx); 10 | - item->NameOffset = gp.LegendLabels.size(); 11 | - gp.LegendLabels.append(label_id, label_id + strlen(label_id) + 1); 12 | + if (ImGui::FindRenderedTextEnd(label_id, NULL) != label_id) { 13 | + gp.LegendIndices.push_back(idx); 14 | + item->NameOffset = gp.LegendLabels.size(); 15 | + gp.LegendLabels.append(label_id, label_id + strlen(label_id) + 1); 16 | + } 17 | if (item->Show) 18 | gp.VisibleItemCount++; 19 | return item; 20 | -------------------------------------------------------------------------------- /tools/workspace/implot/package.BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | cc_library( 20 | name = "implot", 21 | hdrs = [ 22 | "implot.h", 23 | ], 24 | srcs = [ 25 | "implot.cpp", 26 | "implot_demo.cpp", 27 | ], 28 | includes = ["."], 29 | deps = ["@imgui"], 30 | ) 31 | -------------------------------------------------------------------------------- /tools/workspace/implot/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def implot_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "epezent/implot", 23 | commit = "9894df49348a8de412fd3b52ff7bfb322dbeede2", 24 | sha256 = "f5f655ef6d6c167c978373dcd8a53f7188d99ce768a20156fc1e042167a7c296", 25 | build_file = Label("//tools/workspace/implot:package.BUILD"), 26 | patches = [Label("//tools/workspace/implot:empty-legend.diff")], 27 | patch_args = ["-p1"], 28 | ) 29 | -------------------------------------------------------------------------------- /tools/workspace/mjlib/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/mjlib/BUILD -------------------------------------------------------------------------------- /tools/workspace/mjlib/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2022 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def mjlib_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "mjbots/mjlib", 23 | commit = "f5d2bf1cc65e92d99f70dbf8f0d0108e8abec5d1", 24 | sha256 = "031a7301ee7f23fcd7eb79218dd38ee7858ea739652b076ff464fc74df49cce5", 25 | ) 26 | -------------------------------------------------------------------------------- /tools/workspace/moteus/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/moteus/BUILD -------------------------------------------------------------------------------- /tools/workspace/moteus/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def moteus_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "mjbots/moteus", 23 | commit = "fe3a19f2c05de9aa36017f7e21f95a2bca362c75", 24 | sha256 = "361969521fc0165f78d78fb918642562b58f5b55a42fc5428aece70906e8daeb", 25 | ) 26 | -------------------------------------------------------------------------------- /tools/workspace/pi3hat/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/pi3hat/BUILD -------------------------------------------------------------------------------- /tools/workspace/pi3hat/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def pi3hat_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "mjbots/pi3hat", 23 | commit = "ab632c82bd501b9fcb6f8200df0551989292b7a1", 24 | sha256 = "45bf7f021214eb1b9390e3d06e8e0afa3d243f5b739fae776dd22c855c178a09", 25 | ) 26 | -------------------------------------------------------------------------------- /tools/workspace/raspberrypi-firmware/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/raspberrypi-firmware/BUILD -------------------------------------------------------------------------------- /tools/workspace/raspberrypi-firmware/package.BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | cc_library( 20 | name = "bcm_host", 21 | hdrs = [ 22 | "hardfp/opt/vc/include/bcm_host.h", 23 | ] + glob([ 24 | "hardfp/opt/vc/include/interface/**/*.h", 25 | "hardfp/opt/vc/include/vcinclude/**/*.h", 26 | ]), 27 | srcs = ["hardfp/opt/vc/lib/libbcm_host.so"], 28 | includes = [ 29 | "hardfp/opt/vc/include", 30 | ], 31 | ) 32 | 33 | cc_library( 34 | name = "raspberrypi-firmware", 35 | hdrs = glob(["hardfp/opt/vc/include/**/*.h"]), 36 | srcs = glob(["hardfp/opt/vc/lib/*.so"]), 37 | includes = [ 38 | "hardfp/opt/vc/include", 39 | "hardfp/opt/vc/include/interface/vcos", 40 | "hardfp/opt/vc/include/interface/mmal", 41 | ], 42 | ) 43 | -------------------------------------------------------------------------------- /tools/workspace/raspberrypi-firmware/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 18 | 19 | 20 | def raspberrypi_firmware_repository(name): 21 | http_archive( 22 | name = name, 23 | url = "https://github.com/raspberrypi/firmware/archive/1.20200601.tar.gz", 24 | sha256 = "d826cdfdcf5931b5ccdcf89b206a83983bea8c94ec349552eeccdd20666430c0", 25 | strip_prefix = "firmware-1.20200601", 26 | build_file = Label("//tools/workspace/raspberrypi-firmware:package.BUILD"), 27 | ) 28 | -------------------------------------------------------------------------------- /tools/workspace/raspicam/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/raspicam/BUILD -------------------------------------------------------------------------------- /tools/workspace/raspicam/package.BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2019 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | cc_library( 20 | name = "raspicam", 21 | hdrs = ['src/' + x for x in [ 22 | 'raspicam.h', 23 | 'raspicam_cv.h', 24 | 'raspicam_still.h', 25 | 'raspicam_still_cv.h', 26 | 'raspicamtypes.h', 27 | 'scaler.h', 28 | ]], 29 | srcs = ['src/' + x for x in [ 30 | 'raspicam.cpp', 31 | 'raspicam_cv.cpp', 32 | 'raspicam_still.cpp', 33 | 'raspicam_still_cv.cpp', 34 | 'private/exceptions.h', 35 | 'private/private_impl.cpp', 36 | 'private/private_impl.h', 37 | 'private/private_types.h', 38 | 'private/threadcondition.h', 39 | 'private/threadcondition.cpp', 40 | 'private_still/private_still_impl.cpp', 41 | 'private_still/private_still_impl.h', 42 | ]] + glob(['dependencies/**/*.h']), 43 | deps = [ 44 | "@opencv//:videoio", 45 | "@raspberrypi-firmware", 46 | ], 47 | includes = ["src", "dependencies"], 48 | copts = [ 49 | "-Wno-dynamic-exception-spec", 50 | "-Wno-unused-private-field", 51 | ], 52 | ) 53 | -------------------------------------------------------------------------------- /tools/workspace/raspicam/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2019 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def raspicam_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "cedricve/raspicam", 23 | commit = "651c56418a5a594fc12f1414eb14f2b899729cb1", 24 | sha256 = "35348ef9556aa3ebe7a5f6571fb586e9c5c1fbd1290b22245ffcee1fb2e1582c", 25 | build_file = Label("//tools/workspace/raspicam:package.BUILD"), 26 | patches = [ 27 | Label("//tools/workspace/raspicam:raspicam.diff"), 28 | ], 29 | patch_args = ["-p1"], 30 | ) 31 | -------------------------------------------------------------------------------- /tools/workspace/rpi_bazel/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/rpi_bazel/BUILD -------------------------------------------------------------------------------- /tools/workspace/rpi_bazel/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def rpi_bazel_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "mjbots/rpi_bazel", 23 | commit = "3f9e6245972dfb48dffc04d5c520b799d07ea59c", 24 | sha256 = "fbbd18a9358b67eef091ebf8fffcfc03f0c3d5b582dd5b084081ce573c2dfda0", 25 | ) 26 | -------------------------------------------------------------------------------- /tools/workspace/rules_pkg/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/rules_pkg/BUILD -------------------------------------------------------------------------------- /tools/workspace/rules_pkg/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive") 18 | 19 | 20 | def rules_pkg_repository(name): 21 | http_archive( 22 | name = name, 23 | urls = [ 24 | "https://github.com/bazelbuild/rules_pkg/releases/download/0.2.4/rules_pkg-0.2.4.tar.gz", 25 | ], 26 | sha256 = "4ba8f4ab0ff85f2484287ab06c0d871dcb31cc54d439457d28fd4ae14b18450a", 27 | ) 28 | -------------------------------------------------------------------------------- /tools/workspace/sophus/BUILD: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mjbots/hoverbot/83acee48e6648176f81c7ee471fe97089372dfb5/tools/workspace/sophus/BUILD -------------------------------------------------------------------------------- /tools/workspace/sophus/package.BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2019 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | cc_library( 20 | name = "sophus", 21 | hdrs = glob(['sophus/**/*.hpp']), 22 | deps = ["@eigen"], 23 | includes = ["."], 24 | ) 25 | -------------------------------------------------------------------------------- /tools/workspace/sophus/repository.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2019 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | load("//tools/workspace:github_archive.bzl", "github_archive") 18 | 19 | def sophus_repository(name): 20 | github_archive( 21 | name = name, 22 | repo = "strasdat/Sophus", 23 | commit = "ef9551ff429899b5adae66eabd5a23f165953199", 24 | sha256 = "b5a260f5db7ace1718e9bd44c21fb1a8588e1fb05ae0da29e04bb0eca1906143", 25 | build_file = Label("//tools/workspace/sophus:package.BUILD"), 26 | ) 27 | -------------------------------------------------------------------------------- /tools/workspace/template_file.bzl: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | def _dictify(data): 18 | result = dict([x.split('=', 1) for x in data]) 19 | return result 20 | 21 | def _template_file_impl(ctx): 22 | out = ctx.actions.declare_file(ctx.label.name) 23 | 24 | substitutions = dict(ctx.attr.substitutions) 25 | substitutions.update(_dictify(ctx.attr.substitution_list)) 26 | 27 | ctx.actions.expand_template( 28 | template = ctx.files.src[0], 29 | output = out, 30 | substitutions = substitutions, 31 | is_executable = ctx.attr.is_executable) 32 | return [DefaultInfo( 33 | files = depset([out]), 34 | data_runfiles = ctx.runfiles(files = [out]), 35 | )] 36 | 37 | 38 | template_file = rule( 39 | attrs = { 40 | "src": attr.label(allow_files = True), 41 | "is_executable": attr.bool(default = False), 42 | "substitutions": attr.string_dict(), 43 | "substitution_list": attr.string_list(), 44 | }, 45 | output_to_genfiles = True, 46 | implementation = _template_file_impl, 47 | ) 48 | -------------------------------------------------------------------------------- /tools/workspace_status.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | GIT_REV=$(git rev-parse HEAD) 4 | if [[ $? != 0 ]]; 5 | then 6 | exit 1 7 | fi 8 | 9 | echo "BUILD_SCM_REVISION ${GIT_REV}" 10 | 11 | # Check whether there are any uncommitted changes. 12 | git diff-index --quiet HEAD -- 13 | if [[ $? == 0 ]]; 14 | then 15 | TREE_STATUS=0 16 | else 17 | TREE_STATUS=1 18 | fi 19 | 20 | echo "BUILD_SCM_STATUS ${TREE_STATUS}" 21 | -------------------------------------------------------------------------------- /travis-ci.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import subprocess 4 | import sys 5 | 6 | def run_all(cmds): 7 | for cmd in [x for x in cmds.split('\n') if x.strip() != '']: 8 | print(">>>", cmd) 9 | subprocess.run(cmd, shell=True) 10 | 11 | 12 | ALWAYS = """ 13 | sudo apt-get update 14 | ./install-packages --yes 15 | """ 16 | 17 | 18 | CMDS = [ 19 | "./tools/bazel test //base/...", 20 | "./tools/bazel build @ffmpeg//...", 21 | "./tools/bazel build @opencv//...", 22 | "./tools/bazel build @dart//...", 23 | "./tools/bazel build @gstreamer//... @gst-libav//... @gst-plugins-bad//... @gst-plugins-base//... @gst-plugins-ugly//... @gst-plugins-base//:plugins", 24 | "./tools/bazel test //...", 25 | ] 26 | 27 | def main(): 28 | run_all(ALWAYS) 29 | 30 | if len(sys.argv) < 2: 31 | [run_all(x) for x in CMDS] 32 | else: 33 | run_all(CMDS[int(sys.argv[1])]) 34 | 35 | 36 | if __name__ == '__main__': 37 | main() 38 | -------------------------------------------------------------------------------- /utils/BUILD: -------------------------------------------------------------------------------- 1 | # -*- python -*- 2 | 3 | # Copyright 2018-2020 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | package(default_visibility = ["//visibility:public"]) 18 | 19 | exports_files([ 20 | "config_servos.py", 21 | "performance_governor.sh", 22 | "hoverbot-start.sh", 23 | "hoverbot_screen.conf", 24 | "hoverbot.cmd", 25 | "start-robot.sh", 26 | ]) 27 | -------------------------------------------------------------------------------- /utils/config_servos.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2020-2022 Josh Pieper, jjp@pobox.com. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | '''Set servo-level configuration for a quad A1 robot.''' 18 | 19 | 20 | import argparse 21 | import asyncio 22 | import moteus 23 | import moteus_pi3hat 24 | import os 25 | import subprocess 26 | import sys 27 | import tempfile 28 | 29 | 30 | SCRIPT_PATH = os.path.dirname(__file__) 31 | 32 | CONFIG = { 33 | 'servopos.position_min' : 'nan', 34 | 'servopos.position_max' : 'nan', 35 | 'servo.flux_brake_min_voltage' : '20.5', 36 | 'servo.flux_brake_resistance_ohm' : '0.05', 37 | 'servo.pid_position.kp' : '4', 38 | 'servo.pid_position.kd' : '0.05', 39 | 40 | 'aux2.spi.mode' : '1', # disabled 41 | 'aux2.pins.0.mode' : '6', # hall 42 | 'aux2.pins.1.mode' : '6', # hall 43 | 'aux2.pins.2.mode' : '6', # hall 44 | 'aux2.pins.0.pull' : '1', # pullup 45 | 'aux2.pins.1.pull' : '1', # pullup 46 | 'aux2.pins.2.pull' : '1', # pullup 47 | 'aux2.hall.enabled' : '1', 48 | 49 | 'motor_position.sources.0.aux_number' : '2', 50 | 'motor_position.sources.0.type' : '4', 51 | 'motor_position.sources.0.cpr' : '90', 52 | 'motor_position.sources.0.offset' : '-4', 53 | 'motor_position.sources.0.pll_filter_hz' : '45', 54 | 'motor.poles' : '30', 55 | 'motor.resistance_ohm' : '0.326', 56 | 'motor.v_per_hz' : '1.293', 57 | } 58 | 59 | async def config_servo(args, transport, servo_id): 60 | if args.verbose: 61 | print(f"*** SERVO {servo_id} ***") 62 | 63 | c = moteus.Controller(id=servo_id, transport=transport) 64 | s = moteus.Stream(c, verbose=args.verbose) 65 | 66 | await s.flush_read() 67 | 68 | for key, data_or_value in CONFIG.items(): 69 | try: 70 | if type(data_or_value) == str: 71 | value = data_or_value 72 | await s.command( 73 | "conf set {} {}".format(key, value).encode('utf8')) 74 | else: 75 | for servo_selector, value in data_or_value: 76 | ids = set([int(x) for x in servo_selector.split(',')]) 77 | if not servo_id in ids: 78 | continue 79 | await s.command( 80 | "conf set {} {}".format(key, value).encode('utf8')) 81 | except: 82 | print(f"While setting {key} / {data_or_value}") 83 | raise 84 | 85 | await s.command(b'conf write') 86 | 87 | 88 | async def main(): 89 | parser = argparse.ArgumentParser() 90 | parser.add_argument('-v', '--verbose', action='store_true') 91 | 92 | args = parser.parse_args() 93 | 94 | if os.geteuid() != 0: 95 | raise RuntimeError('This must be run as root') 96 | 97 | transport = moteus_pi3hat.Pi3HatRouter( 98 | servo_bus_map = { 99 | 1: [1], 100 | 3: [2], 101 | }, 102 | ) 103 | 104 | for i in range(1, 3): 105 | await config_servo(args, transport, i) 106 | 107 | if __name__ == '__main__': 108 | asyncio.run(main()) 109 | -------------------------------------------------------------------------------- /utils/hoverbot-start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | screen -d -m -c /home/pi/hoverbot/hoverbot_screen.conf 4 | -------------------------------------------------------------------------------- /utils/hoverbot.cmd: -------------------------------------------------------------------------------- 1 | export LD_LIBRARY_PATH=. 2 | cd /home/pi/hoverbot; ./start-robot.sh 3 | -------------------------------------------------------------------------------- /utils/hoverbot_screen.conf: -------------------------------------------------------------------------------- 1 | screen 1 bash --init-file /home/pi/hoverbot/hoverbot.cmd 2 | 3 | detach 4 | -------------------------------------------------------------------------------- /utils/performance_governor.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | for a in /sys/devices/system/cpu/cpu?/cpufreq/scaling_governor; do 6 | echo "performance" > $a 7 | done 8 | -------------------------------------------------------------------------------- /utils/ssh/default.ssh: -------------------------------------------------------------------------------- 1 | -----BEGIN RSA PRIVATE KEY----- 2 | MIIJKQIBAAKCAgEA4enzTiabiExId1HheWUDT8JJ1fRQ0V079Zextsn46ynVhC3k 3 | q/j0tbVU+es+WRzMff7DN0c+4gJZ0L3F/aYZ0uZzbpu2TCFhBzl/n4a7bx+ELb4h 4 | KvgrWCqhyfwW7tlhSh4femn0KbP64sHkOKlgOquRkcB611mr94AhMBVJfbvH5fPu 5 | ZnnaA+r2eX9CDEe6RoLI0u1w9bbPcXu2q/ciS7iqDNgG4KCTNUizAtLo5yhzTAXq 6 | dkxnPzw3QbjwO5cFuCefF1tXuMw9nr2sxyCN/UHnzedU7DAZFdylXEmGrK4LxlOE 7 | MEBEPJifL05rvZ57du5nvPO7hUJLyIExcErP4A8//e5613EY+nQkFF1gkOuaNHQv 8 | 3NuDryQ49vMxqOO0ogI4VtDx4dAUk7HzzSd54hQwojyU4dPF6IoISBq6Q213WdA7 9 | 6glqYQs+3TbvUGfnOkVej38SkPd86npDdPCO1IGya8Q8PkfS27SzztQ9y1TO2p+S 10 | Y99cFGL9PBulyL7t1K7l+nyWIMN5bpgEPhcAUii/tcnZeS17n2CZZmaiS0d7rbI5 11 | 2km3IjYAeWRW+jb2GfACzPO0A8X/R2hBh2n5VilVtcoi5kJu+gqihgzcGVcXpmvu 12 | BibnrUemxxzvEwGO6Ovalf9DmCn18bSHHMUQG2VcOgafTF84PjGjaAB/MW8CAwEA 13 | AQKCAgAkoCo69FVo3n0tIxA7/x1vU99mmYp0tvneki5UpR/++DROjqYxH1aFExXo 14 | wrPD/MBGeQQrNRjDyX/IMMwlmEU9hPEX8JS4ePzjD/ZZ5Z0Dogdq5xU3gy57x8Wd 15 | GoqZRJLaUlHmJcFY9X3qP2cBrKwWNvSNvbfsq/QOVy6KPhlBEi2RYV+4K28mDXTS 16 | M8iAdyyC+/cVAzu6Cl5NVrJB5PohbpnbJTbQOhhDnjqvbYQm3c77d0YQ0cg3X0rS 17 | E+qWrqBVv86yGbP6GNqwnq2hRuzeL04meqew9wdKHRCOCrmTy0/LYoTe0bx+WnwM 18 | Tll3T3dSy8nzv0waiaoBorhiLS7QJW13lzcDe3k+5JP02vW2yJFpSUqMp7u7KlOn 19 | JfKSPIbHFpyYHEPHLIbF+HsxbexBdDCkAkAfHaaxFMI6jlBdxgoY6nIrEINqs9vN 20 | JFUmvdJ55Z+mSMULa0qaejoBet8UuS2IGzgRh9Fn6hW6VOz9XCGZNKIOGTQ3Ijtk 21 | SJpYYPKCVqgIYCDQZzN5e5SSn3Eo3Nu33wIri0gqbNqhCME4doBzTp14mgOYB0Wo 22 | ell7hpbkef69SUFUP9Sc+9qX6FKA70ki4vqBubDiBOrSp2ZMps1gHXF3zVCRjdlL 23 | hTlnC+dbh8LnG8zGJOMHawzoPRLcEOYXQ3AcIeuYkb/gVUVBEQKCAQEA/rlw+CZO 24 | s7IhS6bgGtuCDw72SJM45N9bPQhj1xR6rHFoswU/hcMuyVmY4d5XHjFAHUj5WlZv 25 | lhSvM7pfRV4muJObjm9NsSVAAaqDI9b6uHPTh0vEabhZZHe7xBfCdt9w2Al80rCB 26 | 5hnf1jTRvrw1yrUlhUVL6DfW4hh1QqLIBA3mrJOR8Wv5ECfxRmGBW3zq98rKJERO 27 | mV0nzrDzpJHqGp3NU43P66xQjPq3D0x+QNWOp3Wm2K6BnFchVj2aChlPLGLGNQ8v 28 | 7NtER3DwbUg7NswRGxORQQgSuy2Kx7FB/JZgFYjLOJMajJA6p7v5a3DV/AjMjWZk 29 | 5lAG461sGyKvhwKCAQEA4wuS5aR8V/8ElHuH/SnFiUtGx3Ti8Eu8cjFuOznrngw5 30 | 7SsTCEzCGhRI3yOCliVA4OebUkdHrBJzP5/Saa9hGZssO8w8rLt/jVBh4Ls1i2Bs 31 | BEU/pAF5Ua0sFuWdTLAZCXne5yIKf2JMt5e5JZ4OdwTVdJ+rriD6ulhwr/CxF4Tx 32 | hde+lG+ztgUSExww13e79DOSXE+85CqqbqIGG8KfZJk2ewlBYvoSkrHekgABnBn2 33 | /KAm/0Y8uDmb//R27gtqI/SkdEBXeJIy9dfkdDWWDuYNekHQgk7AxqodHltfmdzd 34 | MBVwgKy7vNYCp8h7E8VcToFfB0WvNjibNuqJiq5Y2QKCAQBGzfM59yP/TS92IO/A 35 | Z23Sk70f1VSGQhPRdkiSW43x6yBIStgqbWPO8knN1wjuSpBg040EcGxaEmvU/CRJ 36 | Wu92dQlTYVr+N7WAAQ6s4rAUn5WX0RbKsESg6atWyafzdQ6f7ps00X16IY0XtNzs 37 | oNJvA0VnsyhzZzoi5NuLZ0Yj4l1RcT+rFPFJbWDiyG21qZaCxGrGA/q5JKzRBlzj 38 | TO/+8AhIBm5aOy600WgbJ48cpIAfylNTfcZnV8zbdQohboGUe+xOiWcv9o8XMKyU 39 | I/TZuYWzug+xPPY/81vVQE9wT7hro/ehRTqnAdxcDaM6G0ZP5X4+e8zmPC5+ERGw 40 | xUZ3AoIBAQC9Hf3dFLaSIZRyDbeHU4z7wF0TdLQ3Tg6I282ivasZ+bsiZzeyOSWQ 41 | zYapfw2d/Sfebmz9CuyTUsb2nyyH5ZKu3oboxEVh58fmMKdmpumBz+4JXJkXBjdE 42 | pkdMRzRQFXq1XvMheURfOMUIQ/Pvdok78CXQv9Mj9sqB/3rVwlnwakc3oi5Xu/do 43 | TS7yBhBU2aIQVgi6pZyEPVm7T3BhHSrhg5FSyLHTPNPNpyTqXeD5YtNNOCVjiuC2 44 | 3x2nBL0vqQhfLL7zKkjx4G1cQ9lCRwRYIp3lUP7aG7kNgdNBXwDxy3veOm18cOTr 45 | 9SM/GIoUPNeXeMFr8EpykpIPGys3/eN5AoIBAQCq0WabHY+6DnJKnOdfbY73cHpZ 46 | bv5vnAybadpotZC1rrPrjHrTssOUKSQyj6GnHo5kSeDky+v8QY1yujhwkzNdF9lG 47 | EjGVtCXTEXd5msS6/0tCamoihF8QK4ly/EQTYbQi/QbZwOqJmqDHwOn+XB0dOkGh 48 | Q9BWC94yD9dBUvHxUOptypv6K47Bff2gJBR6l7zEMbtQt4VA0bSuroi/PFPEBiRK 49 | JkPkAi5XOuBzbfsHsGVfLMelMoXKtmv3ELaWl7u4jd7fV+7HYOgbL6Ge6t3n79+s 50 | 7Czp+BhauaqJ3Nw7Glu9JPRNMjN/WxXITp3u2pREIYz3Aj5MF8PYXV3PePU7 51 | -----END RSA PRIVATE KEY----- 52 | -------------------------------------------------------------------------------- /utils/ssh/default.ssh.pub: -------------------------------------------------------------------------------- 1 | ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAACAQDh6fNOJpuITEh3UeF5ZQNPwknV9FDRXTv1l7G2yfjrKdWELeSr+PS1tVT56z5ZHMx9/sM3Rz7iAlnQvcX9phnS5nNum7ZMIWEHOX+fhrtvH4QtviEq+CtYKqHJ/Bbu2WFKHh96afQps/riweQ4qWA6q5GRwHrXWav3gCEwFUl9u8fl8+5medoD6vZ5f0IMR7pGgsjS7XD1ts9xe7ar9yJLuKoM2AbgoJM1SLMC0ujnKHNMBep2TGc/PDdBuPA7lwW4J58XW1e4zD2evazHII39QefN51TsMBkV3KVcSYasrgvGU4QwQEQ8mJ8vTmu9nnt27me887uFQkvIgTFwSs/gDz/97nrXcRj6dCQUXWCQ65o0dC/c24OvJDj28zGo47SiAjhW0PHh0BSTsfPNJ3niFDCiPJTh08XoighIGrpDbXdZ0DvqCWphCz7dNu9QZ+c6RV6PfxKQ93zqekN08I7UgbJrxDw+R9LbtLPO1D3LVM7an5Jj31wUYv08G6XIvu3UruX6fJYgw3lumAQ+FwBSKL+1ydl5LXufYJlmZqJLR3utsjnaSbciNgB5ZFb6NvYZ8ALM87QDxf9HaEGHaflWKVW1yiLmQm76CqKGDNwZVxema+4GJuetR6bHHO8TAY7o69qV/0OYKfXxtIccxRAbZVw6Bp9MXzg+MaNoAH8xbw== info@mjbots.com 2 | -------------------------------------------------------------------------------- /utils/start-robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [[ "$1" == "--help" || "$1" == "-h" ]]; then 3 | cat <