nix: add flake

.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/[email protected]

.gitignore

@@ -3,3 +3,6 @@
 *build*
 
 *.user
+*.code-workspace
+result
+.vscode/c_cpp_properties.json

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)

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

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")