diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml new file mode 100755 index 0000000..e7c7303 --- /dev/null +++ b/.github/workflows/nix.yml @@ -0,0 +1,29 @@ +name: CI - Nix + +on: + push: + branches: + - main + pull_request: + branches: + - main + +jobs: + CI: + strategy: + matrix: + os: [ubuntu-latest, macos-latest] + + runs-on: ${{ matrix.os }} + name: CI (${{ matrix.os }}) + steps: + - uses: actions/checkout@v4 + - uses: cachix/install-nix-action@v30 + - uses: cachix/cachix-action@v15 + with: + name: gepetto + authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' + - run: nix flake check -L + - name: Check if there are changes + if: ${{ matrix.os == 'ubuntu-latest' }} + uses: NathanielHill/fail-if-changes@v1.1.1 diff --git a/.gitignore b/.gitignore index 03a32ce..7d082dd 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ *build* *.user +*.code-workspace +result +.vscode/c_cpp_properties.json diff --git a/CMakeLists.txt b/CMakeLists.txt index fcf8772..66c1e79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,12 +146,15 @@ endif(BUILD_PYTHON_INTERFACE) macro(create_demo source) set(demo_name ${PROJECT_NAME}_${source}) + string(REPLACE "demo_" "" device "${source}") + set(DEMO_ROOT "${CMAKE_INSTALL_PREFIX}/demos") + file(COPY "${PROJECT_SOURCE_DIR}/demos/config_${device}.yaml" DESTINATION "${DEMO_ROOT}") add_executable(${demo_name} demos/${source}.cpp) target_link_libraries(${demo_name} ${PROJECT_NAME}) target_compile_definitions( ${demo_name} - PUBLIC CONFIG_SOLO12_YAML="${PROJECT_SOURCE_DIR}/demos/config_solo12.yaml" - PUBLIC CONFIG_TESTBENCH_YAML="${PROJECT_SOURCE_DIR}/demos/config_testbench.yaml") + PUBLIC CONFIG_SOLO12_YAML="${DEMO_ROOT}/config_solo12.yaml" + PUBLIC CONFIG_TESTBENCH_YAML="${DEMO_ROOT}/config_testbench.yaml") install(TARGETS ${demo_name} DESTINATION bin) endmacro(create_demo source) diff --git a/demos/demo_testbech_joint_calibrator.py b/demos/demo_testbech_joint_calibrator.py index ff19773..0da3738 100644 --- a/demos/demo_testbech_joint_calibrator.py +++ b/demos/demo_testbech_joint_calibrator.py @@ -72,19 +72,6 @@ imu = oci.IMU(robot_if) -robot = oci.Robot(robot_if, joints, imu) -robot.start() -robot.wait_until_ready() - -# As the data is returned by reference, it's enough -# to get hold of the data one. It will update after -# each call to `robot.parse_sensor_data`. -imu_attitude = imu.attitude_euler -positions = joints.positions -velocities = joints.velocities - -des_pos = np.zeros(12) - # Setup the calibration method joint_offsets = np.array( [ @@ -100,15 +87,41 @@ 0.0, 0.0, 0.0, - ] -) + ], + dtype=np.float64, +).reshape(-1, 1) + sdir = oci.CalibrationMethod.positive + +zero_offsets = np.zeros(12, dtype=np.int32).reshape(-1, 1) +some_other_array = np.zeros(12, dtype=np.float64).reshape(-1, 1) + joint_calibrator = oci.JointCalibrator( - joints, 12 * [sdir], joint_offsets, - np.zeros(12, dtype=int), np.zeros(12), - 5.0 / 20.0, 0.05 / 20.0, 2.0, 0.001 + joints, + 12 * [sdir], + joint_offsets, + zero_offsets, + some_other_array, + 5.0 / 20.0, + 0.05 / 20.0, + 2.0, + 0.001, ) +robot = oci.Robot(robot_if, joints, imu, joint_calibrator) +robot.start() +robot.wait_until_ready() + +# As the data is returned by reference, it's enough +# to get hold of the data one. It will update after +# each call to `robot.parse_sensor_data`. +imu_attitude = imu.attitude_euler +positions = joints.positions +velocities = joints.velocities + +des_pos = np.zeros(12) + + c = 0 dt = 0.001 calibration_done = False diff --git a/demos/demo_testbench.py b/demos/demo_testbench.py new file mode 100755 index 0000000..1c1f6f6 --- /dev/null +++ b/demos/demo_testbench.py @@ -0,0 +1,35 @@ +import numpy as np + +np.set_printoptions(suppress=True, precision=2) + +import libodri_control_interface_pywrap as oci + +robot = oci.robot_from_yaml_file("config_testbench.yaml") + +des_pos = np.array([np.pi / 2, -np.pi / 2]) + +robot.initialize(des_pos) + +kp, kd = 0.125, 0.0025 +c = 0 +while not robot.is_timeout: + robot.parse_sensor_data() + + positions = robot.joints.positions + velocities = robot.joints.velocities + + # Compute the PD control on the zero position. + torques = kp * (des_pos - positions) - kd * velocities + + robot.joints.set_torques(torques) + robot.send_command_and_wait_end_of_cycle(0.001) + + c += 1 + + if c % 1000 == 0: + print("joint pos: ", positions) + print("joint vel: ", velocities) + print("torques: ", torques) + robot.robot_interface.PrintStats() + +print("timeout detected") diff --git a/flake.lock b/flake.lock new file mode 100755 index 0000000..9507242 --- /dev/null +++ b/flake.lock @@ -0,0 +1,402 @@ +{ + "nodes": { + "flake-parts": { + "inputs": { + "nixpkgs-lib": "nixpkgs-lib" + }, + "locked": { + "lastModified": 1743550720, + "narHash": "sha256-hIshGgKZCgWh6AYJpJmRgFdR3WUbkY04o82X05xqQiY=", + "owner": "hercules-ci", + "repo": "flake-parts", + "rev": "c621e8422220273271f52058f618c94e405bb0f5", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "flake-parts", + "type": "github" + } + }, + "flake-utils": { + "inputs": { + "systems": "systems" + }, + "locked": { + "lastModified": 1731533236, + "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=", + "owner": "numtide", + "repo": "flake-utils", + "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "flake-utils", + "type": "github" + } + }, + "gepetto": { + "inputs": { + "flake-parts": "flake-parts", + "nix-ros-overlay": "nix-ros-overlay", + "nix-system-graphics": "nix-system-graphics", + "nixpkgs": [ + "gepetto", + "nix-ros-overlay", + "nixpkgs" + ], + "src-agimus-controller": "src-agimus-controller", + "src-agimus-msgs": "src-agimus-msgs", + "src-colmpc": "src-colmpc", + "src-example-parallel-robots": "src-example-parallel-robots", + "src-franka-description": "src-franka-description", + "src-gepetto-viewer": "src-gepetto-viewer", + "src-odri-control-interface": "src-odri-control-interface", + "src-odri-masterboard-sdk": "src-odri-masterboard-sdk", + "src-toolbox-parallel-robots": "src-toolbox-parallel-robots", + "system-manager": "system-manager", + "systems": [ + "gepetto", + "nix-ros-overlay", + "flake-utils", + "systems" + ], + "treefmt-nix": "treefmt-nix" + }, + "locked": { + "lastModified": 1748959317, + "narHash": "sha256-rNdZlsmzyLACqo4UAi3v+64mq8p7XNjUaovaCu6yIsY=", + "owner": "gepetto", + "repo": "nix", + "rev": "90cc507c64489d197d0c7c862617c0766cfd7fb2", + "type": "github" + }, + "original": { + "owner": "gepetto", + "ref": "module", + "repo": "nix", + "type": "github" + } + }, + "nix-ros-overlay": { + "inputs": { + "flake-utils": "flake-utils", + "nixpkgs": "nixpkgs" + }, + "locked": { + "lastModified": 1745796487, + "narHash": "sha256-xvre4xiezhPhU794dpjcHusVLBIfA4jPg9Kq439x4yY=", + "owner": "lopsided98", + "repo": "nix-ros-overlay", + "rev": "4072d6ed51d9053d2cc85c0ec4f69884cc99f392", + "type": "github" + }, + "original": { + "owner": "lopsided98", + "ref": "develop", + "repo": "nix-ros-overlay", + "type": "github" + } + }, + "nix-system-graphics": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1737457219, + "narHash": "sha256-nX9dxoATDCSQgWw/iv6BngXDJEyHVYYEvHEVQ7Ig3fI=", + "owner": "soupglasses", + "repo": "nix-system-graphics", + "rev": "9c875e0c56cf2eb272b9102a4f3e24e4e31629fd", + "type": "github" + }, + "original": { + "owner": "soupglasses", + "repo": "nix-system-graphics", + "type": "github" + } + }, + "nixpkgs": { + "locked": { + "lastModified": 1744849697, + "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=", + "owner": "lopsided98", + "repo": "nixpkgs", + "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4", + "type": "github" + }, + "original": { + "owner": "lopsided98", + "ref": "nix-ros", + "repo": "nixpkgs", + "type": "github" + } + }, + "nixpkgs-lib": { + "locked": { + "lastModified": 1743296961, + "narHash": "sha256-b1EdN3cULCqtorQ4QeWgLMrd5ZGOjLSLemfa00heasc=", + "owner": "nix-community", + "repo": "nixpkgs.lib", + "rev": "e4822aea2a6d1cdd36653c134cacfd64c97ff4fa", + "type": "github" + }, + "original": { + "owner": "nix-community", + "repo": "nixpkgs.lib", + "type": "github" + } + }, + "root": { + "inputs": { + "flake-parts": [ + "gepetto", + "flake-parts" + ], + "gepetto": "gepetto", + "nix-ros-overlay": [ + "gepetto", + "nix-ros-overlay" + ], + "nixpkgs": [ + "gepetto", + "nixpkgs" + ], + "systems": [ + "gepetto", + "systems" + ], + "treefmt-nix": [ + "gepetto", + "treefmt-nix" + ], + "utils": "utils" + } + }, + "src-agimus-controller": { + "flake": false, + "locked": { + "lastModified": 1747904554, + "narHash": "sha256-IHA+1KrtMKIiBw3NaO3VuMk6h/JIPgNpb7vvu2FGrjk=", + "owner": "agimus-project", + "repo": "agimus_controller", + "rev": "92797f93c2a80d4e362ac687f1311a149748bb36", + "type": "github" + }, + "original": { + "owner": "agimus-project", + "repo": "agimus_controller", + "type": "github" + } + }, + "src-agimus-msgs": { + "flake": false, + "locked": { + "lastModified": 1747656482, + "narHash": "sha256-O2DCQeasbrihIveOHmkcK0u4XnVcig33t8n+NFj/PvY=", + "owner": "agimus-project", + "repo": "agimus_msgs", + "rev": "d067e515c59665883b99c5a0028d6f746d0053ab", + "type": "github" + }, + "original": { + "owner": "agimus-project", + "repo": "agimus_msgs", + "type": "github" + } + }, + "src-colmpc": { + "flake": false, + "locked": { + "lastModified": 1747745791, + "narHash": "sha256-git7hITsR58YpgLV1A7TLUvLdhhOxKPHkzY+MBGAIZo=", + "owner": "agimus-project", + "repo": "colmpc", + "rev": "eeb4ae19b487760fe1421a87fe9423202ee78ead", + "type": "github" + }, + "original": { + "owner": "agimus-project", + "repo": "colmpc", + "type": "github" + } + }, + "src-example-parallel-robots": { + "flake": false, + "locked": { + "lastModified": 1747686798, + "narHash": "sha256-UXtJkwoKXYTw90moFr5E1sHHhc1czdHuq2r3wxDIBAE=", + "owner": "gepetto", + "repo": "example-parallel-robots", + "rev": "56fefa6437a4554c81c3b5db7efdc91bea292fdc", + "type": "github" + }, + "original": { + "owner": "gepetto", + "repo": "example-parallel-robots", + "type": "github" + } + }, + "src-franka-description": { + "flake": false, + "locked": { + "lastModified": 1744552174, + "narHash": "sha256-VRPygvJgiJipQ5yB1qmf3UEbwnu2Z916TaGTBDeSGjw=", + "owner": "agimus-project", + "repo": "franka_description", + "rev": "16a77d0c364d6f137cb6d28e7a5f357a5da2fabe", + "type": "github" + }, + "original": { + "owner": "agimus-project", + "repo": "franka_description", + "type": "github" + } + }, + "src-gepetto-viewer": { + "flake": false, + "locked": { + "lastModified": 1745404179, + "narHash": "sha256-mxWRXw0Lh3PpuX8u72ytWVWPPt2Qb8eNJdBf9twweUQ=", + "owner": "Gepetto", + "repo": "gepetto-viewer", + "rev": "c23c86ea979b7591c38100f8667d3c3398626273", + "type": "github" + }, + "original": { + "owner": "Gepetto", + "ref": "devel", + "repo": "gepetto-viewer", + "type": "github" + } + }, + "src-odri-control-interface": { + "flake": false, + "locked": { + "lastModified": 1748957655, + "narHash": "sha256-YhZ8yzgUiN/ZeU/j1cGUEnKycjSD9WOJjMbkqmr+cag=", + "owner": "gwennlbh", + "repo": "odri_control_interface", + "rev": "6cfbb67f0d2764d84bdeb91dcf7698bf67799e03", + "type": "github" + }, + "original": { + "owner": "gwennlbh", + "ref": "nix", + "repo": "odri_control_interface", + "type": "github" + } + }, + "src-odri-masterboard-sdk": { + "flake": false, + "locked": { + "lastModified": 1748957693, + "narHash": "sha256-Z1u12eXOw7mabincVOTmmlVCNPbQubuVMyqQ+INSqww=", + "owner": "gwennlbh", + "repo": "master-board", + "rev": "75eadd8649f0a13248d6e8a0913213ca80c52ea3", + "type": "github" + }, + "original": { + "owner": "gwennlbh", + "ref": "nix", + "repo": "master-board", + "type": "github" + } + }, + "src-toolbox-parallel-robots": { + "flake": false, + "locked": { + "lastModified": 1747686858, + "narHash": "sha256-uX8U/plHRs2eQNRDlm6KU5EgHrIj33o7dO7cA6EM/Ek=", + "owner": "gepetto", + "repo": "toolbox-parallel-robots", + "rev": "f123c491d15cadad6133af353c50560fce2e678b", + "type": "github" + }, + "original": { + "owner": "gepetto", + "repo": "toolbox-parallel-robots", + "type": "github" + } + }, + "system-manager": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1747903970, + "narHash": "sha256-PFpAdjeBLpoY7v1v0GUeFuBWhEOiA187l0b2lmg117A=", + "owner": "numtide", + "repo": "system-manager", + "rev": "94f1c8d9c562e96358dddddb8e276ba8ac16c9ae", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "system-manager", + "type": "github" + } + }, + "systems": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, + "treefmt-nix": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1747912973, + "narHash": "sha256-XgxghfND8TDypxsMTPU2GQdtBEsHTEc3qWE6RVEk8O0=", + "owner": "numtide", + "repo": "treefmt-nix", + "rev": "020cb423808365fa3f10ff4cb8c0a25df35065a3", + "type": "github" + }, + "original": { + "owner": "numtide", + "repo": "treefmt-nix", + "type": "github" + } + }, + "utils": { + "locked": { + "lastModified": 1748363541, + "narHash": "sha256-bDV63Oo+CQqB9joTf+G8hbxPgLjHBzCcmkiOT360/XQ=", + "owner": "Gepetto", + "repo": "nix-lib", + "rev": "eacf43e7f58944507d1be011c25af268aa9048e9", + "type": "github" + }, + "original": { + "owner": "Gepetto", + "repo": "nix-lib", + "type": "github" + } + } + }, + "root": "root", + "version": 7 +} diff --git a/flake.nix b/flake.nix new file mode 100755 index 0000000..a9028e7 --- /dev/null +++ b/flake.nix @@ -0,0 +1,70 @@ +{ + description = "odri-control-interface"; + + inputs = { + # TODO: drop `/module` after https://github.com/Gepetto/nix/pull/54 + gepetto.url = "github:gepetto/nix/module"; + flake-parts.follows = "gepetto/flake-parts"; + nixpkgs.follows = "gepetto/nixpkgs"; + nix-ros-overlay.follows = "gepetto/nix-ros-overlay"; + systems.follows = "gepetto/systems"; + treefmt-nix.follows = "gepetto/treefmt-nix"; + utils.url = "github:Gepetto/nix-lib"; + }; + + outputs = + inputs: + inputs.flake-parts.lib.mkFlake { inherit inputs; } { + systems = import inputs.systems; + imports = [ inputs.gepetto.flakeModule ]; + perSystem = + { + lib, + pkgs, + self', + ... + }: + { + packages = { + default = self'.packages.odri-control-interface; + odri-control-interface = pkgs.odri-control-interface.overrideAttrs { + version = inputs.utils.lib.rosVersion pkgs ./package.xml; + src = lib.fileset.toSource { + root = ./.; + fileset = lib.fileset.unions [ + ./demos + ./include + ./src + ./srcpy + ./CMakeLists.txt + ./package.xml + ]; + }; + }; + }; + + apps = { + testbench = { + type = "app"; + program = lib.getExe' self'.packages.odri-control-interface "odri_control_interface_demo_testbench"; + }; + solo12 = { + type = "app"; + program = lib.getExe' self'.packages.odri-control-interface "odri_control_interface_demo_solo12"; + }; + }; + devShells.python = pkgs.mkShell { + buildInputs = [ + self'.packages.odri-control-interface + pkgs.odri-masterboard-sdk + pkgs.python3Packages.python + pkgs.python3Packages.numpy + ]; + PYTHONPATH = lib.concatMapStringsSep ":" (p: "${p}/${pkgs.python3.sitePackages}") [ + self'.packages.odri-control-interface + pkgs.odri-masterboard-sdk + ]; + }; + }; + }; +} diff --git a/readme.md b/readme.md index 6435129..713c0c1 100644 --- a/readme.md +++ b/readme.md @@ -7,7 +7,9 @@ Common interface for controlling robots build with the odri master board. ### Installation -#### Download the package: +#### Using treep & colcon + +##### Download the package: We use `treep` to download the required packages. Make sure your ssh key is unlocked. Then @@ -20,7 +22,7 @@ treep --clone master-board treep --clone odri_control_interface ``` -#### Build the package +##### Build the package We use [colcon](https://github.com/machines-in-motion/machines-in-motion.github.io/wiki/use_colcon) to build this package: @@ -28,6 +30,13 @@ to build this package: cd mkdir -p ~/devel/workspace colcon build ``` + +#### Using Nix + +``` +nix build +``` + ### Usage: #### Demos/Examples