Skip to content

Commit 162f252

Browse files
juelgjuelg
committed
ci: dependent workflows, caching and extension linting
1 parent 4e15bc2 commit 162f252

13 files changed

Lines changed: 317 additions & 123 deletions

File tree

‎.github/workflows/cpp.yaml‎

Lines changed: 31 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,9 @@ on:
44
pull_request:
55
paths:
66
- "src/**"
7-
- ".github/workflows/**"
7+
- "include/**"
8+
- "extensions/**"
9+
- ".github/workflows/cpp.yaml"
810
push:
911
branches:
1012
- master
@@ -14,25 +16,42 @@ jobs:
1416
runs-on: ubuntu-latest
1517
steps:
1618
- uses: actions/checkout@v4
19+
- name: Cache apt packages
20+
uses: actions/cache@v4
21+
with:
22+
path: /var/cache/apt/archives
23+
key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
24+
restore-keys: |
25+
${{ runner.os }}-apt-
1726
- name: Install dependencies
1827
run: sudo apt install $(cat debian_deps.txt)
19-
- name: Check clang format
20-
run: make cppcheckformat
2128
- name: Set up Python 3.11
2229
id: setup-python
2330
uses: actions/setup-python@v5
2431
with:
2532
python-version: "3.11"
2633
cache: "pip"
27-
- name: Clang build
28-
run: make clangcompile PYTHON_EXECUTABLE=${{ steps.setup-python.outputs.python-path }}
34+
- name: Install Python dependencies
35+
run: |
36+
pip install -r requirements.txt
37+
- name: Check clang format
38+
run: make cppcheckformat
2939
- name: Clang Tidy
3040
run: make cpplint
41+
- name: Clang build
42+
run: make clangcompile PYTHON_EXECUTABLE=${{ steps.setup-python.outputs.python-path }}
3143

3244
build_gcc:
3345
runs-on: ubuntu-latest
3446
steps:
3547
- uses: actions/checkout@v4
48+
- name: Cache apt packages
49+
uses: actions/cache@v4
50+
with:
51+
path: /var/cache/apt/archives
52+
key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
53+
restore-keys: |
54+
${{ runner.os }}-apt-
3655
- name: Install dependencies
3756
run: sudo apt install $(cat debian_deps.txt)
3857
- name: Set up Python 3.11
@@ -41,5 +60,12 @@ jobs:
4160
with:
4261
python-version: "3.11"
4362
cache: "pip"
63+
- name: Install Python dependencies
64+
run: |
65+
pip install -r requirements.txt
4466
- name: GCC build
4567
run: make gcccompile PYTHON_EXECUTABLE=${{ steps.setup-python.outputs.python-path }}
68+
69+
build_extensions:
70+
needs: [build_clang, build_gcc]
71+
uses: ./.github/workflows/extensions.yaml

‎.github/workflows/extensions.yaml‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
name: Extensions
2+
3+
on:
4+
workflow_call:
5+
6+
jobs:
7+
build_extensions:
8+
strategy:
9+
matrix:
10+
extension:
11+
# Python extensions
12+
- rcs_xarm7
13+
- rcs_realsense
14+
- rcs_robotiq
15+
- rcs_tacto
16+
- rcs_usb_cam
17+
- rcs_zed
18+
runs-on: ubuntu-latest
19+
steps:
20+
- uses: actions/checkout@v4
21+
- name: Cache apt packages
22+
uses: actions/cache@v4
23+
with:
24+
path: /var/cache/apt/archives
25+
key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
26+
restore-keys: |
27+
${{ runner.os }}-apt-
28+
- name: Install dependencies
29+
run: sudo apt install $(cat debian_deps.txt)
30+
- name: Set up Python 3.11
31+
id: setup-python
32+
uses: actions/setup-python@v5
33+
with:
34+
python-version: "3.11"
35+
cache: "pip"
36+
37+
- name: Install build dependencies
38+
run: pip install -r requirements.txt
39+
40+
- name: Install rcs
41+
run: pip install -v --no-build-isolation .
42+
43+
- name: Install extension
44+
run: pip install -v --no-build-isolation extensions/${{ matrix.extension }}
45+
46+
build_cpp_extensions_clang:
47+
strategy:
48+
matrix:
49+
extension:
50+
- rcs_fr3
51+
- rcs_panda
52+
- rcs_robotics_library
53+
- rcs_so101
54+
runs-on: ubuntu-latest
55+
steps:
56+
- uses: actions/checkout@v4
57+
- name: Cache apt packages
58+
uses: actions/cache@v4
59+
with:
60+
path: /var/cache/apt/archives
61+
key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
62+
restore-keys: |
63+
${{ runner.os }}-apt-
64+
- name: Install dependencies
65+
run: sudo apt install $(cat debian_deps.txt)
66+
- name: Set up Python 3.11
67+
id: setup-python
68+
uses: actions/setup-python@v5
69+
with:
70+
python-version: "3.11"
71+
cache: "pip"
72+
- name: Install Python dependencies
73+
run: |
74+
pip install -r requirements.txt
75+
- name: Check clang format
76+
run: make -C extensions/${{ matrix.extension }} cppcheckformat
77+
- name: Clang Tidy
78+
run: make -C extensions/${{ matrix.extension }} cpplint
79+
- name: Clang build
80+
run: make -C extensions/${{ matrix.extension }} clangcompile PYTHON_EXECUTABLE=${{ steps.setup-python.outputs.python-path }}
81+
82+
build_cpp_extensions_gcc:
83+
strategy:
84+
matrix:
85+
extension:
86+
- rcs_fr3
87+
- rcs_panda
88+
- rcs_robotics_library
89+
- rcs_so101
90+
runs-on: ubuntu-latest
91+
steps:
92+
- uses: actions/checkout@v4
93+
- name: Cache apt packages
94+
uses: actions/cache@v4
95+
with:
96+
path: /var/cache/apt/archives
97+
key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
98+
restore-keys: |
99+
${{ runner.os }}-apt-
100+
- name: Install dependencies
101+
run: sudo apt install $(cat debian_deps.txt)
102+
- name: Set up Python 3.11
103+
id: setup-python
104+
uses: actions/setup-python@v5
105+
with:
106+
python-version: "3.11"
107+
cache: "pip"
108+
- name: Install Python dependencies
109+
run: |
110+
pip install -r requirements.txt
111+
pip install -v --no-build-isolation .
112+
pip install -v --no-build-isolation extensions/${{ matrix.extension }}
113+
- name: GCC build
114+
run: make -C extensions/${{ matrix.extension }} gcccompile PYTHON_EXECUTABLE=${{ steps.setup-python.outputs.python-path }}
115+
- name: Check that stub files are up-to-date
116+
run: make -C extensions/${{ matrix.extension }} stubgen && git diff --exit-code
117+

‎.github/workflows/py.yaml‎

Lines changed: 5 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ on:
55
paths:
66
- "python/**"
77
- "src/pybind/**"
8-
- ".github/workflows/**"
8+
- ".github/workflows/py.yaml"
99
push:
1010
branches:
1111
- master
@@ -15,17 +15,18 @@ jobs:
1515
runs-on: ubuntu-latest
1616
steps:
1717
- uses: actions/checkout@v4
18-
- name: Set up Python 3.10
18+
- name: Set up Python 3.11
1919
uses: actions/setup-python@v5
2020
with:
21-
python-version: "3.10"
21+
python-version: "3.11"
2222
cache: "pip"
2323
- name: Install linting and formatting dependencies
2424
run: python -m pip install -r requirements.txt
2525
- name: Check formatting
2626
run: make pycheckformat
2727

2828
pythonpackage:
29+
needs: format
2930
runs-on: ubuntu-latest
3031
env:
3132
CC: clang
@@ -47,11 +48,7 @@ jobs:
4748
- name: Install wheel
4849
run: python -m pip install wheel
4950
- name: Install the package
50-
run: python -m pip install --no-build-isolation .
51-
- name: Install the fr3 extension
52-
run: python -m pip install --no-build-isolation extensions/rcs_fr3
53-
- name: Install the panda extension
54-
run: python -m pip install --no-build-isolation extensions/rcs_panda
51+
run: python -m pip install -v --no-build-isolation .
5552
- name: Code linting
5653
run: make pylint
5754
- name: Check that stub files are up-to-date

‎plan.md‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# Implementation Plan
2+
3+
This plan addresses the reported issues of `NaN`/`inf` commands and velocity violations in the robot control stack. The problems likely stem from bugs in the trajectory interpolation logic and could be exacerbated by the new angled mount of the robot.
4+
5+
The plan is divided into three main phases:
6+
7+
### Phase 1: Fix Trajectory Interpolation Bugs
8+
9+
The most critical issues seem to be in `include/rcs/LinearPoseTrajInterpolator.h`. These bugs are likely the primary cause of the observed instabilities.
10+
11+
1. **Correct Trajectory Start Point Discontinuity**:
12+
* **Problem**: The `reset` methods in both `LinearPoseTrajInterpolator` and `LinearJointPositionTrajInterpolator` incorrectly handle the starting point for a new trajectory segment. They are designed to chain trajectories by starting from the previous goal, but this is unsuitable for teleoperation where new commands should generate a trajectory from the robot's current state. This creates a discontinuity, causing a jump in the command, leading to high velocities and safety faults.
13+
* **Solution**: I will modify the `reset` methods to ensure a smooth start for new trajectory segments. The new trajectory will start from the last interpolated point (`last_p_t_`, `last_q_t_`) instead of the previous goal. This will ensure that there are no jumps when a new command is received. I will remove the `prev_..._goal_` members and the `first_goal_` flag to simplify the logic.
14+
15+
2. **Fix Quaternion "Long Way Around" Interpolation**:
16+
* **Problem**: In `LinearPoseTrajInterpolator::reset`, the logic to ensure the shortest path for quaternion interpolation is faulty. It flips the `q_start_` quaternion instead of the `q_goal_` quaternion. This causes the interpolator to choose the longer path, resulting in large, fast rotations and likely contributing to velocity violation errors.
17+
* **Solution**: I will correct the logic to flip the `q_goal_` quaternion if its dot product with `q_start_` is negative. This is the standard and correct approach for shortest-path `slerp`.
18+
19+
3. **Prevent Division by Zero**:
20+
* **Problem**: The `max_time_` in the interpolators is calculated based on `policy_rate`. If `policy_rate` is zero, this will cause a division-by-zero, resulting in `inf` values.
21+
* **Solution**: I will add a check to ensure `policy_rate` is greater than zero before performing the division.
22+
23+
### Phase 2: Add Per-Joint Limits
24+
25+
The user requested the addition of per-joint limits. I will implement this in `extensions/rcs_fr3/src/hw/Franka.cpp`.
26+
27+
1. **Joint Controller (`joint_controller`)**:
28+
* **Enhancement**: The current implementation has basic joint limit avoidance. I will make this more robust by applying a velocity ramp-down as the joints approach their limits. This provides a smoother response than abruptly zeroing out torque. The torque will be scaled down as a joint gets closer to its limit.
29+
30+
2. **OSC Controller (`osc`)**:
31+
* **Enhancement**: The OSC controller already has a form of joint limit avoidance in the nullspace. I will improve this by making the repulsive force stronger and more progressive as the joint limits are approached. I will also ensure that if a joint is very close to its limit, no torque can be applied that would push it further.
32+
33+
### Phase 3: Mitigate NaN/Infinity Commanding Errors
34+
35+
The `Commanding value is infinite or NaN` error is a critical safety issue.
36+
37+
* **Problem**: This error indicates a numerical issue within the control pipeline. While the interpolation fixes from Phase 1 are likely to resolve the root cause, as a safety measure, it's crucial to prevent `NaN` or `inf` values from ever being sent as torque commands.
38+
* **Solution**: I will add a final safety check in both `osc()` and `joint_controller()` methods in `Franka.cpp`. Before the final torque command is returned, I will check each element of the torque vector for `NaN` or `inf` values. If any are found, I will log an error and command zero torques to safely stop the robot instead of throwing an exception. This will make the system more robust.
39+
40+
By implementing these changes, the robot should become more stable and reliable, especially during teleoperation. The joint limit enhancements will add an extra layer of safety.

0 commit comments

Comments
 (0)