diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..4075b4b --- /dev/null +++ b/.dockerignore @@ -0,0 +1,53 @@ +# Docker ignore file for Franka Toolbox build + +# Git +.git +.gitignore + +# Build artifacts (will be recreated) +**/build/ +**/bin/ +**/bin_arm/ + +# Existing archives (optional - comment out to use cached builds) +# common/bin.zip +# common/bin_arm.zip +# franka_robot_server/bin.tar.gz +# franka_robot_server/bin_arm.tar.gz +# dependencies/libfranka.zip +# dependencies/libfranka_arm.zip + +# Temporary files +**/tmp/ +*.tmp +*.log + +# MATLAB generated files +*.mexa64 +*.mexw64 +*.mex +*.asv +*.slxc +slprj/ + +# Distribution +dist/ + +# Documentation build artifacts +docs/_build/ + +# IDE files +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# OS files +.DS_Store +Thumbs.db + +# libfranka source (will be cloned fresh in container) +libfranka/ +libfranka_arm/ + diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..2c52867 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,35 @@ +* text=auto + +*.fig binary +*.mat binary +*.mdl binary diff merge=mlAutoMerge +*.mdlp binary +*.mex* binary +*.mlapp binary +*.mldatx binary merge=mlAutoMerge +*.mlproj binary +*.mlx binary +*.p binary +*.plprj binary +*.sbproj binary +*.sfx binary +*.sldd binary +*.slreqx binary merge=mlAutoMerge +*.slmx binary merge=mlAutoMerge +*.sltx binary +*.slxc binary +*.slx binary merge=mlAutoMerge +*.slxp binary + +## MATLAB Project metadata files use LF line endings +/resources/project/**/*.xml text eol=lf + +## Other common binary file types +*.docx binary +*.exe binary +*.jpg binary +*.pdf binary +*.png binary +*.xlsx binary +*.zip binary +*.tar.gz binary \ No newline at end of file diff --git a/.github/workflows/build-and-release.yml b/.github/workflows/build-and-release.yml new file mode 100644 index 0000000..7cacffb --- /dev/null +++ b/.github/workflows/build-and-release.yml @@ -0,0 +1,666 @@ +name: Build and Release + +on: + push: + tags: + - '[0-9]+.[0-9]+.[0-9]*' # Triggers on version tags like 1.0.0, 4.0.0, etc. + workflow_dispatch: + inputs: + version: + description: 'Version number (e.g., 4.0.0)' + required: true + type: string + prerelease: + description: 'Mark as pre-release' + required: false + default: false + type: boolean + +# Note: MATLAB batch licensing token (MLM_LICENSE_TOKEN) is only needed if: +# - The repository is private, OR +# - The workflow uses transformation products (MATLAB Coder, MATLAB Compiler) +# For public repos without these products, no token is required. + +permissions: + contents: write # Required for creating releases and uploading assets + +jobs: + # ============================================================================= + # Job 1: Build server binaries using Docker (amd64 + arm64) + # ============================================================================= + build-server: + name: Build Server Binaries (Docker) + runs-on: ubuntu-22.04 + + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Set up QEMU for ARM64 cross-compilation + uses: docker/setup-qemu-action@v3 + with: + platforms: arm64 + + - name: Make build scripts executable + run: | + chmod +x docker/build.sh + chmod +x docker/scripts/*.sh + + - name: Build server binaries (amd64 + arm64) + run: | + cd docker + ./build.sh all + + - name: Verify build outputs + run: | + echo "=== Build Outputs ===" + ls -la common/*.zip || echo "No common/*.zip files" + ls -la franka_robot_server/*.tar.gz || echo "No server tar.gz files" + ls -la dependencies/*.zip || echo "No dependencies/*.zip files" + + - name: Upload server artifacts + uses: actions/upload-artifact@v4 + with: + name: server-binaries + path: | + common/bin.zip + common/bin_arm.zip + franka_robot_server/bin.tar.gz + franka_robot_server/bin_arm.tar.gz + dependencies/libfranka.zip + dependencies/libfranka_arm.zip + retention-days: 7 + + # ============================================================================= + # Job 1b: Build server binaries for Gen1 robots (libfranka 0.9.2) + # ============================================================================= + build-server-gen1: + name: Build Server Binaries Gen1 (Docker) + runs-on: ubuntu-22.04 + + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Set up QEMU for ARM64 cross-compilation + uses: docker/setup-qemu-action@v3 + with: + platforms: arm64 + + - name: Make build scripts executable + run: | + chmod +x docker/build.sh + chmod +x docker/scripts/*.sh + + - name: Build server binaries with libfranka 0.9.2 (amd64 + arm64) + run: | + cd docker + ./build.sh --libfranka 0.9.2 + + - name: Rename outputs for Gen1 + run: | + # Rename binaries to distinguish Gen1 from Gen3 + mv common/bin.zip common/bin_gen1.zip || true + mv common/bin_arm.zip common/bin_arm_gen1.zip || true + mv franka_robot_server/bin.tar.gz franka_robot_server/bin_gen1.tar.gz || true + mv franka_robot_server/bin_arm.tar.gz franka_robot_server/bin_arm_gen1.tar.gz || true + mv dependencies/libfranka.zip dependencies/franka_gen1.zip || true + mv dependencies/libfranka_arm.zip dependencies/libfranka_arm_gen1.zip || true + + - name: Verify build outputs + run: | + echo "=== Gen1 Build Outputs ===" + ls -la common/*_gen1.zip || echo "No common/*_gen1.zip files" + ls -la franka_robot_server/*_gen1.tar.gz || echo "No server *_gen1.tar.gz files" + ls -la dependencies/*_gen1.zip || echo "No dependencies/*_gen1.zip files" + + - name: Upload Gen1 server artifacts + uses: actions/upload-artifact@v4 + with: + name: server-binaries-gen1 + path: | + common/bin_gen1.zip + common/bin_arm_gen1.zip + franka_robot_server/bin_gen1.tar.gz + franka_robot_server/bin_arm_gen1.tar.gz + dependencies/franka_gen1.zip + dependencies/libfranka_arm_gen1.zip + retention-days: 7 + + # ============================================================================= + # Job 2a: Build MEX files on Ubuntu 22.04 with MATLAB R2023a + # ============================================================================= + build-mex-linux: + name: Build MEX (Linux R2023a) + runs-on: ubuntu-22.04 + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Install build dependencies + run: | + sudo apt-get update + sudo apt-get install -y cmake build-essential libeigen3-dev + + - name: Install Cap'n Proto from source (static libraries) + run: | + # Build Cap'n Proto as STATIC libraries for static linking into MEX files + # This ensures users don't need Cap'n Proto installed + # PIC flag required for linking into shared objects (MEX files) + CAPNP_VERSION="1.0.2" + + cd /tmp + curl -O https://capnproto.org/capnproto-c++-${CAPNP_VERSION}.tar.gz + tar zxf capnproto-c++-${CAPNP_VERSION}.tar.gz + cd capnproto-c++-${CAPNP_VERSION} + + mkdir build && cd build + cmake \ + -DBUILD_TESTING=OFF \ + -DBUILD_SHARED_LIBS=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + .. + make -j$(nproc) + sudo make install + sudo ldconfig + + # Verify static libraries + capnp --version + echo "=== Static libraries (for linking into MEX) ===" + ls -la /usr/local/lib/libcapnp*.a /usr/local/lib/libkj*.a 2>/dev/null || echo "Warning: .a files not found" + echo "=== Headers ===" + ls /usr/local/include/capnp/ | head -5 + + - name: Set up MATLAB R2023a + uses: matlab-actions/setup-matlab@v2 + with: + release: R2023a + products: | + MATLAB + Simulink + MATLAB_Coder + Simulink_Coder + + - name: Build MEX files + uses: matlab-actions/run-command@v2 + with: + command: | + addpath(genpath(pwd)); + fprintf('Building Simulink library MEX files...\n'); + franka_simulink_library_mex(); + fprintf('Building FrankaRobot MEX files...\n'); + franka_robot_mex(); + fprintf('MEX build complete!\n'); + + - name: Verify MEX outputs + run: | + echo "=== MEX Build Outputs ===" + ls -la franka_robot/*.zip || echo "No franka_robot/*.zip" + ls -la franka_simulink_library/*.zip || echo "No simulink library *.zip" + # Show contents of zip files + unzip -l franka_robot/bin.zip || true + unzip -l franka_simulink_library/bin.zip || true + + - name: Upload Linux MEX artifacts + uses: actions/upload-artifact@v4 + with: + name: mex-linux + path: | + franka_robot/bin.zip + franka_simulink_library/bin.zip + retention-days: 7 + + # ============================================================================= + # Job 2b: Build MEX files on Windows with MATLAB R2023a + # Note: Using windows-2022 because franka_simulink_library_mex.m + # hardcodes "Visual Studio 17 2022" generator + # ============================================================================= + build-mex-windows: + name: Build MEX (Windows R2023a) + runs-on: windows-2022 + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Build Cap'n Proto from source (static libraries) + run: | + # Build Cap'n Proto as STATIC libraries for static linking into MEX files + # This ensures users don't need to install Cap'n Proto separately + # Output compatible with Windows 10 and Windows 11 + # + # Expected directory structure for franka_robot_mex.m: + # C:\Program Files (x86)\capnproto-c++-win32-1.0.2\capnproto-c++-1.0.2\src\capnp\Release + # C:\Program Files (x86)\capnproto-c++-win32-1.0.2\capnproto-c++-1.0.2\src\kj\Release + + $capnpVersion = "1.0.2" + $capnpUrl = "https://capnproto.org/capnproto-c++-$capnpVersion.tar.gz" + $downloadPath = "$env:TEMP\capnproto.tar.gz" + $extractPath = "$env:TEMP\capnproto-extract" + $targetDir = "C:\Program Files (x86)\capnproto-c++-win32-$capnpVersion\capnproto-c++-$capnpVersion" + + Write-Host "Downloading Cap'n Proto $capnpVersion source..." + Invoke-WebRequest -Uri $capnpUrl -OutFile $downloadPath + + Write-Host "Extracting source..." + New-Item -ItemType Directory -Path $extractPath -Force | Out-Null + tar -xzf $downloadPath -C $extractPath + + $sourceDir = "$extractPath\capnproto-c++-$capnpVersion" + + Write-Host "Building Cap'n Proto as STATIC libraries..." + $buildDir = "$sourceDir\build" + New-Item -ItemType Directory -Path $buildDir -Force | Out-Null + + cd $buildDir + # BUILD_SHARED_LIBS=OFF ensures static libraries for static linking + cmake -G "Visual Studio 17 2022" -A x64 ` + -DBUILD_TESTING=OFF ` + -DBUILD_SHARED_LIBS=OFF ` + .. + cmake --build . --config Release --parallel + + Write-Host "Setting up expected directory structure..." + New-Item -ItemType Directory -Path "$targetDir\src\capnp\Release" -Force | Out-Null + New-Item -ItemType Directory -Path "$targetDir\src\kj\Release" -Force | Out-Null + + # Copy headers + Copy-Item -Path "$sourceDir\src\capnp" -Destination "$targetDir\src\" -Recurse -Force + Copy-Item -Path "$sourceDir\src\kj" -Destination "$targetDir\src\" -Recurse -Force + + # Copy static libraries (.lib) + Copy-Item -Path "$buildDir\src\capnp\Release\*.lib" -Destination "$targetDir\src\capnp\Release\" -Force + Copy-Item -Path "$buildDir\src\kj\Release\*.lib" -Destination "$targetDir\src\kj\Release\" -Force + + # Copy tools to bin and add to PATH + $binDir = "$targetDir\bin" + New-Item -ItemType Directory -Path $binDir -Force | Out-Null + Copy-Item -Path "$buildDir\src\capnp\Release\capnp.exe" -Destination $binDir -Force + Copy-Item -Path "$buildDir\src\capnp\Release\capnpc-c++.exe" -Destination $binDir -Force + echo $binDir | Out-File -FilePath $env:GITHUB_PATH -Encoding utf8 -Append + + Write-Host "Verifying static libraries..." + & "$binDir\capnp.exe" --version + Get-ChildItem -Path "$targetDir\src\capnp\Release\*.lib" | ForEach-Object { Write-Host "Static lib: $($_.Name) ($($_.Length) bytes)" } + Get-ChildItem -Path "$targetDir\src\kj\Release\*.lib" | ForEach-Object { Write-Host "Static lib: $($_.Name) ($($_.Length) bytes)" } + shell: pwsh + + - name: Set up MATLAB R2023a + uses: matlab-actions/setup-matlab@v2 + with: + release: R2023a + products: | + MATLAB + Simulink + MATLAB_Coder + Simulink_Coder + + - name: Build MEX files + uses: matlab-actions/run-command@v2 + with: + command: | + addpath(genpath(pwd)); + fprintf('Building Simulink library MEX files...\n'); + franka_simulink_library_mex(); + fprintf('Building FrankaRobot MEX files...\n'); + franka_robot_mex(); + fprintf('MEX build complete!\n'); + + - name: Verify MEX outputs + run: | + Write-Host "=== MEX Build Outputs ===" + Get-ChildItem -Path "franka_robot" -Filter "*.zip" -ErrorAction SilentlyContinue + Get-ChildItem -Path "franka_simulink_library" -Filter "*.zip" -ErrorAction SilentlyContinue + shell: pwsh + + - name: Upload Windows MEX artifacts + uses: actions/upload-artifact@v4 + with: + name: mex-windows + path: | + franka_robot/bin.zip + franka_simulink_library/bin.zip + retention-days: 7 + + # ============================================================================= + # Job 3: Merge artifacts and create distribution packages + # ============================================================================= + package: + name: Package Distribution + runs-on: ubuntu-22.04 + needs: [build-server, build-server-gen1, build-mex-linux, build-mex-windows] + + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Download server artifacts (Gen3 - libfranka from config) + uses: actions/download-artifact@v4 + with: + name: server-binaries + path: . + + - name: Download Linux MEX artifacts + uses: actions/download-artifact@v4 + with: + name: mex-linux + path: mex-linux + + - name: Download Windows MEX artifacts + uses: actions/download-artifact@v4 + with: + name: mex-windows + path: mex-windows + + - name: Merge MEX artifacts + run: | + # Create temp directories for merging + mkdir -p merge_franka_robot + mkdir -p merge_simulink_lib + + # Extract Linux MEX files + if [ -f "mex-linux/franka_robot/bin.zip" ]; then + unzip -o "mex-linux/franka_robot/bin.zip" -d merge_franka_robot/ + fi + if [ -f "mex-linux/franka_simulink_library/bin.zip" ]; then + unzip -o "mex-linux/franka_simulink_library/bin.zip" -d merge_simulink_lib/ + fi + + # Extract Windows MEX files (will overwrite same-named files, but extensions differ) + if [ -f "mex-windows/franka_robot/bin.zip" ]; then + unzip -o "mex-windows/franka_robot/bin.zip" -d merge_franka_robot/ + fi + if [ -f "mex-windows/franka_simulink_library/bin.zip" ]; then + unzip -o "mex-windows/franka_simulink_library/bin.zip" -d merge_simulink_lib/ + fi + + # Create merged zip files + cd merge_franka_robot && zip -r ../franka_robot/bin.zip . && cd .. + cd merge_simulink_lib && zip -r ../franka_simulink_library/bin.zip . && cd .. + + # Verify merged contents + echo "=== Merged franka_robot/bin.zip ===" + unzip -l franka_robot/bin.zip + echo "=== Merged franka_simulink_library/bin.zip ===" + unzip -l franka_simulink_library/bin.zip + + # Cleanup + rm -rf merge_franka_robot merge_simulink_lib mex-linux mex-windows + + - name: Set up MATLAB R2024a + uses: matlab-actions/setup-matlab@v2 + with: + release: R2024a + products: | + MATLAB + Simulink + MATLAB_Coder + Simulink_Coder + + - name: Read Gen3 libfranka version from config + id: libfranka-version + run: | + GEN3_LIBFRANKA_VERSION=$(sed -n '1p' config/libfranka_ver.csv | tr -d '\r') + if [ -z "$GEN3_LIBFRANKA_VERSION" ]; then + echo "config/libfranka_ver.csv is empty" + exit 1 + fi + echo "GEN3_LIBFRANKA_VERSION=$GEN3_LIBFRANKA_VERSION" >> $GITHUB_ENV + echo "gen3_version=$GEN3_LIBFRANKA_VERSION" >> $GITHUB_OUTPUT + echo "Using Gen3 libfranka version: $GEN3_LIBFRANKA_VERSION" + + - name: Create Gen3 distribution package (franka-fr3.mltbx) + uses: matlab-actions/run-command@v2 + with: + command: | + addpath(genpath(pwd)); + gen3LibfrankaVersion = getenv('GEN3_LIBFRANKA_VERSION'); + fprintf('Creating Gen3 distribution package (libfranka %s)...\n', gen3LibfrankaVersion); + franka_toolbox_dist_make('Mode', 'ci', 'OutputName', 'franka-fr3'); + fprintf('Gen3 distribution package created!\n'); + + - name: Download server artifacts (Gen1 - libfranka 0.9.2) + uses: actions/download-artifact@v4 + with: + name: server-binaries-gen1 + path: gen1-binaries + + - name: Swap in Gen1 binaries for Gen1 package + run: | + echo "=== Swapping in Gen1 binaries (libfranka 0.9.2) ===" + + # Replace with Gen1 binaries (franka_toolbox_dist_make preserves existing .mltbx in CI mode) + cp gen1-binaries/common/bin_gen1.zip common/bin.zip + cp gen1-binaries/common/bin_arm_gen1.zip common/bin_arm.zip + cp gen1-binaries/franka_robot_server/bin_gen1.tar.gz franka_robot_server/bin.tar.gz + cp gen1-binaries/franka_robot_server/bin_arm_gen1.tar.gz franka_robot_server/bin_arm.tar.gz + cp gen1-binaries/dependencies/franka_gen1.zip dependencies/libfranka.zip + cp gen1-binaries/dependencies/libfranka_arm_gen1.zip dependencies/libfranka_arm.zip + + echo "Gen1 binaries swapped in successfully" + + # Cleanup: remove temporary Gen1 artifacts folder + rm -rf gen1-binaries + echo "Cleaned up gen1-binaries folder" + + - name: Create Gen1 distribution package (franka-fer.mltbx) + uses: matlab-actions/run-command@v2 + with: + command: | + addpath(genpath(pwd)); + fprintf('Creating Gen1 distribution package (libfranka 0.9.2)...\n'); + franka_toolbox_dist_make('Mode', 'ci', 'OutputName', 'franka-fer'); + fprintf('Gen1 distribution package created!\n'); + + - name: Verify distribution outputs + run: | + echo "=== Distribution Outputs ===" + ls -la dist/ + + if [ -f "dist/franka-fr3.mltbx" ]; then + echo "✓ franka-fr3.mltbx (Gen3) created successfully" + du -h dist/franka-fr3.mltbx + else + echo "✗ franka-fr3.mltbx not found!" + exit 1 + fi + + if [ -f "dist/franka-fer.mltbx" ]; then + echo "✓ franka-fer.mltbx (Gen1) created successfully" + du -h dist/franka-fer.mltbx + else + echo "✗ franka-fer.mltbx not found!" + exit 1 + fi + + - name: Upload distribution artifacts + uses: actions/upload-artifact@v4 + with: + name: distribution + path: | + dist/franka-fr3.mltbx + dist/franka-fer.mltbx + retention-days: 30 + + # ============================================================================= + # Job 4: Create GitHub Release + # ============================================================================= + release: + name: Create Release + runs-on: ubuntu-latest + needs: package + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set version + id: version + run: | + if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then + echo "version=${{ github.event.inputs.version }}" >> $GITHUB_OUTPUT + else + VERSION=${GITHUB_REF#refs/tags/} + echo "version=$VERSION" >> $GITHUB_OUTPUT + fi + + - name: Read Gen3 libfranka version from config + id: libfranka-version + run: | + GEN3_LIBFRANKA_VERSION=$(sed -n '1p' config/libfranka_ver.csv | tr -d '\r') + if [ -z "$GEN3_LIBFRANKA_VERSION" ]; then + echo "config/libfranka_ver.csv is empty" + exit 1 + fi + echo "gen3_version=$GEN3_LIBFRANKA_VERSION" >> $GITHUB_OUTPUT + echo "Using Gen3 libfranka version: $GEN3_LIBFRANKA_VERSION" + + - name: Download distribution artifact + uses: actions/download-artifact@v4 + with: + name: distribution + path: dist + + - name: Verify distribution files + id: check-dist + run: | + echo "=== Checking distribution files ===" + ls -la dist/ + + if [ -f "dist/franka-fr3.mltbx" ]; then + echo "gen3_exists=true" >> $GITHUB_OUTPUT + echo "gen3_size=$(du -h dist/franka-fr3.mltbx | cut -f1)" >> $GITHUB_OUTPUT + echo "✓ franka-fr3.mltbx found" + else + echo "gen3_exists=false" >> $GITHUB_OUTPUT + echo "✗ franka-fr3.mltbx not found!" + exit 1 + fi + + if [ -f "dist/franka-fer.mltbx" ]; then + echo "gen1_exists=true" >> $GITHUB_OUTPUT + echo "gen1_size=$(du -h dist/franka-fer.mltbx | cut -f1)" >> $GITHUB_OUTPUT + echo "✓ franka-fer.mltbx found" + else + echo "gen1_exists=false" >> $GITHUB_OUTPUT + echo "✗ franka-fer.mltbx not found!" + exit 1 + fi + + - name: Extract changelog entry + id: changelog + run: | + VERSION="${{ steps.version.outputs.version }}" + + if [ ! -f "CHANGELOG.md" ]; then + echo "CHANGELOG.md not found, creating basic release notes" + cat > VERSION_CHANGELOG.md << EOF + ## What's New + + Release $VERSION of Franka Toolbox for MATLAB. + EOF + else + echo "Extracting latest changelog entry from CHANGELOG.md" + + # Extract the first (latest) changelog entry: from first "## " to the next "## " or EOF + # This is more robust than matching exact version numbers + awk '/^## [0-9]/{if(found) exit; found=1} found{print}' CHANGELOG.md > VERSION_CHANGELOG.md + + if [ -s VERSION_CHANGELOG.md ] && [ $(wc -l < VERSION_CHANGELOG.md) -gt 1 ]; then + echo "Successfully extracted latest changelog entry:" + head -3 VERSION_CHANGELOG.md + else + echo "Could not extract changelog, using fallback" + cat > VERSION_CHANGELOG.md << EOF + ## What's New + + Release $VERSION of Franka Toolbox for MATLAB. + EOF + fi + fi + + # Build the full release notes + cat > RELEASE_NOTES.md << EOF + $(cat VERSION_CHANGELOG.md) + + ## Installation + + Choose the appropriate package for your robot: + + | Package | Robot | libfranka Version | + |---------|-------|-------------------| + | \`franka-fr3.mltbx\` | FR3 | ${{ steps.libfranka-version.outputs.gen3_version }} | + | \`franka-fer.mltbx\` | FER | 0.9.2 | + + To install, either: + 1. Double-click the \`.mltbx\` file, or + 2. In MATLAB, go to **Home** > **Add-Ons** > **Install Add-On** and select the file + + ## Documentation + + For detailed documentation, examples, and API reference, visit: [Franka Toolbox for MATLAB Documentation](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/index.html) + + ## Build Information + + | Package | Size | libfranka | + |---------|------|-----------| + | franka-fr3.mltbx | ${{ steps.check-dist.outputs.gen3_size }} | ${{ steps.libfranka-version.outputs.gen3_version }} | + | franka-fer.mltbx | ${{ steps.check-dist.outputs.gen1_size }} | 0.9.2 | + + - **Version**: $VERSION + - **MEX Platforms**: Linux (R2023a), Windows (R2023a) + - **Target Architectures**: x86_64, ARM64 + EOF + + echo "changelog<> $GITHUB_OUTPUT + cat RELEASE_NOTES.md >> $GITHUB_OUTPUT + echo "EOF" >> $GITHUB_OUTPUT + + - name: Create Release + uses: softprops/action-gh-release@v1 + with: + tag_name: ${{ steps.version.outputs.version }} + name: Franka Toolbox for MATLAB ${{ steps.version.outputs.version }} + body: ${{ steps.changelog.outputs.changelog }} + draft: false + prerelease: ${{ github.event.inputs.prerelease || false }} + files: | + dist/franka-fr3.mltbx + dist/franka-fer.mltbx + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + + - name: Summary + run: | + echo "## Release Summary" >> $GITHUB_STEP_SUMMARY + echo "- **Version**: ${{ steps.version.outputs.version }}" >> $GITHUB_STEP_SUMMARY + echo "- **Release URL**: https://github.com/${{ github.repository }}/releases/tag/${{ steps.version.outputs.version }}" >> $GITHUB_STEP_SUMMARY + echo "- **Pre-release**: ${{ github.event.inputs.prerelease || false }}" >> $GITHUB_STEP_SUMMARY + echo "" >> $GITHUB_STEP_SUMMARY + echo "### Distribution Packages" >> $GITHUB_STEP_SUMMARY + echo "| Package | Size | Robot |" >> $GITHUB_STEP_SUMMARY + echo "|---------|------|-------|" >> $GITHUB_STEP_SUMMARY + echo "| franka-fr3.mltbx | ${{ steps.check-dist.outputs.gen3_size }} | FR3 |" >> $GITHUB_STEP_SUMMARY + echo "| franka-fer.mltbx | ${{ steps.check-dist.outputs.gen1_size }} | FER |" >> $GITHUB_STEP_SUMMARY + echo "" >> $GITHUB_STEP_SUMMARY + echo "### Build Jobs" >> $GITHUB_STEP_SUMMARY + echo "- ✅ Server Binaries Gen3 (Docker - amd64 + arm64)" >> $GITHUB_STEP_SUMMARY + echo "- ✅ Server Binaries Gen1 (Docker - amd64 + arm64)" >> $GITHUB_STEP_SUMMARY + echo "- ✅ MEX Linux (Ubuntu 22.04, R2023a)" >> $GITHUB_STEP_SUMMARY + echo "- ✅ MEX Windows (R2023a)" >> $GITHUB_STEP_SUMMARY + echo "- ✅ Package & Release" >> $GITHUB_STEP_SUMMARY + diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml deleted file mode 100644 index c794b17..0000000 --- a/.github/workflows/release.yml +++ /dev/null @@ -1,165 +0,0 @@ -name: Create Release - -on: - push: - tags: - - '[0-9]+.[0-9]+.[0-9]*' # Triggers on version tags like 1.0.0, 2.1.3, etc. - workflow_dispatch: # Allows manual triggering from GitHub UI - inputs: - version: - description: 'Version number (e.g., 1.0.0)' - required: true - type: string - prerelease: - description: 'Mark as pre-release' - required: false - default: false - type: boolean - -permissions: - contents: write # Required for creating releases and uploading assets - -jobs: - create-release: - runs-on: ubuntu-latest - - steps: - - name: Checkout code - uses: actions/checkout@v4 - with: - fetch-depth: 0 # Fetch all history for proper tagging - - - name: Set version - id: version - run: | - if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then - echo "version=${{ github.event.inputs.version }}" >> $GITHUB_OUTPUT - else - # Extract version from tag (e.g., 1.0.0) - VERSION=${GITHUB_REF#refs/tags/} - echo "version=$VERSION" >> $GITHUB_OUTPUT - fi - - - name: Check if distribution file exists - id: check-dist - run: | - if [ -f "dist/franka.mltbx" ]; then - echo "exists=true" >> $GITHUB_OUTPUT - echo "size=$(du -h dist/franka.mltbx | cut -f1)" >> $GITHUB_OUTPUT - else - echo "exists=false" >> $GITHUB_OUTPUT - echo "Distribution file not found! Please ensure franka.mltbx is built and committed." - exit 1 - fi - - - name: Extract changelog entry - id: changelog - run: | - # Extract the changelog entry for this version from CHANGELOG.md - VERSION="${{ steps.version.outputs.version }}" - - # Check if CHANGELOG.md exists - if [ ! -f "CHANGELOG.md" ]; then - echo "CHANGELOG.md not found, creating basic release notes" - cat > RELEASE_NOTES.md << EOF - ## Installation - - Download the \`franka.mltbx\` file from the assets below and install it in MATLAB: - - 1. Double-click the \`franka.mltbx\` file, or - 2. In MATLAB, go to **Home** > **Add-Ons** > **Install Add-On** and select the file - - ## Documentation - - For detailed documentation, examples, and API reference, visit: [Franka Toolbox for MATLAB Documentation](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/index.html#) - - ## File Information - - - **File**: franka.mltbx - - **Size**: ${{ steps.check-dist.outputs.size }} - - **Version**: $VERSION - EOF - else - # Extract the changelog entry for this version - echo "Looking for version $VERSION in CHANGELOG.md" - - # Use sed to extract the section - more reliable than awk - # Find the line starting with "## $VERSION" and extract until next "## " line - sed -n "/^## $VERSION/,/^## [0-9]/p" CHANGELOG.md | sed '$d' > VERSION_CHANGELOG.md - - # Check if we got content - if [ -s VERSION_CHANGELOG.md ] && [ $(wc -l < VERSION_CHANGELOG.md) -gt 1 ]; then - echo "Successfully extracted changelog for version $VERSION" - echo "Extracted content:" - cat VERSION_CHANGELOG.md - else - echo "Version $VERSION not found in CHANGELOG.md, creating basic release notes" - cat > VERSION_CHANGELOG.md << EOF - ## Installation - - Download the \`franka.mltbx\` file from the assets below and install it in MATLAB: - - 1. Double-click the \`franka.mltbx\` file, or - 2. In MATLAB, go to **Home** > **Add-Ons** > **Install Add-On** and select the file - - ## Documentation - - For detailed documentation, examples, and API reference, visit: [Franka Toolbox for MATLAB Documentation](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/index.html#) - - ## File Information - - - **File**: franka.mltbx - - **Size**: ${{ steps.check-dist.outputs.size }} - - **Version**: $VERSION - EOF - fi - - # Create final release notes - cat > RELEASE_NOTES.md << EOF - $(cat VERSION_CHANGELOG.md) - - ## Installation - - Download the \`franka.mltbx\` file from the assets below and install it in MATLAB: - - 1. Double-click the \`franka.mltbx\` file, or - 2. In MATLAB, go to **Home** > **Add-Ons** > **Install Add-On** and select the file - - ## Documentation - - For detailed documentation, examples, and API reference, visit: [Franka Toolbox for MATLAB Documentation](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/index.html#) - - ## File Information - - - **File**: franka.mltbx - - **Size**: ${{ steps.check-dist.outputs.size }} - - **Version**: $VERSION - EOF - fi - - echo "changelog<> $GITHUB_OUTPUT - cat RELEASE_NOTES.md >> $GITHUB_OUTPUT - echo "EOF" >> $GITHUB_OUTPUT - - - name: Create Release - id: create_release - uses: softprops/action-gh-release@v1 - with: - tag_name: ${{ steps.version.outputs.version }} - name: Franka Toolbox for MATLAB ${{ steps.version.outputs.version }} - body: ${{ steps.changelog.outputs.changelog }} - draft: false - prerelease: ${{ github.event.inputs.prerelease || false }} - files: | - dist/franka.mltbx - CHANGELOG.md - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - - - name: Summary - run: | - echo "## Release Summary" >> $GITHUB_STEP_SUMMARY - echo "- **Version**: ${{ steps.version.outputs.version }}" >> $GITHUB_STEP_SUMMARY - echo "- **Distribution File**: franka.mltbx (${{ steps.check-dist.outputs.size }})" >> $GITHUB_STEP_SUMMARY - echo "- **Release URL**: https://github.com/${{ github.repository }}/releases/tag/${{ steps.version.outputs.version }}" >> $GITHUB_STEP_SUMMARY - echo "- **Pre-release**: ${{ github.event.inputs.prerelease || false }}" >> $GITHUB_STEP_SUMMARY diff --git a/.gitignore b/.gitignore index 11444cf..5c85f4d 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,13 @@ dist/franka_matlab tmp/ # But include the distribution file for releases !dist/franka.mltbx + +# Build artifacts (generated by docker/build.sh and CI) +dependencies/ +**/bin.zip +**/bin_arm.zip +franka_robot_server/bin.tar.gz +franka_robot_server/bin_arm.tar.gz bin bin_arm *_ert_rtw diff --git a/CHANGELOG.md b/CHANGELOG.md index b084fb1..2aa2816 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,14 @@ # Changelog: +## 4.0.0 (30-03-2026) + + - New auto-build system for franka.mtlbx under github release page. + - Latest libfranka build for fr3. + - FrankaRobot API: Added `FrankaRobotSettings` class for centralized robot configuration. + - FrankaRobot API: New methods for impedance control (`setJointImpedance`, `setCartesianImpedance`), guiding mode (`setGuidingMode`), frame transformations (`setEE`, `setK`), and motion control (`stop`). + - FrankaRobot API: Improved server lifecycle handling. + - FrankaRobot API: Support for multiple `FrankaRobot` instances. + ## 3.1.0 (20-10-2025) - Franka Toolbox for MATLAB open source release. \ No newline at end of file diff --git a/README.md b/README.md index c6ef416..758d90a 100644 --- a/README.md +++ b/README.md @@ -1,99 +1,63 @@ -# Franka Toolbox for MATLAB: Matlab & Simulink library for Franka Robotics research robots +# Franka Toolbox for MATLAB -A Simulink & Matlab library and tools for the Franka Robotics Robots based on the [Franka Control Interface (FCI)](https://frankarobotics.github.io/docs/). See the [documentation page](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/index.html) for more information. +**MATLAB® & Simulink® integration for the Franka Robot** -The repository includes a complete franka.mtlbx distribution (including pre-built binaries) which you can find either under the `dist` folder or in the github release page. +![Simulink Library for Franka Robots](docs/franka_matlab/_static/simulink_library_browser.png) -You can alternatively build the project for your own machine configuration following the instructions bellow. +## Get Started in Minutes -## Generating a franka.mtlbx distribution +Pre-built toolbox packages are available on the [**GitHub Releases**](releases) page — no compilation required! -1. Requirements: +| Package | Robot | Description | +|---------|-------|-------------| +| `franka-fr3.mltbx` | FR3 (Franka Research 3) | For current-generation robots | +| `franka-fer.mltbx` | FER | For first-generation robots | -- Host PC: Windows - - MATLAB R2022a or newer - - Microsoft Visual Studio 2019 or newer - - CMake 3.15 or newer - - Git - - Cap'n Proto: check https://capnproto.org/install.html#installation-windows +### Installation -- Host PC: Linux - - MATLAB R2022a or newer - - Build essentials (gcc, g++, make) - - CMake 3.15 or newer - - Git - - Cap'n Proto (build static library): - ```bash - # Install build dependencies - sudo apt-get update - sudo apt-get install -y build-essential cmake autoconf pkg-config libtool +1. Download the appropriate `.mltbx` file from [Releases](releases) +2. Double-click the file, or drag-and-drop it into MATLAB +3. Run `franka_toolbox_install()` in MATLAB - # Download and build Cap'n Proto from source - curl -O https://capnproto.org/capnproto-c++-1.1.0.tar.gz - tar zxf capnproto-c++-1.1.0.tar.gz - cd capnproto-c++-1.1.0 - mkdir build && - cd build && - cmake -DBUILD_TESTING=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. && - make -j10 && - sudo make install && - cd ../.. && - rm -rf capnproto-c++-1.1.0* && - sudo ldconfig - ``` +That's it! You're ready to control your Franka robot from MATLAB and Simulink. -- Target PC: Host PC Linux or Jetson platform connected - - libfranka's 3d party dependencies (based on libfranka's version). Refer to the [Documentation](https://github.com/frankarobotics/libfranka/blob/main/README.md) for more information. The dependencies in the local Linux native target PC (not AI Companion) should be installed in the native system and not in a container. For the AI compantion a running container can be targeted as well. - - ssh keys setup for connection without password request. - - Cap'n Proto (build static library): - ```bash - # Install build dependencies - sudo apt-get update - sudo apt-get install -y build-essential cmake autoconf pkg-config libtool +## Features - # Install capnproto - curl -O https://capnproto.org/capnproto-c++-1.1.0.tar.gz && - tar zxf capnproto-c++-1.1.0.tar.gz && - cd capnproto-c++-1.1.0 && - mkdir build && - cd build && - cmake -DBUILD_TESTING=OFF .. && - make -j3 && - sudo make install && - cd ../.. && - rm -rf capnproto-c++-1.1.0* && - sudo ldconfig - ``` +- **Simulink Library** — Ready-to-use blocks for robot state, gripper control, dynamics (mass matrix, Coriolis, gravity, Jacobian), and real-time control +- **MATLAB Classes** — `FrankaRobot`, `FrankaGripper`, and `FrankaVacuumGripper` for programmatic robot control +- **Simulink Coder Support** — Generate and deploy C++ code to real-time Linux targets +- **Cross-Platform** — Works on Windows and Linux, targets x86_64 and ARM64 (Jetson) -2. Set the desired libfranka version in the `libfranka_ver.csv` file under the `configs` folder +## Documentation -3. Build the binaries: +**[Franka Toolbox for MATLAB Documentation](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/index.html)** + - Installation & getting started + - Simulink block reference + - MATLAB API reference + - Troubleshooting +**[Franka Control Interface (FCI) Documentation](https://frankarobotics.github.io/docs/index.html)** + - Robot setup & network configuration + - libfranka reference + - System requirements - 1. If executed in windows: franka_toolbox_binaries_all_build(); will generate the necessary binaries for windows host (no libfranka related). +## Examples - 2. If executed in Linux: franka_toolbox_binaries_all_build(); will generate the necessary binaries for linux host (no libfranka related) + the necessary binaries for linux as target (libfranka dependent). +The toolbox includes ready-to-run examples: - 3. If executed with AI Companion connected as well from Linux host: franka_toolbox_binaries_all_build(user,ip,port); will generate the necessary binaries for linux host (no libfranka related) + the necessary binaries for linux as target (libfranka dependent) + the necessary binaries for the AI Companion (libfranka dependent). +- Joint & Cartesian motion generation +- Impedance control (joint & Cartesian) +- Force control +- Gripper operations +- Pick-and-place with RRT path planning - For generating all the necessary binaries for the final project packaging & distribution --> Steps 1. & 3. MUST be performed! +## Building from Source - a. First from a Ubuntu Host PC with AI companion PC connected with user, ip, port provided +The released `.mltbx` packages include pre-built binaries for the default supported setup. If you need a different `libfranka` version, you can build with Docker via `docker/build.sh --libfranka `. +You can also choose `docker/build.sh amd64` for a local host target or `docker/build.sh arm64` for a Jetson target instead of building both architectures. - ``` - >> franka_toolbox_binaries_all_build(user, ip, port); - ``` +If you need to build the toolbox yourself, see the [Custom Build Guide](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/custom_build.html) in the documentation. - b. Then from a Windows Host PC (no Jetson connected) +## License - ``` - >> franka_toolbox_binaries_all_build(); - ``` - - For Windows only the Simulink sfunctions and the Matlab Franka Robot client will be built. - -4. Generate the distribution (Tested in Linux Host PC): - - ``` - >> franka_toolbox_dist_make(); - ``` \ No newline at end of file +Copyright © 2025 Franka Robotics GmbH diff --git a/cartesian_impedance_control b/cartesian_impedance_control deleted file mode 100755 index 60e95c5..0000000 Binary files a/cartesian_impedance_control and /dev/null differ diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index cd7b6ac..15cfa2b 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -7,7 +7,7 @@ project(franka_matlab list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/common/bin.zip b/common/bin.zip deleted file mode 100644 index 9cc9222..0000000 Binary files a/common/bin.zip and /dev/null differ diff --git a/common/bin_arm.zip b/common/bin_arm.zip deleted file mode 100644 index aff81f4..0000000 Binary files a/common/bin_arm.zip and /dev/null differ diff --git a/common/franka_toolbox_common_build.m b/common/franka_common_build.m similarity index 61% rename from common/franka_toolbox_common_build.m rename to common/franka_common_build.m index 41e4f7a..6e0cfbd 100644 --- a/common/franka_toolbox_common_build.m +++ b/common/franka_common_build.m @@ -1,4 +1,4 @@ -function franka_toolbox_common_build(user,ip,port) +function franka_common_build(user,ip,port) if nargin == 0 @@ -23,23 +23,17 @@ function franka_toolbox_common_build(user,ip,port) rmdir(buildDir,'s'); end fprintf('Creating new build directory...\n'); - [status, cmdout] = franka_toolbox_system_cmd('mkdir build',fullfile(installation_path, 'common')); - if status ~= 0 - error('Failed to create build directory: %s', cmdout); - end + opts = struct('nothrow', false); + franka_toolbox_local_exec('mkdir build', fullfile(installation_path, 'common'), opts); % Build common library for the Franka Toolbox for MATLAB fprintf('Configuring CMake build...\n'); - [status, cmdout] = franka_toolbox_system_cmd(['cmake -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=',fullfile(installation_path,'libfranka', 'build'),' ..'],buildDir); - if status ~= 0 - error('CMake configuration failed: %s', cmdout); - end + cmake_cmd = ['cmake -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=', ... + fullfile(installation_path,'libfranka', 'build'), ' ..']; + franka_toolbox_local_exec(cmake_cmd, buildDir, opts); fprintf('Building library...\n'); - [status, cmdout] = franka_toolbox_system_cmd('cmake --build .',buildDir); - if status ~= 0 - error('CMake build failed: %s', cmdout); - end + franka_toolbox_local_exec('cmake --build .', buildDir, opts); % common if ~isfolder(fullfile(installation_path,'common','bin')) @@ -77,22 +71,31 @@ function franka_toolbox_common_build(user,ip,port) % common fprintf('Preparing remote build...\n'); + sshOpts = struct('verbose', true, 'nothrow', false); + scpOpts = struct('recursive', true, 'nothrow', false); + % Check if remote directory exists before removing - [~, cmdout] = franka_toolbox_remote_system_cmd('ls -d ~/franka_matlab 2>/dev/null || echo "Directory does not exist"', '', user, ip, port,true); - if ~contains(cmdout, 'Directory does not exist') - franka_toolbox_remote_system_cmd('rm -rf ~/franka_matlab', '', user, ip, port,true); + [status, ~] = franka_toolbox_ssh_exec('ls -d ~/franka_matlab 2>/dev/null', user, ip, port); + if status == 0 + franka_toolbox_ssh_exec('rm -rf ~/franka_matlab', user, ip, port, sshOpts); end - franka_toolbox_remote_system_cmd('mkdir franka_matlab','~',user,ip,port,true); - franka_toolbox_foder_remote_cp(fullfile(franka_toolbox_installation_path_get(),'common'),user,ip,'franka_matlab',port,true); - franka_toolbox_foder_remote_cp(fullfile(franka_toolbox_installation_path_get(),'libfranka_arm'),user,ip,'franka_matlab',port,true); - franka_toolbox_remote_system_cmd('mkdir build',fullfile('~','franka_matlab','common'),user,ip,port,true); - franka_toolbox_remote_system_cmd([ - 'cmake -DCMAKE_BUILD_TYPE=Release ' ... - '-DFranka_DIR:PATH=', fullfile('~','franka_matlab','libfranka_arm','build'), ' ' ... - '-DFRANKA_FOLDER:STRING=libfranka_arm ' ... - '..' - ], fullfile('~','franka_matlab','common','build'), user, ip, port, true); - franka_toolbox_remote_system_cmd('cmake --build .',fullfile('~','franka_matlab', 'common', 'build'),user,ip,port,true); + franka_toolbox_ssh_exec('mkdir franka_matlab', user, ip, port, sshOpts); + + % Copy folders to remote + franka_toolbox_scp(fullfile(franka_toolbox_installation_path_get(),'common'), ... + ':franka_matlab/', user, ip, port, scpOpts); + franka_toolbox_scp(fullfile(franka_toolbox_installation_path_get(),'libfranka_arm'), ... + ':franka_matlab/', user, ip, port, scpOpts); + + % Build on remote + franka_toolbox_ssh_exec('mkdir build', user, ip, port, sshOpts); + + cmake_cmd = ['cd ~/franka_matlab/common/build && cmake -DCMAKE_BUILD_TYPE=Release ' ... + '-DFranka_DIR:PATH=~/franka_matlab/libfranka_arm/build ' ... + '-DFRANKA_FOLDER:STRING=libfranka_arm ..']; + franka_toolbox_ssh_exec(cmake_cmd, user, ip, port, sshOpts); + + franka_toolbox_ssh_exec('cd ~/franka_matlab/common/build && cmake --build .', user, ip, port, sshOpts); if ~isfolder(fullfile(installation_path,'common','bin_arm')) mkdir(fullfile(installation_path,'common','bin_arm')); @@ -102,7 +105,10 @@ function franka_toolbox_common_build(user,ip,port) unzip(fullfile(installation_path,'common','bin_arm.zip'),fullfile(installation_path,'common')); end - franka_toolbox_foder_from_remote_cp(fullfile('~','franka_matlab','common','build','libfranka_matlab.a'),fullfile(installation_path,'common','bin_arm','libfranka_matlab.a'),user,ip,port,true); + % Copy built library from remote + franka_toolbox_scp(':~/franka_matlab/common/build/libfranka_matlab.a', ... + fullfile(installation_path,'common','bin_arm','libfranka_matlab.a'), ... + user, ip, port, scpOpts); zip(fullfile(installation_path,'common','bin_arm.zip'),fullfile(installation_path,'common','bin_arm')); diff --git a/common/src/robot_api.cpp b/common/src/robot_api.cpp index f98e469..c099c2c 100644 --- a/common/src/robot_api.cpp +++ b/common/src/robot_api.cpp @@ -67,6 +67,13 @@ SimulinkFrankaRobot::SimulinkFrankaRobot(const char * robotIPmask, robot = std::make_unique(robotIPString); + try { + robot->automaticErrorRecovery(); + } catch (const franka::Exception& e) { + // Robot might already be in a good state or error is unrecoverable + // Log but continue - the actual control loop will catch persistent errors + } + robotModel = std::make_unique(robot->loadModel()); } @@ -105,6 +112,13 @@ SimulinkFrankaRobot& SimulinkFrankaRobot::operator=(SimulinkFrankaRobot&& otherS robot = std::make_unique(robotIPString); + try { + robot->automaticErrorRecovery(); + } catch (const franka::Exception& e) { + // Robot might already be in a good state or error is unrecoverable + // Log but continue - the actual control loop will catch persistent errors + } + robotModel = std::make_unique(robot->loadModel()); return *this; diff --git a/config/libfranka_ver.csv b/config/libfranka_ver.csv index 2a0970c..1b619f3 100644 --- a/config/libfranka_ver.csv +++ b/config/libfranka_ver.csv @@ -1 +1 @@ -0.16.1 +0.20.5 diff --git a/dependencies/libfranka.zip b/dependencies/libfranka.zip deleted file mode 100644 index 560d6d4..0000000 Binary files a/dependencies/libfranka.zip and /dev/null differ diff --git a/dependencies/libfranka_arm.zip b/dependencies/libfranka_arm.zip deleted file mode 100644 index 2cf7b67..0000000 Binary files a/dependencies/libfranka_arm.zip and /dev/null differ diff --git a/dist/franka.mltbx b/dist/franka.mltbx index 7c8fff0..2f9b85d 100644 Binary files a/dist/franka.mltbx and b/dist/franka.mltbx differ diff --git a/doc/GettingStarted.mlx b/doc/GettingStarted.mlx index 136fc2c..4325178 100644 Binary files a/doc/GettingStarted.mlx and b/doc/GettingStarted.mlx differ diff --git a/docker/Dockerfile.amd64 b/docker/Dockerfile.amd64 new file mode 100644 index 0000000..75abb6e --- /dev/null +++ b/docker/Dockerfile.amd64 @@ -0,0 +1,97 @@ +# Dockerfile for building Franka Toolbox target binaries (x86_64/amd64) +# Ubuntu 22.04 base with all necessary build dependencies + +FROM ubuntu:22.04 AS builder + +# Avoid interactive prompts during package installation +ENV DEBIAN_FRONTEND=noninteractive + +# Install build dependencies +RUN apt-get update && apt-get install -y \ + build-essential \ + cmake \ + git \ + wget \ + curl \ + pkg-config \ + libtool \ + autoconf \ + fuse \ + file \ + patchelf \ + zip \ + # libfranka dependencies + libpoco-dev \ + libeigen3-dev \ + libnlopt-dev \ + libccd-dev \ + libfcl-dev \ + liburdfdom-dev \ + liburdfdom-headers-dev \ + liboctomap-dev \ + libboost-all-dev \ + libtinyxml2-dev \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + +# Build and install Pinocchio 3.4.0 (required by libfranka) +# Only clone the cmake submodule, skip robot-data which is huge and unnecessary +RUN git clone https://github.com/stack-of-tasks/pinocchio /opt/pinocchio && \ + cd /opt/pinocchio && \ + git checkout v3.4.0 && \ + git submodule update --init cmake && \ + mkdir build && \ + cd build && \ + cmake .. \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DBUILD_PYTHON_INTERFACE=OFF \ + -DBUILD_TESTING=OFF \ + -DBUILD_WITH_COLLISION_SUPPORT=OFF \ + -DBUILD_WITH_AUTODIFF_SUPPORT=OFF \ + -DBUILD_WITH_CASADI_SUPPORT=OFF \ + -DBUILD_WITH_CODEGEN_SUPPORT=OFF && \ + make -j$(nproc) && \ + make install && \ + cd / && \ + rm -rf /opt/pinocchio && \ + ldconfig + +# Set environment variables for pinocchio +ENV PATH=/usr/local/bin:${PATH} +ENV PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:${PKG_CONFIG_PATH} +ENV LD_LIBRARY_PATH=/usr/local/lib:${LD_LIBRARY_PATH} +ENV CMAKE_PREFIX_PATH=/usr/local:${CMAKE_PREFIX_PATH} + +# Build and install Cap'n Proto (static library) +ARG CAPNP_VERSION=1.1.0 +RUN curl -O https://capnproto.org/capnproto-c++-${CAPNP_VERSION}.tar.gz && \ + tar zxf capnproto-c++-${CAPNP_VERSION}.tar.gz && \ + cd capnproto-c++-${CAPNP_VERSION} && \ + mkdir build && \ + cd build && \ + cmake -DBUILD_TESTING=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. && \ + make -j$(nproc) && \ + make install && \ + cd ../.. && \ + rm -rf capnproto-c++-${CAPNP_VERSION}* && \ + ldconfig + +# Download linuxdeploy for bundling dependencies +RUN mkdir -p /opt/tools && \ + wget -q -O /opt/tools/linuxdeploy-x86_64.AppImage \ + https://github.com/linuxdeploy/linuxdeploy/releases/download/1-alpha-20250213-2/linuxdeploy-x86_64.AppImage && \ + chmod +x /opt/tools/linuxdeploy-x86_64.AppImage && \ + cd /opt/tools && ./linuxdeploy-x86_64.AppImage --appimage-extract && \ + mv squashfs-root linuxdeploy + +# Set working directory +WORKDIR /workspace + +# Copy build scripts +COPY docker/scripts/ /scripts/ +RUN chmod +x /scripts/*.sh + +# Default command +ENTRYPOINT ["/scripts/entrypoint.sh"] +CMD ["all"] diff --git a/docker/Dockerfile.arm64 b/docker/Dockerfile.arm64 new file mode 100644 index 0000000..e742121 --- /dev/null +++ b/docker/Dockerfile.arm64 @@ -0,0 +1,175 @@ +# Dockerfile for cross-compiling Franka Toolbox target binaries (aarch64/arm64) +# Ubuntu 22.04 base with multiarch support for Jetson Orin + +FROM ubuntu:22.04 AS builder + +# Avoid interactive prompts during package installation +ENV DEBIAN_FRONTEND=noninteractive + +# 1. Setup Multiarch (AMD64 host, ARM64 target) +# - Enable arm64 architecture +# - Update sources.list to separate amd64 (archive) and arm64 (ports) +RUN dpkg --add-architecture arm64 && \ + cp /etc/apt/sources.list /etc/apt/sources.list.bak && \ + sed -i 's/^deb /deb [arch=amd64] /g' /etc/apt/sources.list && \ + echo "deb [arch=arm64] http://ports.ubuntu.com/ubuntu-ports jammy main universe multiverse restricted" >> /etc/apt/sources.list && \ + echo "deb [arch=arm64] http://ports.ubuntu.com/ubuntu-ports jammy-updates main universe multiverse restricted" >> /etc/apt/sources.list && \ + echo "deb [arch=arm64] http://ports.ubuntu.com/ubuntu-ports jammy-security main universe multiverse restricted" >> /etc/apt/sources.list + +# 2. Install build tools and dependencies +RUN apt-get update && apt-get install -y \ + build-essential \ + cmake \ + git \ + wget \ + curl \ + pkg-config \ + patchelf \ + zip \ + # Cross-compilation toolchain + crossbuild-essential-arm64 \ + # Target Libraries (ARM64) - Installing these prevents manual compilation! + libeigen3-dev:arm64 \ + # libboost-all-dev pulls in MPI which breaks multiarch on 22.04 due to gfortran conflict + libboost-system-dev:arm64 \ + libboost-filesystem-dev:arm64 \ + libboost-thread-dev:arm64 \ + libboost-program-options-dev:arm64 \ + libboost-serialization-dev:arm64 \ + libboost-chrono-dev:arm64 \ + libboost-date-time-dev:arm64 \ + libboost-atomic-dev:arm64 \ + libpoco-dev:arm64 \ + libccd-dev:arm64 \ + libfcl-dev:arm64 \ + liboctomap-dev:arm64 \ + liburdfdom-dev:arm64 \ + liburdfdom-headers-dev:arm64 \ + libconsole-bridge-dev:arm64 \ + libtinyxml2-dev:arm64 \ + libnlopt-dev:arm64 \ + libassimp-dev:arm64 \ + && apt-get clean \ + && rm -rf /var/lib/apt/lists/* + +# Set environment variables for cross-compilation +ENV PATH="/usr/local/bin:${PATH}" +ENV PKG_CONFIG_PATH="/usr/local/lib/pkgconfig:/opt/sysroot-aarch64/usr/lib/pkgconfig:/usr/lib/aarch64-linux-gnu/pkgconfig" +ENV LD_LIBRARY_PATH="/usr/local/lib" +# Note: We include both our custom sysroot and the standard multiarch paths +ENV CMAKE_PREFIX_PATH="/usr/local:/opt/sysroot-aarch64/usr:/usr/aarch64-linux-gnu/usr" + +# 3. Cross-compile hpp-fcl for ARM64 (Required by Pinocchio) +# Note: hpp-fcl's cmake config does nested find_package calls that can fail version checks +# We split clone/configure/build/install into separate layers so Docker logs show +# which stage is slow or failing and retries can reuse earlier cached layers. +ARG HPP_FCL_VERSION=v2.4.5 +RUN git clone --branch "${HPP_FCL_VERSION}" --depth 1 \ + https://github.com/humanoid-path-planner/hpp-fcl.git /opt/hpp-fcl +RUN cd /opt/hpp-fcl && git submodule update --init --depth 1 +RUN cmake -S /opt/hpp-fcl -B /opt/hpp-fcl/build \ + -DCMAKE_SYSTEM_NAME=Linux \ + -DCMAKE_SYSTEM_PROCESSOR=aarch64 \ + -DCMAKE_C_COMPILER=aarch64-linux-gnu-gcc \ + -DCMAKE_CXX_COMPILER=aarch64-linux-gnu-g++ \ + -DCMAKE_FIND_ROOT_PATH="/usr/aarch64-linux-gnu" \ + -DCMAKE_INSTALL_PREFIX=/opt/sysroot-aarch64/usr \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_PYTHON_INTERFACE=OFF \ + -DBUILD_TESTING=OFF \ + -DGENERATE_VERSION_HEADER=ON +RUN cmake --build /opt/hpp-fcl/build -j"$(nproc)" +RUN cmake --install /opt/hpp-fcl/build && rm -rf /opt/hpp-fcl + +# 4. Cross-compile Pinocchio for ARM64 +# Pinocchio is not in standard Ubuntu repos, so we build it. +# It will link against the apt-installed libs (Eigen, Boost, URDFDOM, etc.) +# Note: hpp-fcl cmake config files might be in share/ or lib/cmake - we handle both +RUN CONFIG_FILE="/opt/sysroot-aarch64/usr/lib/cmake/hpp-fcl/hpp-fclConfig.cmake" && \ + if [ -f "$CONFIG_FILE" ]; then \ + echo "Patching $CONFIG_FILE to bypass version check failure and prevent recursion" && \ + # Add include guard at the very top to prevent infinite recursion + sed -i '1i if(HPP_FCL_CONFIG_INCLUDED)\n return()\nendif()\nset(HPP_FCL_CONFIG_INCLUDED TRUE)' "$CONFIG_FILE" && \ + # Disable the fatal error check + sed -i 's/message(FATAL_ERROR/message(STATUS/g' "$CONFIG_FILE" && \ + # Force version variable + sed -i '1i set(hpp-fcl_VERSION "2.4.5")' "$CONFIG_FILE" && \ + # Force FOUND=TRUE at the END of the file to override any internal logic + echo 'set(hpp-fcl_FOUND TRUE)' >> "$CONFIG_FILE" && \ + # Ensure ConfigVersion file says we are compatible + VER_FILE="$(dirname $CONFIG_FILE)/hpp-fclConfigVersion.cmake" && \ + echo 'set(PACKAGE_VERSION "2.4.5")' > "$VER_FILE" && \ + echo 'set(PACKAGE_VERSION_COMPATIBLE TRUE)' >> "$VER_FILE" && \ + echo 'set(PACKAGE_VERSION_EXACT TRUE)' >> "$VER_FILE"; \ + else \ + echo "WARNING: hpp-fclConfig.cmake not found at $CONFIG_FILE"; \ + ls -R /opt/sysroot-aarch64/usr/lib/cmake; \ + fi && \ + git clone https://github.com/stack-of-tasks/pinocchio /opt/pinocchio && \ + cd /opt/pinocchio && \ + git checkout v3.4.0 && \ + git submodule update --init cmake && \ + mkdir build && cd build && \ + cmake .. \ + -DCMAKE_SYSTEM_NAME=Linux \ + -DCMAKE_SYSTEM_PROCESSOR=aarch64 \ + -DCMAKE_C_COMPILER=aarch64-linux-gnu-gcc \ + -DCMAKE_CXX_COMPILER=aarch64-linux-gnu-g++ \ + -DCMAKE_FIND_ROOT_PATH="/usr/aarch64-linux-gnu;/opt/sysroot-aarch64" \ + -DCMAKE_PREFIX_PATH=/opt/sysroot-aarch64/usr \ + -DCMAKE_INSTALL_PREFIX=/opt/sysroot-aarch64/usr \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_PYTHON_INTERFACE=OFF \ + -DBUILD_TESTING=OFF \ + -DBUILD_WITH_COLLISION_SUPPORT=ON \ + -DBUILD_WITH_AUTODIFF_SUPPORT=OFF \ + -DBUILD_WITH_CASADI_SUPPORT=OFF \ + -DBUILD_WITH_CODEGEN_SUPPORT=OFF \ + -Dhpp-fcl_DIR=/opt/sysroot-aarch64/usr/lib/cmake/hpp-fcl \ + -Dhpp-fcl_VERSION=2.4.5 && \ + make -j$(nproc) && make install && \ + cd / && rm -rf /opt/pinocchio + +# 4. Build Cap'n Proto (HOST and TARGET) +# We stick to manual build to ensure version consistency (1.1.0) +# as Ubuntu 22.04 only has 0.9.1 which might be too old. +ARG CAPNP_VERSION=1.1.0 +RUN curl -O https://capnproto.org/capnproto-c++-${CAPNP_VERSION}.tar.gz && \ + tar zxf capnproto-c++-${CAPNP_VERSION}.tar.gz && \ + # Host build (for capnp tool) + cd capnproto-c++-${CAPNP_VERSION} && \ + mkdir build-host && cd build-host && \ + # Clean cross-compilation env vars for host build + PKG_CONFIG_PATH="" CMAKE_PREFIX_PATH="" CC=gcc CXX=g++ cmake -DBUILD_TESTING=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. && \ + make -j$(nproc) && make install && \ + # Target build (for ARM64 libraries) + cd .. && mkdir build-arm64 && cd build-arm64 && \ + cmake \ + -DCMAKE_SYSTEM_NAME=Linux \ + -DCMAKE_SYSTEM_PROCESSOR=aarch64 \ + -DCMAKE_C_COMPILER=aarch64-linux-gnu-gcc \ + -DCMAKE_CXX_COMPILER=aarch64-linux-gnu-g++ \ + -DCMAKE_FIND_ROOT_PATH="/usr/aarch64-linux-gnu" \ + -DCMAKE_FIND_ROOT_PATH_MODE_PROGRAM=NEVER \ + -DCMAKE_FIND_ROOT_PATH_MODE_LIBRARY=ONLY \ + -DCMAKE_FIND_ROOT_PATH_MODE_INCLUDE=ONLY \ + -DBUILD_TESTING=OFF \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/opt/sysroot-aarch64/usr \ + .. && \ + make -j$(nproc) && make install && \ + cd ../.. && rm -rf capnproto-c++-${CAPNP_VERSION}* + +# Download linuxdeploy for arm64 +RUN mkdir -p /opt/tools && \ + wget -q -O /opt/tools/linuxdeploy-aarch64.AppImage \ + https://github.com/linuxdeploy/linuxdeploy/releases/download/1-alpha-20250213-2/linuxdeploy-aarch64.AppImage && \ + chmod +x /opt/tools/linuxdeploy-aarch64.AppImage + +WORKDIR /workspace + +COPY docker/scripts/ /scripts/ +RUN chmod +x /scripts/*.sh + +ENTRYPOINT ["/scripts/entrypoint.sh"] +CMD ["all", "--arch", "arm64"] diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 0000000..fb2f07c --- /dev/null +++ b/docker/README.md @@ -0,0 +1,38 @@ +# Docker Build System + +This directory contains Docker infrastructure for building Franka Toolbox server binaries. + +## Quick Start + +```bash +cd docker +./build.sh # Build for both x86_64 and ARM64 +./build.sh amd64 # x86_64 only +./build.sh arm64 # ARM64 only (cross-compiled) +``` + +## Output + +| File | Description | +|------|-------------| +| `common/bin.zip` | x86_64 common library | +| `common/bin_arm.zip` | ARM64 common library | +| `franka_robot_server/bin.tar.gz` | x86_64 server | +| `franka_robot_server/bin_arm.tar.gz` | ARM64 server | +| `dependencies/libfranka.zip` | x86_64 libfranka | +| `dependencies/libfranka_arm.zip` | ARM64 libfranka | + +## Options + +```bash +./build.sh [arch] [options] + + amd64 | arm64 | all Architecture (default: all) + --no-cache Rebuild without Docker cache + --libfranka Override libfranka version + --build-type Release or Debug (default: Release) +``` + +## Documentation + +For complete build instructions including MEX files and distribution packaging, see the [Custom Build Guide](https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/custom_build.html). diff --git a/docker/build.sh b/docker/build.sh new file mode 100755 index 0000000..b84c70d --- /dev/null +++ b/docker/build.sh @@ -0,0 +1,350 @@ +#!/bin/bash +# Main build script for Franka Toolbox target binaries +# This script orchestrates Docker builds for both amd64 and arm64 architectures +# +# Usage: +# ./build.sh # Build for both architectures +# ./build.sh amd64 # Build for x86_64 only +# ./build.sh arm64 # Build for ARM64 only (cross-compilation) +# ./build.sh --help # Show help + +set -e + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)" + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +BLUE='\033[0;34m' +CYAN='\033[0;36m' +NC='\033[0m' # No Color + +log_info() { + echo -e "${BLUE}[INFO]${NC} $1" +} + +log_success() { + echo -e "${GREEN}[SUCCESS]${NC} $1" +} + +log_warn() { + echo -e "${YELLOW}[WARNING]${NC} $1" +} + +log_error() { + echo -e "${RED}[ERROR]${NC} $1" +} + +log_header() { + echo -e "${CYAN}=====================================================${NC}" + echo -e "${CYAN} $1${NC}" + echo -e "${CYAN}=====================================================${NC}" +} + +print_dir_listing() { + local dir_path="$1" + local label="$2" + + echo " ${label}: ${dir_path}" + if [[ -d "${dir_path}" ]]; then + ls -la "${dir_path}" || true + else + echo " (directory not found)" + fi +} + +require_file() { + local file_path="$1" + local description="$2" + + if [[ ! -f "${file_path}" ]]; then + log_error "Missing ${description}: ${file_path}" + return 1 + fi +} + +show_help() { + echo "Franka Toolbox Docker Build System" + echo "" + echo "Usage: $0 [architecture] [options]" + echo "" + echo "Architectures:" + echo " amd64 Build for x86_64 (native Linux build)" + echo " arm64 Build for ARM64 (cross-compilation for Jetson)" + echo " all Build for both architectures (default)" + echo "" + echo "Options:" + echo " --no-cache Build without Docker cache" + echo " --libfranka Specify libfranka version (default: from config)" + echo " --build-type Build type: Release or Debug (default: Release)" + echo " --help, -h Show this help message" + echo "" + echo "Examples:" + echo " $0 # Build everything" + echo " $0 amd64 # Build x86_64 binaries only" + echo " $0 arm64 # Build ARM64 binaries only" + echo " $0 amd64 --no-cache # Rebuild x86_64 without cache" + echo " $0 --libfranka 0.16.1 # Build with specific libfranka version" + echo "" + echo "Output:" + echo " common/bin.zip # x86_64 common library" + echo " common/bin_arm.zip # ARM64 common library" + echo " franka_robot_server/bin.tar.gz # x86_64 server" + echo " franka_robot_server/bin_arm.tar.gz # ARM64 server" + echo " dependencies/libfranka.zip # x86_64 libfranka" + echo " dependencies/libfranka_arm.zip # ARM64 libfranka" +} + +# Default values +BUILD_ARCH="all" +NO_CACHE="" +LIBFRANKA_VERSION="" +BUILD_TYPE="Release" + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + amd64|arm64|all) + BUILD_ARCH="$1" + shift + ;; + --no-cache) + NO_CACHE="--no-cache" + shift + ;; + --libfranka) + LIBFRANKA_VERSION="$2" + shift 2 + ;; + --build-type) + BUILD_TYPE="$2" + shift 2 + ;; + --help|-h) + show_help + exit 0 + ;; + *) + log_error "Unknown argument: $1" + show_help + exit 1 + ;; + esac +done + +# Check Docker is installed and running +if ! command -v docker &> /dev/null; then + log_error "Docker is not installed. Please install Docker first." + exit 1 +fi + +if ! docker info &> /dev/null; then + log_error "Docker daemon is not running. Please start Docker." + exit 1 +fi + +# Read libfranka version from config if not provided +if [[ -z "$LIBFRANKA_VERSION" ]]; then + if [[ -f "${PROJECT_ROOT}/config/libfranka_ver.csv" ]]; then + LIBFRANKA_VERSION=$(cat "${PROJECT_ROOT}/config/libfranka_ver.csv" | head -1 | tr -d '\r\n') + else + log_error "libfranka version not specified and config/libfranka_ver.csv not found" + exit 1 + fi +fi + +log_header "Franka Toolbox Docker Build" +log_info "Project Root: ${PROJECT_ROOT}" +log_info "Architecture: ${BUILD_ARCH}" +log_info "Build Type: ${BUILD_TYPE}" +log_info "Libfranka Version: ${LIBFRANKA_VERSION}" +echo "" + +# Create output directories +mkdir -p "${PROJECT_ROOT}/common" +mkdir -p "${PROJECT_ROOT}/franka_robot_server" +mkdir -p "${PROJECT_ROOT}/dependencies" + +# Build function for a specific architecture +build_for_arch() { + local arch=$1 + local dockerfile="Dockerfile.${arch}" + local image_name="franka-toolbox-builder-${arch}" + local container_output="/tmp/franka-build-output-${arch}" + + log_header "Building for ${arch}" + + # Fix any existing root-owned files in project directories (from previous failed runs) + log_info "Fixing existing file permissions..." + docker run --rm -v "${PROJECT_ROOT}:/workspace:rw" alpine sh -c \ + "chown -R $(id -u):$(id -g) /workspace/common /workspace/franka_robot_server /workspace/dependencies /workspace/libfranka /workspace/libfranka_arm 2>/dev/null || true" + + # Build Docker image + log_info "Building Docker image: ${image_name}..." + docker build \ + ${NO_CACHE} \ + -f "${SCRIPT_DIR}/${dockerfile}" \ + -t "${image_name}" \ + "${PROJECT_ROOT}" + + # Create temp output directory + rm -rf "${container_output}" + mkdir -p "${container_output}" + + # Run the build container + log_info "Running build container..." + docker run --rm \ + -v "${PROJECT_ROOT}:/workspace:rw" \ + -v "${PROJECT_ROOT}/docker/scripts:/scripts:ro" \ + -v "${container_output}:/output:rw" \ + --privileged \ + "${image_name}" \ + all \ + --arch "${arch}" \ + --build-type "${BUILD_TYPE}" \ + --libfranka-version "${LIBFRANKA_VERSION}" + + # Fix ownership of workspace files (libfranka clone, build artifacts, etc.) + log_info "Fixing workspace file permissions..." + docker run --rm -v "${PROJECT_ROOT}:/workspace:rw" alpine sh -c \ + "chown -R $(id -u):$(id -g) /workspace/common /workspace/franka_robot_server /workspace/dependencies /workspace/libfranka /workspace/libfranka_arm 2>/dev/null || true" + + # Fix ownership of output files using docker (avoids needing sudo password) + log_info "Fixing file ownership..." + docker run --rm \ + -v "${container_output}:/output:rw" \ + alpine chown -R "$(id -u):$(id -g)" /output + + # Copy output files to project + log_info "Copying build artifacts..." + + if [[ "$arch" == "amd64" ]]; then + local expected_output_paths=( + "${container_output}/bin.zip" + "${container_output}/bin.tar.gz" + "${container_output}/libfranka.zip" + ) + local expected_output_labels=( + "x86_64 common archive" + "x86_64 server archive" + "x86_64 libfranka archive" + ) + local project_output_paths=( + "${PROJECT_ROOT}/common/bin.zip" + "${PROJECT_ROOT}/franka_robot_server/bin.tar.gz" + "${PROJECT_ROOT}/dependencies/libfranka.zip" + ) + + else + local expected_output_paths=( + "${container_output}/bin_arm.zip" + "${container_output}/bin_arm.tar.gz" + "${container_output}/libfranka_arm.zip" + ) + local expected_output_labels=( + "ARM64 common archive" + "ARM64 server archive" + "ARM64 libfranka archive" + ) + local project_output_paths=( + "${PROJECT_ROOT}/common/bin_arm.zip" + "${PROJECT_ROOT}/franka_robot_server/bin_arm.tar.gz" + "${PROJECT_ROOT}/dependencies/libfranka_arm.zip" + ) + fi + + local idx + for idx in "${!expected_output_paths[@]}"; do + if ! require_file "${expected_output_paths[$idx]}" "${expected_output_labels[$idx]}"; then + log_error "Docker build completed without exporting all expected ${arch} archives." + print_dir_listing "${container_output}" "Container output directory" + exit 1 + fi + done + + if [[ "$arch" == "amd64" ]]; then + cp "${container_output}/bin.zip" "${PROJECT_ROOT}/common/" + cp "${container_output}/bin.tar.gz" "${PROJECT_ROOT}/franka_robot_server/" + cp "${container_output}/libfranka.zip" "${PROJECT_ROOT}/dependencies/" + else + cp "${container_output}/bin_arm.zip" "${PROJECT_ROOT}/common/" + cp "${container_output}/bin_arm.tar.gz" "${PROJECT_ROOT}/franka_robot_server/" + cp "${container_output}/libfranka_arm.zip" "${PROJECT_ROOT}/dependencies/" + fi + + for idx in "${!project_output_paths[@]}"; do + if ! require_file "${project_output_paths[$idx]}" "${expected_output_labels[$idx]} in project root"; then + log_error "Artifact copy failed for ${arch}." + print_dir_listing "${container_output}" "Container output directory" + print_dir_listing "${PROJECT_ROOT}/common" "Project common directory" + print_dir_listing "${PROJECT_ROOT}/franka_robot_server" "Project server directory" + print_dir_listing "${PROJECT_ROOT}/dependencies" "Project dependencies directory" + exit 1 + fi + done + + # Cleanup + rm -rf "${container_output}" + + log_success "Build for ${arch} completed!" +} + +# Execute builds +case $BUILD_ARCH in + amd64) + build_for_arch "amd64" + ;; + arm64) + build_for_arch "arm64" + ;; + all) + build_for_arch "amd64" + build_for_arch "arm64" + ;; +esac + +log_header "Build Summary" +echo "" +log_info "Output files:" +echo "" + +if [[ "$BUILD_ARCH" == "amd64" ]] || [[ "$BUILD_ARCH" == "all" ]]; then + echo " x86_64 (amd64):" + [[ -f "${PROJECT_ROOT}/common/bin.zip" ]] && \ + echo " ✓ common/bin.zip" || echo " ✗ common/bin.zip (missing)" + [[ -f "${PROJECT_ROOT}/franka_robot_server/bin.tar.gz" ]] && \ + echo " ✓ franka_robot_server/bin.tar.gz" || echo " ✗ franka_robot_server/bin.tar.gz (missing)" + [[ -f "${PROJECT_ROOT}/dependencies/libfranka.zip" ]] && \ + echo " ✓ dependencies/libfranka.zip" || echo " ✗ dependencies/libfranka.zip (missing)" + echo "" +fi + +if [[ "$BUILD_ARCH" == "arm64" ]] || [[ "$BUILD_ARCH" == "all" ]]; then + echo " ARM64 (arm64):" + [[ -f "${PROJECT_ROOT}/common/bin_arm.zip" ]] && \ + echo " ✓ common/bin_arm.zip" || echo " ✗ common/bin_arm.zip (missing)" + [[ -f "${PROJECT_ROOT}/franka_robot_server/bin_arm.tar.gz" ]] && \ + echo " ✓ franka_robot_server/bin_arm.tar.gz" || echo " ✗ franka_robot_server/bin_arm.tar.gz (missing)" + [[ -f "${PROJECT_ROOT}/dependencies/libfranka_arm.zip" ]] && \ + echo " ✓ dependencies/libfranka_arm.zip" || echo " ✗ dependencies/libfranka_arm.zip (missing)" + echo "" +fi + +log_success "All builds completed successfully!" + +echo "" +log_info "Next steps:" +echo " 1. Start MATLAB in the project root." +echo " 2. Build the MATLAB MEX artifacts:" +echo " franka_robot_mex()" +echo " franka_simulink_library_mex()" +echo " 3. Run the toolbox installer:" +echo " franka_toolbox_install()" +echo "" +log_info "Documentation:" +echo " - Official documentation: https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/index.html" +echo " - Custom Build guide: https://frankarobotics.github.io/docs/franka_toolbox_for_matlab/docs/franka_matlab/custom_build.html" +echo " - Repository overview: ${PROJECT_ROOT}/README.md" + diff --git a/docker/scripts/build_common.sh b/docker/scripts/build_common.sh new file mode 100755 index 0000000..a9f3f09 --- /dev/null +++ b/docker/scripts/build_common.sh @@ -0,0 +1,57 @@ +#!/bin/bash +# Build the common library (libfranka_matlab.a) +# Supports both native (amd64) and cross-compilation (arm64) + +set -e + +source /scripts/common.sh + +log_info "Building common library for ${ARCH}..." + +COMMON_PATH="${WORKSPACE}/common" +COMMON_BUILD_PATH="${COMMON_PATH}/build" +LIBFRANKA_PATH="${WORKSPACE}/${FRANKA_FOLDER}" + +# Verify libfranka exists +if [[ ! -d "$LIBFRANKA_PATH/build" ]]; then + log_error "libfranka not built. Run build_libfranka.sh first." + exit 1 +fi + +# Clean and create build directory +rm -rf "$COMMON_BUILD_PATH" +mkdir -p "$COMMON_BUILD_PATH" +cd "$COMMON_BUILD_PATH" + +# Configure CMake based on architecture +if [[ "$ARCH" == "arm64" ]]; then + log_info "Configuring common library for ARM64 cross-compilation..." + cmake \ + -DCMAKE_TOOLCHAIN_FILE=/scripts/toolchain-aarch64.cmake \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DFranka_DIR="${LIBFRANKA_PATH}/build" \ + -DFRANKA_FOLDER="${FRANKA_FOLDER}" \ + .. +else + log_info "Configuring common library for native AMD64 build..." + cmake \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DFranka_DIR="${LIBFRANKA_PATH}/build" \ + -DFRANKA_FOLDER="${FRANKA_FOLDER}" \ + .. +fi + +# Build +log_info "Building common library..." +cmake --build . -j$(nproc) + +# Copy output to bin directory +log_info "Copying build artifacts..." +BIN_PATH="${COMMON_PATH}/${BIN_FOLDER}" +mkdir -p "$BIN_PATH" +cp "${COMMON_BUILD_PATH}/libfranka_matlab.a" "$BIN_PATH/" + +log_success "Common library build completed!" +log_info "Output: ${BIN_PATH}/libfranka_matlab.a" + + diff --git a/docker/scripts/build_libfranka.sh b/docker/scripts/build_libfranka.sh new file mode 100755 index 0000000..cad8b9c --- /dev/null +++ b/docker/scripts/build_libfranka.sh @@ -0,0 +1,83 @@ +#!/bin/bash +# Build libfranka from source +# Supports both native (amd64) and cross-compilation (arm64) + +set -e + +source /scripts/common.sh + +log_info "Building libfranka ${LIBFRANKA_VERSION} for ${ARCH}..." + +LIBFRANKA_PATH="${WORKSPACE}/${FRANKA_FOLDER}" +LIBFRANKA_BUILD_PATH="${LIBFRANKA_PATH}/build" + +# Check for corrupt or incomplete git repository +if [[ -d "$LIBFRANKA_PATH" ]]; then + if [[ ! -d "$LIBFRANKA_PATH/.git" ]]; then + log_warn "libfranka directory exists but is not a git repository. Removing..." + rm -rf "$LIBFRANKA_PATH" + else + cd "$LIBFRANKA_PATH" + if ! git rev-parse --git-dir > /dev/null 2>&1; then + log_warn "libfranka git repository appears corrupt. Removing and re-cloning..." + cd "$WORKSPACE" + rm -rf "$LIBFRANKA_PATH" + fi + fi +fi + +# Clone libfranka if not present +if [[ ! -d "$LIBFRANKA_PATH" ]]; then + log_info "Cloning libfranka repository..." + cd "$WORKSPACE" + git clone --recursive https://github.com/frankarobotics/libfranka "$FRANKA_FOLDER" + cd "$LIBFRANKA_PATH" + git checkout "$LIBFRANKA_VERSION" + git submodule update --init --recursive +else + log_info "Using existing libfranka directory" + cd "$LIBFRANKA_PATH" + # Ensure we're on the correct version + # If previous clone failed/was interrupted, git operations might fail. + # We'll try to recover, otherwise fail. + git reset --hard + git fetch --all + git checkout "$LIBFRANKA_VERSION" + git submodule update --init --recursive +fi + +# Clean and create build directory +rm -rf "$LIBFRANKA_BUILD_PATH" +mkdir -p "$LIBFRANKA_BUILD_PATH" +cd "$LIBFRANKA_BUILD_PATH" + +# Configure CMake based on architecture +if [[ "$ARCH" == "arm64" ]]; then + log_info "Configuring libfranka for ARM64 cross-compilation..." + cmake \ + -DCMAKE_TOOLCHAIN_FILE=/scripts/toolchain-aarch64.cmake \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTS=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DCMAKE_PREFIX_PATH="/opt/sysroot-aarch64/usr;/usr/lib/aarch64-linux-gnu;/usr/aarch64-linux-gnu/usr" \ + -Dpinocchio_DIR="/opt/sysroot-aarch64/usr/lib/cmake/pinocchio" \ + .. +else + log_info "Configuring libfranka for native AMD64 build..." + cmake \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTS=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DCMAKE_PREFIX_PATH="/usr/local" \ + .. +fi + +# Build +log_info "Building libfranka..." +cmake --build . -j$(nproc) + +# Bundle runtime dependencies using linuxdeploy +log_info "Bundling libfranka runtime dependencies..." +bundle_libfranka_deps + +log_success "libfranka build completed!" diff --git a/docker/scripts/build_server.sh b/docker/scripts/build_server.sh new file mode 100755 index 0000000..39c94f1 --- /dev/null +++ b/docker/scripts/build_server.sh @@ -0,0 +1,74 @@ +#!/bin/bash +# Build the franka_robot_server executable +# Supports both native (amd64) and cross-compilation (arm64) + +set -e + +source /scripts/common.sh + +log_info "Building franka_robot_server for ${ARCH}..." + +SERVER_PATH="${WORKSPACE}/franka_robot_server" +SERVER_BUILD_PATH="${SERVER_PATH}/build" +LIBFRANKA_PATH="${WORKSPACE}/${FRANKA_FOLDER}" +COMMON_BIN_PATH="${WORKSPACE}/common/${BIN_FOLDER}" + +# Verify dependencies +if [[ ! -d "$LIBFRANKA_PATH/build" ]]; then + log_error "libfranka not built. Run build_libfranka.sh first." + exit 1 +fi + +if [[ ! -f "$COMMON_BIN_PATH/libfranka_matlab.a" ]]; then + log_error "Common library not built. Run build_common.sh first." + exit 1 +fi + +# Clean and create build directory +rm -rf "$SERVER_BUILD_PATH" +mkdir -p "$SERVER_BUILD_PATH" +cd "$SERVER_BUILD_PATH" + +# Configure CMake based on architecture +if [[ "$ARCH" == "arm64" ]]; then + log_info "Configuring franka_robot_server for ARM64 cross-compilation..." + + # For cross-compilation, we need to use the host's capnp compiler + # but link against ARM64 libraries + cmake \ + -DCMAKE_TOOLCHAIN_FILE=/scripts/toolchain-aarch64.cmake \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DFranka_DIR="${LIBFRANKA_PATH}/build" \ + -DFRANKA_FOLDER="${FRANKA_FOLDER}" \ + -DBIN_FOLDER="${BIN_FOLDER}" \ + -DCapnProto_DIR="/opt/sysroot-aarch64/usr/lib/cmake/CapnProto" \ + -DCAPNP_EXECUTABLE="/usr/local/bin/capnp" \ + -DCAPNPC_CXX_EXECUTABLE="/usr/local/bin/capnpc-c++" \ + .. +else + log_info "Configuring franka_robot_server for native AMD64 build..." + cmake \ + -DCMAKE_BUILD_TYPE="${BUILD_TYPE}" \ + -DFranka_DIR="${LIBFRANKA_PATH}/build" \ + -DFRANKA_FOLDER="${FRANKA_FOLDER}" \ + -DBIN_FOLDER="${BIN_FOLDER}" \ + .. +fi + +# Build +log_info "Building franka_robot_server..." +cmake --build . --config "${BUILD_TYPE}" -j$(nproc) + +# Set executable permissions +chmod +x franka_robot_server + +# Copy output to bin directory +log_info "Copying build artifacts..." +BIN_PATH="${SERVER_PATH}/${BIN_FOLDER}" +mkdir -p "$BIN_PATH" +cp "${SERVER_BUILD_PATH}/franka_robot_server" "$BIN_PATH/" + +log_success "franka_robot_server build completed!" +log_info "Output: ${BIN_PATH}/franka_robot_server" + + diff --git a/docker/scripts/common.sh b/docker/scripts/common.sh new file mode 100755 index 0000000..31c1cf1 --- /dev/null +++ b/docker/scripts/common.sh @@ -0,0 +1,164 @@ +#!/bin/bash +# Common functions and variables for build scripts + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +BLUE='\033[0;34m' +NC='\033[0m' # No Color + +log_info() { + echo -e "${BLUE}[INFO]${NC} $1" +} + +log_success() { + echo -e "${GREEN}[SUCCESS]${NC} $1" +} + +log_warn() { + echo -e "${YELLOW}[WARNING]${NC} $1" +} + +log_error() { + echo -e "${RED}[ERROR]${NC} $1" +} + +# Smart dependency bundler for ARM64 (mimics linuxdeploy using readelf) +bundle_arm64_deps_smart() { + local binary="$1" + local dest_lib="$2" + local search_paths=("/opt/sysroot-aarch64/usr/lib" "/usr/lib/aarch64-linux-gnu") + + # Excludelist: base system libs that should NOT be bundled + local exclude_pattern="^(libc\.so|libm\.so|libdl\.so|libpthread\.so|librt\.so|libgcc_s\.so|libstdc\+\+\.so|ld-linux.*\.so|libresolv\.so|libnss_.*\.so|libutil\.so)" + + # Track copied files by their real path (to avoid duplicates) + declare -A copied_files + + # Already processed libraries (to avoid infinite loops) + declare -A processed + + # Queue of libraries to process + local queue=("$binary") + + while [[ ${#queue[@]} -gt 0 ]]; do + local current="${queue[0]}" + queue=("${queue[@]:1}") # Pop first element + + # Skip if already processed + [[ -n "${processed[$current]}" ]] && continue + processed[$current]=1 + + # Get NEEDED libraries using readelf + local needed_libs=$(readelf -d "$current" 2>/dev/null | grep 'NEEDED' | sed -n 's/.*\[\(.*\)\]/\1/p') + + for lib in $needed_libs; do + # Skip if matches exclude pattern + if [[ "$lib" =~ $exclude_pattern ]]; then + continue + fi + + # Find the library in search paths + local lib_path="" + for search_path in "${search_paths[@]}"; do + if [[ -f "$search_path/$lib" ]]; then + lib_path="$search_path/$lib" + break + fi + done + + # If found, copy it intelligently (as the NEEDED name, no symlinks) + if [[ -n "$lib_path" ]]; then + # Resolve to the real file content + local real_file=$(readlink -f "$lib_path") + + # Check if we already have this library file (by content/path) provided as this name + # But wait, we want to ensure 'lib' (NEEDED name) exists in dest. + + if [[ ! -f "$dest_lib/$lib" ]]; then + # Copy the real file content to the destination AS the NEEDED name + cp "$real_file" "$dest_lib/$lib" 2>/dev/null || true + fi + + # Add real file to queue for recursive dependency analysis + [[ -z "${processed[$real_file]}" ]] && queue+=("$real_file") + fi + done + done +} + +# Bundle libfranka runtime dependencies using linuxdeploy +bundle_libfranka_deps() { + local libfranka_build="${WORKSPACE}/${FRANKA_FOLDER}/build" + + # Read libfranka version for the shared library name + local libfranka_ver="${LIBFRANKA_VERSION}" + if [[ -z "$libfranka_ver" ]]; then + if [[ -f "${WORKSPACE}/config/libfranka_ver.csv" ]]; then + libfranka_ver=$(cat "${WORKSPACE}/config/libfranka_ver.csv" | head -1 | tr -d '\r\n') + else + log_error "libfranka version not found" + return 1 + fi + fi + local libfranka_major_minor=$(echo "$libfranka_ver" | cut -d. -f1,2) + local libfranka_so="libfranka.so.${libfranka_major_minor}" + + if [[ "$ARCH" == "arm64" ]]; then + # For ARM64, intelligently bundle dependencies like linuxdeploy does + log_info "Analyzing ARM64 dependencies with readelf (like linuxdeploy)..." + + local dest_lib="${libfranka_build}/usr/lib" + mkdir -p "${dest_lib}" + + # Copy libfranka itself (copy real content to the SONAME, no symlinks) + local libfranka_path="${libfranka_build}/${libfranka_so}" + if [[ -f "$libfranka_path" ]]; then + local real_libfranka=$(readlink -f "$libfranka_path") + # Copy content to destination with the versioned name (e.g. libfranka.so.0.16) + cp "$real_libfranka" "${dest_lib}/${libfranka_so}" 2>/dev/null || true + fi + + # Smart dependency bundling (analyze the real file) + local real_libfranka=$(readlink -f "$libfranka_path") + bundle_arm64_deps_smart "$real_libfranka" "${dest_lib}" + + # Post-processing: + # 1. Create usr/bin and copy libfranka.so there + local dest_bin="${libfranka_build}/usr/bin" + mkdir -p "${dest_bin}" + cp "$real_libfranka" "${dest_bin}/libfranka.so" 2>/dev/null || true + + # 2. Set RPATHs using patchelf + # For libfranka.so in bin: needs to look in ../lib + if [[ -f "${dest_bin}/libfranka.so" ]]; then + patchelf --set-rpath '$ORIGIN/../lib' "${dest_bin}/libfranka.so" 2>/dev/null || true + fi + + # For libraries in lib: need to look in . (to find each other) + for lib in "${dest_lib}"/*.so*; do + patchelf --set-rpath '$ORIGIN' "$lib" 2>/dev/null || true + done + + log_success "Dependencies bundled and RPATHs set." + + log_success "Dependencies bundled intelligently ($(ls -1 ${dest_lib} | wc -l) libraries)." + else + log_info "Running linuxdeploy to bundle dependencies..." + + local linuxdeploy="/opt/tools/linuxdeploy/AppRun" + + if [[ -f "$linuxdeploy" ]]; then + "$linuxdeploy" \ + --appdir="${libfranka_build}" \ + --executable="${libfranka_build}/libfranka.so" \ + --library="${libfranka_build}/${libfranka_so}" \ + || log_warn "linuxdeploy completed with warnings" + else + log_warn "linuxdeploy not found, skipping dependency bundling" + fi + fi +} + + diff --git a/docker/scripts/entrypoint.sh b/docker/scripts/entrypoint.sh new file mode 100755 index 0000000..f7c4742 --- /dev/null +++ b/docker/scripts/entrypoint.sh @@ -0,0 +1,171 @@ +#!/bin/bash +# Entrypoint script for Franka Toolbox Docker build container +# This script orchestrates the entire build process + +set -e + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +BLUE='\033[0;34m' +NC='\033[0m' # No Color + +# Configure git to treat the workspace as safe (avoids dubious ownership errors) +git config --global --add safe.directory '*' + +log_info() { + echo -e "${BLUE}[INFO]${NC} $1" +} + +log_success() { + echo -e "${GREEN}[SUCCESS]${NC} $1" +} + +log_warn() { + echo -e "${YELLOW}[WARNING]${NC} $1" +} + +log_error() { + echo -e "${RED}[ERROR]${NC} $1" +} + +# Default values +ARCH="amd64" +BUILD_TYPE="Release" +LIBFRANKA_VERSION="" +OUTPUT_DIR="/output" +WORKSPACE="/workspace" + +# Parse arguments +while [[ $# -gt 0 ]]; do + case $1 in + --arch) + ARCH="$2" + shift 2 + ;; + --build-type) + BUILD_TYPE="$2" + shift 2 + ;; + --libfranka-version) + LIBFRANKA_VERSION="$2" + shift 2 + ;; + --output) + OUTPUT_DIR="$2" + shift 2 + ;; + all) + BUILD_TARGET="all" + shift + ;; + libfranka) + BUILD_TARGET="libfranka" + shift + ;; + common) + BUILD_TARGET="common" + shift + ;; + server) + BUILD_TARGET="server" + shift + ;; + help|--help|-h) + echo "Usage: $0 [target] [options]" + echo "" + echo "Targets:" + echo " all Build everything (default)" + echo " libfranka Build only libfranka" + echo " common Build only common library" + echo " server Build only franka_robot_server" + echo "" + echo "Options:" + echo " --arch Target architecture (amd64 or arm64, default: amd64)" + echo " --build-type Build type (Release or Debug, default: Release)" + echo " --libfranka-version Libfranka version (default: from config/libfranka_ver.csv)" + echo " --output Output directory (default: /output)" + exit 0 + ;; + *) + log_error "Unknown argument: $1" + exit 1 + ;; + esac +done + +# Set default build target +BUILD_TARGET=${BUILD_TARGET:-all} + +# Read libfranka version from config if not provided +if [[ -z "$LIBFRANKA_VERSION" ]]; then + if [[ -f "$WORKSPACE/config/libfranka_ver.csv" ]]; then + LIBFRANKA_VERSION=$(cat "$WORKSPACE/config/libfranka_ver.csv" | head -1 | tr -d '\r\n') + else + log_error "libfranka version not specified and config/libfranka_ver.csv not found" + exit 1 + fi +fi + +# Set architecture-specific variables +if [[ "$ARCH" == "arm64" ]]; then + ARCH_SUFFIX="_arm" + BIN_SUFFIX="_arm" + FRANKA_FOLDER="libfranka_arm" + BIN_FOLDER="bin_arm" + export CMAKE_TOOLCHAIN_FILE="/scripts/toolchain-aarch64.cmake" +else + ARCH_SUFFIX="" + BIN_SUFFIX="" + FRANKA_FOLDER="libfranka" + BIN_FOLDER="bin" + unset CMAKE_TOOLCHAIN_FILE +fi + +# Export variables for sub-scripts +export ARCH +export ARCH_SUFFIX +export BIN_SUFFIX +export FRANKA_FOLDER +export BIN_FOLDER +export BUILD_TYPE +export LIBFRANKA_VERSION +export OUTPUT_DIR +export WORKSPACE + +log_info "============================================" +log_info "Franka Toolbox Build System" +log_info "============================================" +log_info "Target: $BUILD_TARGET" +log_info "Architecture: $ARCH" +log_info "Build Type: $BUILD_TYPE" +log_info "Libfranka Version: $LIBFRANKA_VERSION" +log_info "Output Directory: $OUTPUT_DIR" +log_info "============================================" + +# Create output directory +mkdir -p "$OUTPUT_DIR" + +# Run the appropriate build script(s) +case $BUILD_TARGET in + all) + /scripts/build_libfranka.sh + /scripts/build_common.sh + /scripts/build_server.sh + /scripts/package.sh + ;; + libfranka) + /scripts/build_libfranka.sh + ;; + common) + /scripts/build_common.sh + ;; + server) + /scripts/build_server.sh + ;; +esac + +log_success "Build completed successfully!" + + diff --git a/docker/scripts/package.sh b/docker/scripts/package.sh new file mode 100755 index 0000000..6c6a288 --- /dev/null +++ b/docker/scripts/package.sh @@ -0,0 +1,90 @@ +#!/bin/bash +# Package build artifacts into distributable archives +# Creates bin.zip/bin_arm.zip for common and bin.tar.gz/bin_arm.tar.gz for server + +set -e + +source /scripts/common.sh + +log_info "Packaging build artifacts for ${ARCH}..." + +COMMON_BIN_PATH="${WORKSPACE}/common/${BIN_FOLDER}" +SERVER_BIN_PATH="${WORKSPACE}/franka_robot_server/${BIN_FOLDER}" + +# Verify build artifacts exist +if [[ ! -f "$COMMON_BIN_PATH/libfranka_matlab.a" ]]; then + log_error "Common library not found at ${COMMON_BIN_PATH}/libfranka_matlab.a" + exit 1 +fi + +if [[ ! -f "$SERVER_BIN_PATH/franka_robot_server" ]]; then + log_error "Server executable not found at ${SERVER_BIN_PATH}/franka_robot_server" + exit 1 +fi + +# Create temp directory for packaging (avoids permission issues with mounted volumes) +TEMP_PKG_DIR="/tmp/franka_package" +rm -rf "$TEMP_PKG_DIR" +mkdir -p "$TEMP_PKG_DIR" + +# Package common library (bin.zip or bin_arm.zip) +log_info "Creating common library archive..." +COMMON_ARCHIVE="bin${BIN_SUFFIX}.zip" +cd "${WORKSPACE}/common" +zip -r -y "${TEMP_PKG_DIR}/${COMMON_ARCHIVE}" "${BIN_FOLDER}" +log_info "Created: ${COMMON_ARCHIVE}" + +# Copy to output directory +cp "${TEMP_PKG_DIR}/${COMMON_ARCHIVE}" "${OUTPUT_DIR}/" + +# Package server executable (bin.tar.gz or bin_arm.tar.gz) +log_info "Creating server archive..." +SERVER_ARCHIVE="bin${BIN_SUFFIX}.tar.gz" +cd "${WORKSPACE}/franka_robot_server" +tar -czvf "${TEMP_PKG_DIR}/${SERVER_ARCHIVE}" "${BIN_FOLDER}" +log_info "Created: ${SERVER_ARCHIVE}" + +# Copy to output directory +cp "${TEMP_PKG_DIR}/${SERVER_ARCHIVE}" "${OUTPUT_DIR}/" + +# Also package libfranka dependencies if they exist +LIBFRANKA_PATH="${WORKSPACE}/${FRANKA_FOLDER}" +if [[ -d "$LIBFRANKA_PATH/build/usr" ]] || [[ -f "$LIBFRANKA_PATH/build/libfranka.so" ]]; then + log_info "Creating libfranka dependencies archive..." + + # Use temp directory to avoid permission issues + LIBFRANKA_TEMP="${TEMP_PKG_DIR}/${FRANKA_FOLDER}" + mkdir -p "${LIBFRANKA_TEMP}/build" + + # Copy necessary libfranka components + cp -r "${LIBFRANKA_PATH}/build/usr" "${LIBFRANKA_TEMP}/build/" 2>/dev/null || true + cp -r "${LIBFRANKA_PATH}/include" "${LIBFRANKA_TEMP}/" 2>/dev/null || true + cp -r "${LIBFRANKA_PATH}/common" "${LIBFRANKA_TEMP}/" 2>/dev/null || true + + # Copy libfranka.so files + cp "${LIBFRANKA_PATH}/build/libfranka.so"* "${LIBFRANKA_TEMP}/build/" 2>/dev/null || true + + LIBFRANKA_ARCHIVE="${FRANKA_FOLDER}.zip" + cd "${TEMP_PKG_DIR}" + zip -r -y "${LIBFRANKA_ARCHIVE}" "${FRANKA_FOLDER}" + + # Copy to output directory + cp "${LIBFRANKA_ARCHIVE}" "${OUTPUT_DIR}/" + + log_info "Created: ${LIBFRANKA_ARCHIVE}" +fi + +# Clean up build directories +log_info "Cleaning up build directories..." +rm -rf "${COMMON_BIN_PATH}" +rm -rf "${SERVER_BIN_PATH}" +rm -rf "${WORKSPACE}/common/build" +rm -rf "${WORKSPACE}/franka_robot_server/build" +rm -rf "$TEMP_PKG_DIR" + +log_success "Packaging completed!" +log_info "" +log_info "Output files in ${OUTPUT_DIR}:" +ls -la "${OUTPUT_DIR}/" + + diff --git a/docker/scripts/toolchain-aarch64.cmake b/docker/scripts/toolchain-aarch64.cmake new file mode 100644 index 0000000..9355877 --- /dev/null +++ b/docker/scripts/toolchain-aarch64.cmake @@ -0,0 +1,102 @@ +# CMake toolchain file for cross-compiling to aarch64 (ARM64) +# Used for Jetson Orin and other ARM64 Linux targets + +set(CMAKE_SYSTEM_NAME Linux) +set(CMAKE_SYSTEM_PROCESSOR aarch64) + +# Specify the cross compiler +set(CMAKE_C_COMPILER aarch64-linux-gnu-gcc) +set(CMAKE_CXX_COMPILER aarch64-linux-gnu-g++) +set(CMAKE_AR aarch64-linux-gnu-ar) +set(CMAKE_RANLIB aarch64-linux-gnu-ranlib) +set(CMAKE_STRIP aarch64-linux-gnu-strip) + +# Search paths configuration +# 1. /opt/sysroot-aarch64: Custom builds (Pinocchio, hpp-fcl, CapnProto) +# 2. /usr/lib/aarch64-linux-gnu: Apt-installed arm64 libraries +# 3. /usr/aarch64-linux-gnu: Cross-compiler sysroot +set(CMAKE_FIND_ROOT_PATH + /opt/sysroot-aarch64 + /opt/sysroot-aarch64/usr + /usr/lib/aarch64-linux-gnu + /usr/aarch64-linux-gnu +) + +# Search modes for cross-compilation +# NEVER for programs (use host tools like capnp compiler) +# ONLY for libraries (use target sysroot only) +# BOTH for includes/packages to allow finding headers in /usr/include and configs in /usr/share +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE BOTH) +set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE BOTH) + +# Additional include/library paths +set(CMAKE_PREFIX_PATH + /opt/sysroot-aarch64/usr + /usr/lib/aarch64-linux-gnu + /usr/share +) + +# Custom paths for things we built manually +set(CapnProto_DIR /opt/sysroot-aarch64/usr/lib/cmake/CapnProto) +set(pinocchio_DIR /opt/sysroot-aarch64/usr/lib/cmake/pinocchio) +set(hpp-fcl_DIR /opt/sysroot-aarch64/usr/lib/cmake/hpp-fcl) + +# Eigen3 from apt multiarch install (libeigen3-dev:arm64) +# This prevents CMake from finding the host's Eigen3 and causing recursion +# Use CACHE variables with FORCE to ensure they survive find_package() calls +# libfranka's FindEigen3.cmake requires EIGEN3_INCLUDE_DIRS to be set +set(Eigen3_DIR "/usr/lib/aarch64-linux-gnu/cmake/eigen3" CACHE PATH "Eigen3 config dir" FORCE) +set(EIGEN3_INCLUDE_DIR "/usr/include/eigen3" CACHE PATH "Eigen3 include dir" FORCE) +set(EIGEN3_INCLUDE_DIRS "/usr/include/eigen3" CACHE PATH "Eigen3 include dirs" FORCE) +set(Eigen3_FOUND TRUE CACHE BOOL "Eigen3 found" FORCE) + +# Poco from apt multiarch install (libpoco-dev:arm64) +set(Poco_INCLUDE_DIR "/usr/include" CACHE PATH "Poco include dir" FORCE) +set(Poco_Net_LIBRARY "/usr/lib/aarch64-linux-gnu/libPocoNet.so" CACHE FILEPATH "Poco Net library" FORCE) +set(Poco_Foundation_LIBRARY "/usr/lib/aarch64-linux-gnu/libPocoFoundation.so" CACHE FILEPATH "Poco Foundation library" FORCE) +set(Poco_LIBRARIES "/usr/lib/aarch64-linux-gnu/libPocoNet.so;/usr/lib/aarch64-linux-gnu/libPocoFoundation.so" CACHE STRING "Poco libraries" FORCE) +set(Poco_FOUND TRUE CACHE BOOL "Poco found" FORCE) + +# Boost from apt multiarch install (libboost-*-dev:arm64) +set(BOOST_ROOT "/usr" CACHE PATH "Boost root" FORCE) +set(BOOST_INCLUDEDIR "/usr/include" CACHE PATH "Boost include dir" FORCE) +set(BOOST_LIBRARYDIR "/usr/lib/aarch64-linux-gnu" CACHE PATH "Boost library dir" FORCE) +set(Boost_LIBRARY_DIR "/usr/lib/aarch64-linux-gnu" CACHE PATH "Boost library dir" FORCE) +set(Boost_NO_SYSTEM_PATHS OFF CACHE BOOL "Boost no system paths" FORCE) +set(Boost_INCLUDE_DIR "/usr/include" CACHE PATH "Boost include dir" FORCE) +set(Boost_FILESYSTEM_LIBRARY "/usr/lib/aarch64-linux-gnu/libboost_filesystem.so" CACHE FILEPATH "Boost filesystem" FORCE) +set(Boost_SERIALIZATION_LIBRARY "/usr/lib/aarch64-linux-gnu/libboost_serialization.so" CACHE FILEPATH "Boost serialization" FORCE) +set(Boost_SYSTEM_LIBRARY "/usr/lib/aarch64-linux-gnu/libboost_system.so" CACHE FILEPATH "Boost system" FORCE) + +# urdfdom from apt multiarch install (liburdfdom-dev:arm64, liburdfdom-headers-dev:arm64) +# Ubuntu 22.04 puts urdfdom_headers config in /usr/share/urdfdom_headers/cmake +set(urdfdom_headers_DIR "/usr/share/urdfdom_headers/cmake" CACHE PATH "urdfdom_headers config" FORCE) +set(urdfdom_DIR "/usr/lib/aarch64-linux-gnu/urdfdom/cmake" CACHE PATH "urdfdom config" FORCE) + +# console_bridge from apt multiarch install +set(console_bridge_DIR "/usr/lib/aarch64-linux-gnu/console_bridge/cmake" CACHE PATH "console_bridge config" FORCE) + +# ZLIB from apt multiarch install (zlib1g-dev:arm64) +set(ZLIB_LIBRARY "/usr/lib/aarch64-linux-gnu/libz.so" CACHE FILEPATH "ZLIB library" FORCE) +set(ZLIB_INCLUDE_DIR "/usr/include" CACHE PATH "ZLIB include" FORCE) +set(ZLIB_FOUND TRUE CACHE BOOL "ZLIB found" FORCE) + +# PCRE from apt multiarch install (libpcre3-dev:arm64) +set(PCRE_LIBRARY "/usr/lib/aarch64-linux-gnu/libpcre.so" CACHE FILEPATH "PCRE library" FORCE) +set(PCRE_INCLUDE_DIR "/usr/include" CACHE PATH "PCRE include" FORCE) +set(PCRE_FOUND TRUE CACHE BOOL "PCRE found" FORCE) + +# TinyXML2 from apt multiarch install (libtinyxml2-dev:arm64) +set(TinyXML2_INCLUDE_DIR "/usr/include" CACHE PATH "TinyXML2 include dir" FORCE) +set(TinyXML2_LIBRARY "/usr/lib/aarch64-linux-gnu/libtinyxml2.so" CACHE FILEPATH "TinyXML2 library" FORCE) +set(TinyXML2_FOUND TRUE CACHE BOOL "TinyXML2 found" FORCE) + +# Position independent code +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Linker flags +# We need -rpath-link so the linker can find transitive dependencies (like libpinocchio needed by libfranka) +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -L/opt/sysroot-aarch64/usr/lib -L/usr/lib/aarch64-linux-gnu -Wl,-rpath-link,/opt/sysroot-aarch64/usr/lib -Wl,-rpath-link,/usr/lib/aarch64-linux-gnu") +set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -L/opt/sysroot-aarch64/usr/lib -L/usr/lib/aarch64-linux-gnu -Wl,-rpath-link,/opt/sysroot-aarch64/usr/lib -Wl,-rpath-link,/usr/lib/aarch64-linux-gnu") diff --git a/docs/franka_matlab/compatibility.rst b/docs/franka_matlab/compatibility.rst index f5d353d..751f7b3 100644 --- a/docs/franka_matlab/compatibility.rst +++ b/docs/franka_matlab/compatibility.rst @@ -9,7 +9,7 @@ Compatibility with Matlab & libfranka +------------------------+---------------------+----------------------------+ | Franka MATLAB version | libfranka version | MATLAB Version | +========================+=====================+============================+ -| 3.1.0 | :math:`\geq` 0.9.x | :math:`\geq` R2023a | +| 4.0.0 | :math:`\geq` 0.9.x | :math:`\geq` R2024b | +------------------------+---------------------+----------------------------+ .. important:: diff --git a/docs/franka_matlab/custom_build.rst b/docs/franka_matlab/custom_build.rst new file mode 100644 index 0000000..671c97b --- /dev/null +++ b/docs/franka_matlab/custom_build.rst @@ -0,0 +1,120 @@ +Custom Build +============ + +This guide covers building the Franka Toolbox from source. For most users, we recommend downloading the pre-built toolbox from the `GitHub Releases `_ page. + +Overview +-------- + +The toolbox consists of two parts: + +1. **Server binaries** — Run on the target Linux PC connected to the robot +2. **MEX files** — Run on your MATLAB host (Windows or Linux) + +Building Server Binaries (Docker) +--------------------------------- + +Server binaries are built using Docker, which handles all dependencies automatically. +If you need a specific ``libfranka`` version instead of the one used for the pre-built release packages, build with ``./build.sh --libfranka `` from the ``docker/`` directory. +You can also build only ``amd64`` for a local host target or only ``arm64`` when targeting Jetson, instead of building both architectures. + +**Requirements:** Docker installed and running + +.. code-block:: bash + + cd docker + ./build.sh # Build for both x86_64 and ARM64 + # Or: + ./build.sh amd64 # x86_64 only + ./build.sh arm64 # ARM64 only (cross-compiled) + +**Output files:** + +- ``common/bin.zip``, ``common/bin_arm.zip`` — Common library +- ``franka_robot_server/bin.tar.gz``, ``franka_robot_server/bin_arm.tar.gz`` — Server executable +- ``dependencies/libfranka.zip``, ``dependencies/libfranka_arm.zip`` — libfranka with dependencies + +For implementation details, see the ``Dockerfile.amd64`` and ``Dockerfile.arm64`` files in the ``docker/`` directory. + +Building MEX Files +------------------ + +MEX files must be built on each platform where you want to use the toolbox. + +Linux +^^^^^ + +**Install dependencies:** + +.. code-block:: bash + + sudo apt-get update + sudo apt-get install -y cmake build-essential libeigen3-dev + + # Build Cap'n Proto (static libraries with PIC) + CAPNP_VERSION="1.0.2" + curl -O https://capnproto.org/capnproto-c++-${CAPNP_VERSION}.tar.gz + tar zxf capnproto-c++-${CAPNP_VERSION}.tar.gz + cd capnproto-c++-${CAPNP_VERSION} + mkdir build && cd build + cmake -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON .. + make -j$(nproc) + sudo make install + sudo ldconfig + +**Build MEX files (in MATLAB):** + +.. code-block:: matlab + + franka_simulink_library_mex(); + franka_robot_mex(); + +Windows +^^^^^^^ + +**Requirements:** + +- Visual Studio 2022 +- CMake 3.15+ + +**Install Cap'n Proto:** + +Build Cap'n Proto from source as static libraries. See the CI workflow in ``.github/workflows/build-and-release.yml`` for the exact steps. + +**Build MEX files (in MATLAB):** + +.. code-block:: matlab + + franka_simulink_library_mex(); + franka_robot_mex(); + +Creating the Distribution Package +--------------------------------- + +After building both server binaries and MEX files: + +.. code-block:: matlab + + franka_toolbox_dist_make(); + +By default, ``franka_toolbox_dist_make()`` now uses the CI-safe packaging behavior and preserves the generated server and MEX archives in your working tree. +This creates ``dist/franka.mltbx`` by default. You can also specify the output name: + +.. code-block:: matlab + + franka_toolbox_dist_make('OutputName', 'franka-fr3'); % For FR3 + franka_toolbox_dist_make('OutputName', 'franka-fer'); % For FER + +If you explicitly want the old local cleanup behavior, you can run: + +.. code-block:: matlab + + franka_toolbox_dist_make('Mode', 'local'); + +``Mode='local'`` runs ``git clean -ffxd`` before packaging. This removes untracked generated artifacts from the repository checkout, including locally built archives that have not been committed. + +CI/CD Reference +--------------- + +For a complete automated build example, see ``.github/workflows/build-and-release.yml`` which builds for all platforms and creates release packages. + diff --git a/docs/franka_matlab/index.rst b/docs/franka_matlab/index.rst index 602610b..b177650 100644 --- a/docs/franka_matlab/index.rst +++ b/docs/franka_matlab/index.rst @@ -38,3 +38,4 @@ The toolbox comprises two main components: simulink_library matlab_library troubleshooting + custom_build diff --git a/docs/franka_matlab/installation.rst b/docs/franka_matlab/installation.rst index cf650bb..3619198 100644 --- a/docs/franka_matlab/installation.rst +++ b/docs/franka_matlab/installation.rst @@ -4,15 +4,15 @@ Installation Toolbox Add-On Installation Methods ----------------------------------- -Option 1: Drag and drop the franka.mltbx file -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Drag and drop the ``franka.mltbx`` file into your MATLAB Command Window or +Option 1: Drag and drop the .mltbx file +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Drag and drop the ``franka-fr3.mltbx`` (or ``franka-fer.mltbx``) file into your MATLAB Command Window or Option 2: Programmatically ^^^^^^^^^^^^^^^^^^^^^^^^^^ .. code-block:: matlab - uiopen('', 1); + uiopen('', 1); Install --------- diff --git a/docs/franka_matlab/matlab_library.rst b/docs/franka_matlab/matlab_library.rst index 4381b50..83d4219 100644 --- a/docs/franka_matlab/matlab_library.rst +++ b/docs/franka_matlab/matlab_library.rst @@ -8,12 +8,23 @@ FrankaRobot Class The ``FrankaRobot`` constructor initializes a connection to the Franka robot. It can be configured for two primary scenarios: connecting to a robot on a local network (Host PC) or connecting to a robot via an external AI companion computer like a Jetson. +Multiple ``FrankaRobot`` instances can be created simultaneously, each managing its own server lifecycle independently. + **Local Host PC as Target PC** .. code-block:: matlab + fr = FrankaRobot(); % Uses default robot IP (172.16.0.2) + + % Or with custom robot IP fr = FrankaRobot('RobotIP', '172.16.0.2'); + % Or with a full settings object for advanced configuration + settings = FrankaRobotSettings(); + settings.robot_ip = '172.16.0.2'; + settings.home_configuration = [0, -pi/4, 0, -3*pi/4, 0, pi/2, pi/4]; + fr = FrankaRobot('Settings', settings); + **Connecting via AI Companion/NVIDIA Jetson** When using an external Target PC to control the robot, you must provide connection details for that computer, including its IP address and a username. @@ -30,10 +41,21 @@ When using an external Target PC to control the robot, you must provide connecti 'Username', 'jetson_user', ... 'ServerIP', '192.168.1.100'); + % Or with custom settings object + settings = FrankaRobotSettings(); + settings.home_configuration = [0, -pi/4, 0, -3*pi/4, 0, pi/2, pi/4]; + fr = FrankaRobot('RobotIP', '172.16.0.2', ... + 'Settings', settings, ... + 'Username', 'jetson_user', ... + 'ServerIP', '192.168.1.100'); + All constructor parameters are optional and have default values. Parameters: - - RobotIP: IP address of the Franka robot (default: '172.16.0.2') + - RobotIP: IP address of the Franka robot (default: '172.16.0.2'). + Overrides ``Settings.robot_ip`` if both are provided. + - Settings: ``FrankaRobotSettings`` object containing robot configuration (optional). + Other settings like ``collision_thresholds`` and ``load_inertia`` can be modified at runtime. - Username: Username for the server on the AI companion (default: 'franka') - ServerIP: IP address of the server on the AI companion (default: '172.16.1.2') - SSHPort: SSH port for server connection (default: '22') @@ -106,7 +128,7 @@ Collision Thresholds Sets or gets the collision thresholds for the robot. Parameters: - - thresholds: Struct containing collision threshold parameters + - thresholds: ``FrankaRobotCollisionThresholds`` object Load Inertia ^^^^^^^^^^^^ @@ -119,7 +141,7 @@ Load Inertia Sets or gets the load inertia parameters for the robot. Parameters: - - loadInertia: Struct containing mass, center of mass, and inertia matrix + - loadInertia: ``FrankaRobotLoadInertia`` object (mass, center_of_mass, inertia_matrix) Robot Homing ^^^^^^^^^^^^ @@ -140,7 +162,114 @@ Reset Settings fr.resetSettings(); -Resets all robot settings to their default values. +Resets all robot settings to their default values and applies them to the robot. + +Joint Impedance +^^^^^^^^^^^^^^^ + +.. code-block:: matlab + + fr.setJointImpedance(K_theta); + fr.setJointImpedance(); % Uses Settings.joint_impedance_stiffness + K_theta = fr.getJointImpedance(); + +Sets or gets the impedance for each joint in the internal controller. +The value is stored in ``Settings.joint_impedance_stiffness``. + +Parameters: + - K_theta: 7-element array of joint stiffness values [Nm/rad] + +Default values: ``[3000, 3000, 3000, 2500, 2500, 2000, 2000]`` + +Cartesian Impedance +^^^^^^^^^^^^^^^^^^^ + +.. code-block:: matlab + + fr.setCartesianImpedance(K_x); + fr.setCartesianImpedance(); % Uses Settings.cartesian_impedance_stiffness + K_x = fr.getCartesianImpedance(); + +Sets or gets the Cartesian stiffness/compliance in the internal controller. +The value is stored in ``Settings.cartesian_impedance_stiffness``. + +Parameters: + - K_x: 6-element array for (x, y, z, roll, pitch, yaw) stiffness + [N/m, N/m, N/m, Nm/rad, Nm/rad, Nm/rad] + +Default values: ``[3000, 3000, 3000, 300, 300, 300]`` + +Guiding Mode +^^^^^^^^^^^^ + +.. code-block:: matlab + + fr.setGuidingMode(guiding_mode, elbow); + +Locks or unlocks guiding mode movement in (x, y, z, roll, pitch, yaw) directions. + +Parameters: + - guiding_mode: 6-element logical array where ``true`` = unlocked, ``false`` = locked + - elbow: logical scalar, ``true`` = unlock elbow movement + +Returns: + - true if successful, false otherwise + +Example: + +.. code-block:: matlab + + % Unlock all directions and elbow + fr.setGuidingMode([true true true true true true], true); + + % Lock rotation, allow translation and elbow + fr.setGuidingMode([true true true false false false], true); + +End Effector Frame (NE_T_EE) +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: matlab + + fr.setEE(NE_T_EE); + fr.setEE(); % Uses Settings.NE_T_EE + NE_T_EE = fr.getEE(); + +Sets or gets the transformation from nominal end effector to end effector frame. +The value is stored in ``Settings.NE_T_EE``. + +Parameters: + - NE_T_EE: 4x4 homogeneous transformation matrix + +Default: Identity matrix (eye(4)) + +Stiffness Frame (EE_T_K) +^^^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: matlab + + fr.setK(EE_T_K); + fr.setK(); % Uses Settings.EE_T_K + EE_T_K = fr.getK(); + +Sets or gets the transformation from end effector frame to stiffness frame. +The value is stored in ``Settings.EE_T_K``. + +Parameters: + - EE_T_K: 4x4 homogeneous transformation matrix + +Default: Identity matrix (eye(4)) + +Stop Robot +^^^^^^^^^^ + +.. code-block:: matlab + + result = fr.stop(); + +Stops all currently running motions on the robot. + +Returns: + - true if successful, false otherwise Gripper Control ^^^^^^^^^^^^^^^ diff --git a/docs/franka_matlab/system_requirements.rst b/docs/franka_matlab/system_requirements.rst index f0a9414..3e00fca 100644 --- a/docs/franka_matlab/system_requirements.rst +++ b/docs/franka_matlab/system_requirements.rst @@ -49,7 +49,7 @@ given that the Target PC is running a supported version of Ubuntu as defined bel +-------------------------+---------------------------------------------+----------------------------------------------+ | Franka Toolbox Version | AI Companion/Jetson Orin Platform | Real-Time kernel Linux Host PC as Target PC | +=========================+=============================================+==============================================+ -| 3.0.0 | Ubuntu 22.04 LTS | Ubuntu 22.04 LTS | +| 4.0.0 | Ubuntu 22.04 LTS | :math:`\geq` Ubuntu 22.04 LTS | +-------------------------+---------------------------------------------+----------------------------------------------+ .. warning:: diff --git a/examples/cartesian_impedance_control.slx b/examples/cartesian_impedance_control.slx index 47784db..13a4a80 100644 Binary files a/examples/cartesian_impedance_control.slx and b/examples/cartesian_impedance_control.slx differ diff --git a/examples/force_control.slx b/examples/force_control.slx index f271fff..b79f244 100644 Binary files a/examples/force_control.slx and b/examples/force_control.slx differ diff --git a/examples/generate_cartesian_pose_motion.slx b/examples/generate_cartesian_pose_motion.slx index b1d734c..ea7140d 100644 Binary files a/examples/generate_cartesian_pose_motion.slx and b/examples/generate_cartesian_pose_motion.slx differ diff --git a/examples/generate_cartesian_velocity_motion.slx b/examples/generate_cartesian_velocity_motion.slx index 2ba6b97..58b35e6 100644 Binary files a/examples/generate_cartesian_velocity_motion.slx and b/examples/generate_cartesian_velocity_motion.slx differ diff --git a/examples/generate_elbow_motion.slx b/examples/generate_elbow_motion.slx index 8c44b5f..8d817f7 100644 Binary files a/examples/generate_elbow_motion.slx and b/examples/generate_elbow_motion.slx differ diff --git a/examples/generate_joint_position_motion.slx b/examples/generate_joint_position_motion.slx index 459306f..5f29a4e 100644 Binary files a/examples/generate_joint_position_motion.slx and b/examples/generate_joint_position_motion.slx differ diff --git a/examples/generate_joint_velocity_motion.slx b/examples/generate_joint_velocity_motion.slx index a23901b..bf21d39 100644 Binary files a/examples/generate_joint_velocity_motion.slx and b/examples/generate_joint_velocity_motion.slx differ diff --git a/examples/grasp_object.slx b/examples/grasp_object.slx index 40e27e8..995d94b 100644 Binary files a/examples/grasp_object.slx and b/examples/grasp_object.slx differ diff --git a/examples/joint_impedance_control.slx b/examples/joint_impedance_control.slx index 0aebcb8..fb5e8dd 100644 Binary files a/examples/joint_impedance_control.slx and b/examples/joint_impedance_control.slx differ diff --git a/examples/motion_with_control.slx b/examples/motion_with_control.slx index 193c8f3..402b581 100644 Binary files a/examples/motion_with_control.slx and b/examples/motion_with_control.slx differ diff --git a/examples/pick_and_place_with_RRT.mlx b/examples/pick_and_place_with_RRT.mlx index 7858233..0289aae 100644 Binary files a/examples/pick_and_place_with_RRT.mlx and b/examples/pick_and_place_with_RRT.mlx differ diff --git a/examples/vacuum_object.slx b/examples/vacuum_object.slx index a954d39..4f145f7 100644 Binary files a/examples/vacuum_object.slx and b/examples/vacuum_object.slx differ diff --git a/franka_robot/FrankaRobot.m b/franka_robot/FrankaRobot.m index 59cf480..0604f6f 100644 --- a/franka_robot/FrankaRobot.m +++ b/franka_robot/FrankaRobot.m @@ -1,42 +1,18 @@ classdef FrankaRobot < handle + %FRANKAROBOT High-level interface to Franka Emika robots + % + % Example (single robot): + % robot = FrankaRobot('RobotIP', '172.16.0.2'); + % + % Example (multiple robots): + % robot1 = FrankaRobot('RobotIP', '172.16.0.2', 'ServerPort', '5001'); + % robot2 = FrankaRobot('RobotIP', '172.16.0.3', 'ServerPort', '5002'); properties (Constant, Access = private) - % Set standard default values for the Remote Server DefaultServerUsername = 'franka'; DefaultServerIP = '172.16.1.2'; DefaultSSHPort = '22'; DefaultServerPort = '5001'; - - % Set standard default values for the Robot Settings - DefaultTorqueThresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]; - - DefaultForceThresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]; - - DefaultCollisionThresholds = struct(... - 'lower_torque_thresholds_acceleration', FrankaRobot.DefaultTorqueThresholds, ... - 'upper_torque_thresholds_acceleration', FrankaRobot.DefaultTorqueThresholds, ... - 'lower_torque_thresholds_nominal', FrankaRobot.DefaultTorqueThresholds, ... - 'upper_torque_thresholds_nominal', FrankaRobot.DefaultTorqueThresholds, ... - 'lower_force_thresholds_acceleration', FrankaRobot.DefaultForceThresholds, ... - 'upper_force_thresholds_acceleration', FrankaRobot.DefaultForceThresholds, ... - 'lower_force_thresholds_nominal', FrankaRobot.DefaultForceThresholds, ... - 'upper_force_thresholds_nominal', FrankaRobot.DefaultForceThresholds ... - ) - - DefaultLoadInertia = struct(... - 'mass', 0, ... - 'center_of_mass', [0,0,0], ... - 'load_inertia', [0.001,0,0;0,0.001,0;0,0,0.001] ... - ) - % Robot IP Address - DefaultRobotIP = '172.16.0.2'; - - % Default Settings - DefaultSettings = struct(... - 'homeConfiguration', [0, -pi/4, 0, -3 * pi/4, 0, pi/2, pi/4], ... - 'collisionThresholds', FrankaRobot.DefaultCollisionThresholds, ... - 'loadInertia', FrankaRobot.DefaultLoadInertia ... - ) end properties @@ -46,79 +22,76 @@ VacuumGripper end - properties (SetAccess = private, Hidden = true) + properties (SetAccess = private) RobotIP + end + + properties (SetAccess = private, Hidden = true) frankaRobotHandle end methods - %% Constructor function obj = FrankaRobot(varargin) - % Initialize Settings with DefaultSettings - obj.Settings = obj.DefaultSettings; + % FrankaRobot Constructor + % + % Parameters: + % 'RobotIP' - IP address of the Franka robot + % 'Settings' - FrankaRobotSettings object + % 'ServerPort' - RPC server port (use different ports for multiple robots) + % 'ServerIP' - Remote server IP (triggers SSH mode) + % 'Username' - SSH username (default: 'franka') + % 'SSHPort' - SSH port (default: '22') - % Create input parser p = inputParser; - - % Define parameters with default values - addParameter(p, 'RobotIP', FrankaRobot.DefaultRobotIP, @ischar); - - addParameter(p, 'Username', obj.DefaultServerUsername, @ischar); - addParameter(p, 'ServerIP', obj.DefaultServerIP, @ischar); - addParameter(p, 'SSHPort', obj.DefaultSSHPort, @ischar); - addParameter(p, 'ServerPort', obj.DefaultServerPort, @ischar); - - % Parse the inputs + addParameter(p, 'RobotIP', '', @(x) ischar(x) || isstring(x)); + addParameter(p, 'Settings', FrankaRobotSettings(), @(x) isa(x, 'FrankaRobotSettings')); + addParameter(p, 'Username', '', @(x) ischar(x) || isstring(x)); + addParameter(p, 'ServerIP', '', @(x) ischar(x) || isstring(x)); + addParameter(p, 'SSHPort', obj.DefaultSSHPort, @(x) ischar(x) || isstring(x)); + addParameter(p, 'ServerPort', obj.DefaultServerPort, @(x) ischar(x) || isstring(x)); parse(p, varargin{:}); - - % Get the results params = p.Results; - % Store the RobotIP in Settings - obj.RobotIP = params.RobotIP; + obj.Settings = params.Settings; + obj.RobotIP = char(params.RobotIP); + if isempty(obj.RobotIP) + obj.RobotIP = obj.Settings.robot_ip; + end - % Initialize server based on parameters - if all(strcmp({params.Username, params.ServerIP, params.SSHPort, params.ServerPort}, ... - {obj.DefaultServerUsername, obj.DefaultServerIP, obj.DefaultSSHPort, obj.DefaultServerPort})) - % Case 1: All parameters are default values - use local server - obj.Server = FrankaRobotServer(); + % Remote mode if ServerIP is provided + serverIP = char(params.ServerIP); + if ~isempty(serverIP) + username = char(params.Username); + if isempty(username), username = obj.DefaultServerUsername; end + obj.Server = FrankaRobotServer(username, serverIP, ... + char(params.SSHPort), char(params.ServerPort)); else - % Case 2: At least one server parameter was specified - use remote server - obj.Server = FrankaRobotServer(params.Username, params.ServerIP, ... - params.SSHPort, params.ServerPort); + obj.Server = FrankaRobotServer(); + if ~strcmp(params.ServerPort, obj.DefaultServerPort) + obj.Server.setServerPort(char(params.ServerPort)); + end end try obj.Server.start(); catch ME - ME = MException('FrankaRobot:InitError', 'Failed to start Franka Robot Server: %s', ME.message); - throw(ME); + error('FrankaRobot:InitError', 'Failed to start server: %s', ME.message); end - % Create a new Franka Robot handle for the client obj.frankaRobotHandle = franka_robot('new', obj.Server.getServerIp(), obj.Server.getServerPort()); - % Initialize the robot try obj.initialize(); catch ME - % Clean up the server and handle before throwing error - if ~isempty(obj.Server) - obj.Server.stop(); - end - if ~isempty(obj.frankaRobotHandle) - franka_robot('delete', obj.frankaRobotHandle); - end - ME = MException('FrankaRobot:InitError', 'Failed to initialize robot: %s', ME.message); - throw(ME); + obj.Server.stop(); + franka_robot('delete', obj.frankaRobotHandle); + error('FrankaRobot:InitError', 'Failed to initialize robot: %s', ME.message); end - % Create the gripper instance obj.Gripper = FrankaGripper(obj.frankaRobotHandle); obj.VacuumGripper = FrankaVacuumGripper(obj.frankaRobotHandle); end - %% Destructor function delete(obj) if ~isempty(obj.Server) obj.Server.stop(); @@ -128,123 +101,214 @@ function delete(obj) end end - %% Franka Robot Automatic Error Recovery function automatic_error_recovery(obj, varargin) - - if ~isempty(obj.frankaRobotHandle) - franka_robot('automatic_error_recovery', obj.frankaRobotHandle, varargin{:}); - else - error('Franka Robot server is not connected. Please check the Server Status.'); - end - + obj.checkHandle(); + franka_robot('automatic_error_recovery', obj.frankaRobotHandle, varargin{:}); end function robot_state = robot_state(obj) - if ~isempty(obj.frankaRobotHandle) - robot_state = franka_robot('robot_state', obj.frankaRobotHandle); - else - error('Franka Robot server is not connected. Please check the Server Status.'); - end + obj.checkHandle(); + robot_state = franka_robot('robot_state', obj.frankaRobotHandle); end function joint_poses = joint_poses(obj) - if ~isempty(obj.frankaRobotHandle) - joint_poses = franka_robot('joint_poses', obj.frankaRobotHandle); - else - error('Franka Robot server is not connected. Please check the Server Status.'); - end + obj.checkHandle(); + joint_poses = franka_robot('joint_poses', obj.frankaRobotHandle); end function result = joint_point_to_point_motion(obj, joints_target_configuration, speed_factor) - if ~isempty(obj.frankaRobotHandle) - if nargin < 3 - speed_factor = 0.5; % Default speed factor - end - result = franka_robot('joint_point_to_point_motion', obj.frankaRobotHandle, ... - joints_target_configuration, speed_factor); - else - error('Franka Robot server is not connected. Please check the Server Status.'); - end + obj.checkHandle(); + if nargin < 3, speed_factor = 0.5; end + result = franka_robot('joint_point_to_point_motion', obj.frankaRobotHandle, ... + joints_target_configuration, speed_factor); end function result = joint_trajectory_motion(obj, positions) - if ~isempty(obj.frankaRobotHandle) - - % Validate dimensions - [m, ~] = size(positions); - if m ~= 7 - error('Positions must be a 7xN array'); - end - - result = franka_robot('joint_trajectory_motion', obj.frankaRobotHandle, positions); - else - error('Franka Robot server is not connected. Please check the Server Status.'); + obj.checkHandle(); + [m, ~] = size(positions); + if m ~= 7 + error('Positions must be a 7xN array'); end + result = franka_robot('joint_trajectory_motion', obj.frankaRobotHandle, positions); end function result = setCollisionThresholds(obj, thresholds) - % Set collision thresholds and apply them to the robot - obj.Settings.collisionThresholds = thresholds; + obj.Settings.collision_thresholds = thresholds; + ct = obj.Settings.collision_thresholds; result = franka_robot('set_collision_behavior', obj.frankaRobotHandle, ... - obj.Settings.collisionThresholds.lower_torque_thresholds_acceleration, ... - obj.Settings.collisionThresholds.upper_torque_thresholds_acceleration, ... - obj.Settings.collisionThresholds.lower_torque_thresholds_nominal, ... - obj.Settings.collisionThresholds.upper_torque_thresholds_nominal, ... - obj.Settings.collisionThresholds.lower_force_thresholds_acceleration, ... - obj.Settings.collisionThresholds.upper_force_thresholds_acceleration, ... - obj.Settings.collisionThresholds.lower_force_thresholds_nominal, ... - obj.Settings.collisionThresholds.upper_force_thresholds_nominal); + ct.lower_torque_thresholds_acceleration, ... + ct.upper_torque_thresholds_acceleration, ... + ct.lower_torque_thresholds_nominal, ... + ct.upper_torque_thresholds_nominal, ... + ct.lower_force_thresholds_acceleration, ... + ct.upper_force_thresholds_acceleration, ... + ct.lower_force_thresholds_nominal, ... + ct.upper_force_thresholds_nominal); end function thresholds = getCollisionThresholds(obj) - % Get the current collision thresholds - thresholds = obj.Settings.collisionThresholds; + thresholds = obj.Settings.collision_thresholds; end function result = setLoadInertia(obj, loadInertia) - % Set load inertia parameters and apply them to the robot - obj.Settings.loadInertia = loadInertia; + obj.Settings.load_inertia = loadInertia; + li = obj.Settings.load_inertia; result = franka_robot('set_load', obj.frankaRobotHandle, ... - obj.Settings.loadInertia.mass, ... - obj.Settings.loadInertia.center_of_mass, ... - obj.Settings.loadInertia.load_inertia); + li.mass, li.center_of_mass, li.inertia_matrix); end function inertia = getLoadInertia(obj) - % Get the current load inertia parameters - inertia = obj.Settings.loadInertia; + inertia = obj.Settings.load_inertia; end - %% Robot Homing function result = robot_homing(obj) - % Move the robot to its home configuration using point-to-point motion - % Returns true if the motion was successful, false otherwise - if ~isempty(obj.frankaRobotHandle) - result = obj.joint_point_to_point_motion(obj.DefaultSettings.homeConfiguration,.1); + obj.checkHandle(); + result = obj.joint_point_to_point_motion(obj.Settings.home_configuration, 0.1); + end + + function resetSettings(obj) + obj.Settings = FrankaRobotSettings(); + obj.applySettings(); + end + + function result = setJointImpedance(obj, K_theta) + obj.checkHandle(); + if nargin < 2 + K_theta = obj.Settings.joint_impedance_stiffness; else - error('Franka Robot server is not connected. Please check the Server Status.'); + validateattributes(K_theta, {'numeric'}, {'numel', 7, 'positive'}); + obj.Settings.joint_impedance_stiffness = K_theta(:)'; end + result = franka_robot('set_joint_impedance', obj.frankaRobotHandle, K_theta(:)'); end - %% Reset Settings - function resetSettings(obj) - % Reset all settings to their default values - obj.Settings = obj.DefaultSettings; + function K_theta = getJointImpedance(obj) + K_theta = obj.Settings.joint_impedance_stiffness; + end + + function result = setCartesianImpedance(obj, K_x) + obj.checkHandle(); + if nargin < 2 + K_x = obj.Settings.cartesian_impedance_stiffness; + else + validateattributes(K_x, {'numeric'}, {'numel', 6, 'positive'}); + obj.Settings.cartesian_impedance_stiffness = K_x(:)'; + end + result = franka_robot('set_cartesian_impedance', obj.frankaRobotHandle, K_x(:)'); + end + + function K_x = getCartesianImpedance(obj) + K_x = obj.Settings.cartesian_impedance_stiffness; + end + + function result = setGuidingMode(obj, guiding_mode, elbow) + obj.checkHandle(); + validateattributes(guiding_mode, {'logical'}, {'numel', 6}); + validateattributes(elbow, {'logical'}, {'scalar'}); + result = franka_robot('set_guiding_mode', obj.frankaRobotHandle, guiding_mode(:)', elbow); + end + + function result = setK(obj, EE_T_K) + obj.checkHandle(); + if nargin < 2 + EE_T_K = obj.Settings.EE_T_K; + else + validateattributes(EE_T_K, {'numeric'}, {'size', [4, 4]}); + obj.Settings.EE_T_K = EE_T_K; + end + result = franka_robot('set_k', obj.frankaRobotHandle, EE_T_K(:)'); + end + + function EE_T_K = getK(obj) + EE_T_K = obj.Settings.EE_T_K; + end + + function result = setEE(obj, NE_T_EE) + obj.checkHandle(); + if nargin < 2 + NE_T_EE = obj.Settings.NE_T_EE; + else + validateattributes(NE_T_EE, {'numeric'}, {'size', [4, 4]}); + obj.Settings.NE_T_EE = NE_T_EE; + end + result = franka_robot('set_ee', obj.frankaRobotHandle, NE_T_EE(:)'); + end + + function NE_T_EE = getEE(obj) + NE_T_EE = obj.Settings.NE_T_EE; + end + + function result = stop(obj) + obj.checkHandle(); + result = franka_robot('stop_robot', obj.frankaRobotHandle); + end + + function result = ping(obj) + % Ping the server to verify connectivity + obj.checkHandle(); + result = franka_robot('ping', obj.frankaRobotHandle); + end + + function healthy = isHealthy(obj) + % Check if server connection is healthy via RPC ping + try + result = obj.ping(); + healthy = ~isempty(result) && result.port == str2double(obj.Server.getServerPort()); + catch + healthy = false; + end + end + + function reconnect(obj) + % Reconnect to server after it was restarted + % + % Use this method after calling Server.stop() and Server.start() + % to re-establish the RPC connection. - % Apply the reset settings to the robot - obj.applySettings(); + % Ensure server is running + if ~obj.Server.isRunning() + obj.Server.start(); + end + + % Delete old handle if exists + if ~isempty(obj.frankaRobotHandle) + try + franka_robot('delete', obj.frankaRobotHandle); + catch + % Ignore errors from stale handle + end + end + + % Create new connection + obj.frankaRobotHandle = franka_robot('new', obj.Server.getServerIp(), obj.Server.getServerPort()); + + % Reinitialize robot + obj.initialize(); + + % Recreate gripper interfaces with new handle + obj.Gripper = FrankaGripper(obj.frankaRobotHandle); + obj.VacuumGripper = FrankaVacuumGripper(obj.frankaRobotHandle); end function initialize(obj) - % Initialize the robot franka_robot('initialize_robot', obj.frankaRobotHandle, obj.RobotIP); end end methods (Access = private) + function checkHandle(obj) + if isempty(obj.frankaRobotHandle) + error('FrankaRobot:NotConnected', 'Server not connected'); + end + end + function applySettings(obj) - obj.setCollisionThresholds(obj.Settings.collisionThresholds); - obj.setLoadInertia(obj.Settings.loadInertia); + obj.setCollisionThresholds(obj.Settings.collision_thresholds); + obj.setLoadInertia(obj.Settings.load_inertia); + obj.setJointImpedance(); + obj.setCartesianImpedance(); + obj.setEE(); + obj.setK(); end end -end \ No newline at end of file +end diff --git a/franka_robot/bin.zip b/franka_robot/bin.zip deleted file mode 100644 index 6905b83..0000000 Binary files a/franka_robot/bin.zip and /dev/null differ diff --git a/franka_robot/franka_robot_mex.m b/franka_robot/franka_robot_mex.m index c52fe91..5dfb457 100644 --- a/franka_robot/franka_robot_mex.m +++ b/franka_robot/franka_robot_mex.m @@ -15,9 +15,11 @@ function franka_robot_mex() mkdir(destination_path); end + opts = struct('verbose', true, 'nothrow', false); + if isunix() % Generate capnp files first - franka_toolbox_system_cmd('./generate_capnp.sh',franka_robot_server_path,true); + franka_toolbox_local_exec('./generate_capnp.sh', franka_robot_server_path, opts); mex_string = strjoin({... 'mex', ... @@ -31,14 +33,14 @@ function franka_robot_mex() '/usr/local/lib/libkj-async.a', ... '/usr/local/lib/libkj.a', ... '-lpthread', ... % Added pthread dependency - 'CXXFLAGS="\$CXXFLAGS -std=c++14 -fPIC"', ... + 'CXXFLAGS="\$CXXFLAGS -std=c++17 -fPIC"', ... [' -outdir ', destination_path] }); elseif ispc() % Generate capnp files first - franka_toolbox_system_cmd('generate_capnp.bat',franka_robot_server_path,true); + franka_toolbox_local_exec('generate_capnp.bat', franka_robot_server_path, opts); capnproto_installation_dir = 'C:\Program Files (x86)\capnproto-c++-win32-1.0.2\capnproto-c++-1.0.2\src'; @@ -56,7 +58,7 @@ function franka_robot_mex() '-lkj', ... '-lkj-async', ... '-lWs2_32', ... - 'CXXFLAGS="\$CXXFLAGS -std=c++14"', ... + 'COMPFLAGS="$COMPFLAGS /std:c++17"', ... fullfile(franka_robot_path,'franka_robot.cpp'), ... fullfile(franka_robot_server_build_path,'interface','rpc.capnp.cpp'), ... [' -outdir ', destination_path] @@ -93,4 +95,4 @@ function franka_robot_mex() rmdir(fullfile(installation_path,'franka_robot','bin'), 's'); end -end \ No newline at end of file +end diff --git a/franka_robot/src/franka_robot.cpp b/franka_robot/src/franka_robot.cpp index 9c95781..2bf1f09 100644 --- a/franka_robot/src/franka_robot.cpp +++ b/franka_robot/src/franka_robot.cpp @@ -616,6 +616,156 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) return; } + // Set Joint Impedance + if (!strcmp("set_joint_impedance", cmd)) { + if (nlhs != 1 || nrhs != 3) + mexErrMsgTxt("Set Joint Impedance: One output and two inputs (handle, K_theta) expected."); + + if (!mxIsDouble(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 7) + mexErrMsgTxt("Joint impedance must be a 7-element double array."); + + try { + std::array K_theta{}; + double* k_ptr = mxGetPr(prhs[2]); + for (size_t i = 0; i < 7; i++) { + K_theta[i] = k_ptr[i]; + } + + bool success = franka_robot_instance->setJointImpedance(K_theta); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to set joint impedance"); + } + return; + } + + // Set Cartesian Impedance + if (!strcmp("set_cartesian_impedance", cmd)) { + if (nlhs != 1 || nrhs != 3) + mexErrMsgTxt("Set Cartesian Impedance: One output and two inputs (handle, K_x) expected."); + + if (!mxIsDouble(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 6) + mexErrMsgTxt("Cartesian impedance must be a 6-element double array."); + + try { + std::array K_x{}; + double* k_ptr = mxGetPr(prhs[2]); + for (size_t i = 0; i < 6; i++) { + K_x[i] = k_ptr[i]; + } + + bool success = franka_robot_instance->setCartesianImpedance(K_x); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to set Cartesian impedance"); + } + return; + } + + // Set Guiding Mode + if (!strcmp("set_guiding_mode", cmd)) { + if (nlhs != 1 || nrhs != 4) + mexErrMsgTxt("Set Guiding Mode: One output and three inputs (handle, guiding_mode, elbow) expected."); + + if (!mxIsLogical(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 6) + mexErrMsgTxt("Guiding mode must be a 6-element logical array."); + + if (!mxIsLogicalScalar(prhs[3])) + mexErrMsgTxt("Elbow must be a logical scalar."); + + try { + std::array guiding_mode{}; + mxLogical* mode_ptr = mxGetLogicals(prhs[2]); + for (size_t i = 0; i < 6; i++) { + guiding_mode[i] = mode_ptr[i]; + } + bool elbow = mxIsLogicalScalarTrue(prhs[3]); + + bool success = franka_robot_instance->setGuidingMode(guiding_mode, elbow); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to set guiding mode"); + } + return; + } + + // Set K (EE_T_K transformation) + if (!strcmp("set_k", cmd)) { + if (nlhs != 1 || nrhs != 3) + mexErrMsgTxt("Set K: One output and two inputs (handle, EE_T_K) expected."); + + if (!mxIsDouble(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 16) + mexErrMsgTxt("EE_T_K must be a 16-element double array (4x4 matrix in column-major format)."); + + try { + std::array EE_T_K{}; + double* k_ptr = mxGetPr(prhs[2]); + for (size_t i = 0; i < 16; i++) { + EE_T_K[i] = k_ptr[i]; + } + + bool success = franka_robot_instance->setK(EE_T_K); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to set EE_T_K transformation"); + } + return; + } + + // Set EE (NE_T_EE transformation) + if (!strcmp("set_ee", cmd)) { + if (nlhs != 1 || nrhs != 3) + mexErrMsgTxt("Set EE: One output and two inputs (handle, NE_T_EE) expected."); + + if (!mxIsDouble(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 16) + mexErrMsgTxt("NE_T_EE must be a 16-element double array (4x4 matrix in column-major format)."); + + try { + std::array NE_T_EE{}; + double* ee_ptr = mxGetPr(prhs[2]); + for (size_t i = 0; i < 16; i++) { + NE_T_EE[i] = ee_ptr[i]; + } + + bool success = franka_robot_instance->setEE(NE_T_EE); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to set NE_T_EE transformation"); + } + return; + } + + // Stop Robot + if (!strcmp("stop_robot", cmd)) { + if (nlhs != 1 || nrhs != 2) + mexErrMsgTxt("Stop Robot: One output and one input (handle) expected."); + try { + bool success = franka_robot_instance->stopRobot(); + plhs[0] = mxCreateLogicalScalar(success); + } catch (...) { + mexErrMsgTxt("Failed to stop robot"); + } + return; + } + + // Ping (health check) + if (!strcmp("ping", cmd)) { + if (nlhs != 1 || nrhs != 2) + mexErrMsgTxt("Ping: One output and one input (handle) expected."); + try { + auto [timestamp, port] = franka_robot_instance->ping(); + + // Create struct with timestamp and port + const char* fieldnames[] = {"timestamp", "port"}; + plhs[0] = mxCreateStructMatrix(1, 1, 2, fieldnames); + mxSetField(plhs[0], 0, "timestamp", mxCreateDoubleScalar(static_cast(timestamp))); + mxSetField(plhs[0], 0, "port", mxCreateDoubleScalar(static_cast(port))); + } catch (...) { + mexErrMsgTxt("Failed to ping server"); + } + return; + } + // Got here, so command not recognized mexErrMsgTxt("Command not recognized."); } diff --git a/franka_robot/src/franka_robot.hpp b/franka_robot/src/franka_robot.hpp index ea48a3c..1ab7a34 100644 --- a/franka_robot/src/franka_robot.hpp +++ b/franka_robot/src/franka_robot.hpp @@ -307,6 +307,114 @@ class FrankaRobot { } } + // Impedance control + bool setJointImpedance(const std::array& K_theta) { + auto request = rpcInterface.setJointImpedanceRequest(); + auto k_theta = request.initKTheta(7); + for (size_t i = 0; i < 7; i++) { + k_theta.set(i, K_theta[i]); + } + + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + bool setCartesianImpedance(const std::array& K_x) { + auto request = rpcInterface.setCartesianImpedanceRequest(); + auto k_x = request.initKX(6); + for (size_t i = 0; i < 6; i++) { + k_x.set(i, K_x[i]); + } + + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + // Guiding mode + bool setGuidingMode(const std::array& guiding_mode, bool elbow) { + auto request = rpcInterface.setGuidingModeRequest(); + auto mode = request.initGuidingMode(6); + for (size_t i = 0; i < 6; i++) { + mode.set(i, guiding_mode[i]); + } + request.setElbow(elbow); + + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + // Frame transformations + bool setK(const std::array& EE_T_K) { + auto request = rpcInterface.setKRequest(); + auto ee_t_k = request.initEeTK(16); + for (size_t i = 0; i < 16; i++) { + ee_t_k.set(i, EE_T_K[i]); + } + + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + bool setEE(const std::array& NE_T_EE) { + auto request = rpcInterface.setEERequest(); + auto ne_t_ee = request.initNeTEe(16); + for (size_t i = 0; i < 16; i++) { + ne_t_ee.set(i, NE_T_EE[i]); + } + + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + // Robot control + bool stopRobot() { + auto request = rpcInterface.stopRobotRequest(); + try { + auto response = request.send().wait(client.getWaitScope()); + return response.getSuccess(); + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + + // Health check - returns server timestamp and port + std::pair ping() { + auto request = rpcInterface.pingRequest(); + try { + auto response = request.send().wait(client.getWaitScope()); + return {response.getTimestamp(), response.getPort()}; + } catch (const kj::Exception& e) { + std::cerr << "Franka Robot Error: " << e.getDescription().cStr() << std::endl; + throw; + } + } + private: capnp::EzRpcClient client; RPCService::Client rpcInterface; diff --git a/franka_robot_server/CMakeLists.txt b/franka_robot_server/CMakeLists.txt index 33bc77e..9b9f035 100644 --- a/franka_robot_server/CMakeLists.txt +++ b/franka_robot_server/CMakeLists.txt @@ -7,7 +7,7 @@ if(NOT CMAKE_BUILD_TYPE) endif() # Set C++ standard -set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Set default libfranka folder name @@ -68,6 +68,11 @@ set(SOURCES src/joint_trajectory_motion.cpp src/collision_behavior.cpp src/load_inertia.cpp + src/impedance_control.cpp + src/guiding_mode.cpp + src/frame_transformations.cpp + src/stop_robot.cpp + src/ping.cpp ${RPC_SRCS} ${RPC_HDRS} ) diff --git a/franka_robot_server/FrankaRobotServer.m b/franka_robot_server/FrankaRobotServer.m index 6624118..9f1277d 100644 --- a/franka_robot_server/FrankaRobotServer.m +++ b/franka_robot_server/FrankaRobotServer.m @@ -1,226 +1,137 @@ classdef FrankaRobotServer < handle + %FRANKAROBOTSERVER Manages franka_robot_server process lifecycle + properties (Access = private) pid logFile outputFid - Username % SSH username for remote connection + Username ServerIP = 'localhost' SSHPort = '22' ServerPort = '5001' execDir - isRemote % Flag to indicate remote operation - isWindows % Flag to indicate Windows host + isRemote + isWindows + archSuffix end methods function obj = FrankaRobotServer(Username, ServerIP, SSHPort, ServerPort) - % Constructor for FrankaRobotServer obj.isRemote = false; obj.isWindows = ispc(); + obj.archSuffix = ''; if nargin == 4 obj.isRemote = true; - obj.Username = Username; obj.ServerIP = ServerIP; obj.SSHPort = SSHPort; obj.ServerPort = ServerPort; + obj.archSuffix = obj.detectRemoteArch(); end - % Set up execution directory and log file - arch = ''; - if obj.isRemote - arch = '_arm'; - end - obj.execDir = fullfile(franka_toolbox_installation_path_get(), 'franka_robot_server', ['bin',arch]); - obj.logFile = fullfile(obj.execDir, 'output.log'); + obj.execDir = fullfile(franka_toolbox_installation_path_get(), ... + 'franka_robot_server', ['bin', obj.archSuffix]); end function start(obj) - % Start the executable and save its PID - execPath = fullfile(obj.execDir,'franka_robot_server'); - pidFile = fullfile(obj.execDir, 'pidfile'); + % Check if already running + if obj.isRunning() + warning('FrankaRobotServer:AlreadyRunning', ... + 'Server already running on port %s', obj.ServerPort); + return; + end + + % Clean up any orphaned process on same port + obj.cleanupOrphan(); + + execPath = fullfile(obj.execDir, 'franka_robot_server'); + pidFile = obj.pidPath(); + obj.logFile = obj.logPath(); if obj.isRemote - % Set up remote workspace if not already done - remoteDir = '~/franka_matlab_ws/franka_robot_server/bin_arm'; - - % Construct SSH command based on platform - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - - % Check if executable already exists on remote machine - if obj.isWindows - % For Windows, use explicit home directory path instead of tilde - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - [status, ~] = system([sshCmd ' "test -x ' fullRemoteDir '/franka_robot_server"']); - else - [status, ~] = system([sshCmd ' "test -x ' remoteDir '/franka_robot_server"']); - end - if status ~= 0 + remoteDir = obj.remoteDir(); + [s, ~] = obj.ssh(['test -x ' remoteDir '/franka_robot_server']); + if s ~= 0 obj.setupRemoteWorkspace(); end - % Run on remote machine - if obj.isWindows - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - cmd = [sshCmd ' "cd ' fullRemoteDir ' && (./franka_robot_server ' obj.ServerIP ' ' ... - obj.ServerPort ' > output.log 2>&1 & echo $! > pidfile)"']; - else - cmd = [sshCmd ' "cd ' remoteDir ' && (./franka_robot_server ' obj.ServerIP ' ' ... - obj.ServerPort ' > output.log 2>&1 & echo \$! > pidfile)"']; - end - - % Update paths for remote operation - if obj.isWindows - obj.logFile = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm/output.log']; - pidFile = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm/pidfile']; - else - obj.logFile = [remoteDir '/output.log']; - pidFile = [remoteDir '/pidfile']; + esc = ternary(obj.isWindows, '$!', '\$!'); + cmd = sprintf('cd %s && (./franka_robot_server %s %s > %s 2>&1 & echo %s > %s)', ... + remoteDir, obj.ServerIP, obj.ServerPort, obj.logFile, esc, pidFile); + [s, ~] = obj.ssh(cmd); + if s ~= 0 + error('FrankaRobotServer:StartFailed', 'Failed to start server'); end + pause(0.5); + [~, pidStr] = obj.ssh(['cat ' pidFile]); + obj.pid = str2double(strtrim(pidStr)); else if obj.isWindows - error('Local operation is not supported on Windows hosts'); + error('FrankaRobotServer:Unsupported', 'Local mode not supported on Windows'); end - - % Check if executable exists if ~exist(execPath, 'file') - error('Executable not found at: %s', execPath); - end - - % Run on local machine - % Quote paths to handle spaces in directories/file names - q = @(p) ['''', p, '''']; - cmd = [q(execPath) ' ' obj.ServerIP ' ' obj.ServerPort ... - ' > ' q(obj.logFile) ' 2>&1 & echo $! > ' q(pidFile)]; - end - - % Execute command - [status, ~] = system(cmd); - if status ~= 0 - error('Failed to start the franka_robot_server'); - end - - % Wait and read PID - pause(1); % Give time for remote operation - - if obj.isRemote - % Wait for remote pidfile to be created (with timeout) - maxWaitTime = 5; % seconds - waitTime = 0; - pidExists = false; - - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - - while waitTime < maxWaitTime && ~pidExists - if obj.isWindows - % For Windows, use explicit home directory path instead of tilde - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - [status, ~] = system([sshCmd ' "test -f ' fullRemoteDir '/pidfile"']); - else - [status, ~] = system([sshCmd ' "test -f ' remoteDir '/pidfile"']); - end - pidExists = (status == 0); - if ~pidExists - pause(0.1); - waitTime = waitTime + 0.1; - end - end - - if ~pidExists - error('Remote PID file was not created within timeout period'); + error('FrankaRobotServer:NotFound', ... + 'Executable not found. Run franka_robot_server_build()'); end - if obj.isWindows - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - [~, pidStr] = system([sshCmd ' "cat ' fullRemoteDir '/pidfile"']); - else - [~, pidStr] = system([sshCmd ' "cat ' remoteDir '/pidfile"']); - end - obj.pid = str2double(strtrim(pidStr)); - else - % Wait for pidfile to be created (with timeout) - maxWaitTime = 5; % seconds - waitTime = 0; - while ~exist(pidFile, 'file') && waitTime < maxWaitTime - pause(0.1); - waitTime = waitTime + 0.1; + q = @(p) ['''' p '''']; + cmd = sprintf('%s %s %s > %s 2>&1 & echo $! > %s', ... + q(execPath), obj.ServerIP, obj.ServerPort, q(obj.logFile), q(pidFile)); + [s, ~] = franka_toolbox_local_exec(cmd, obj.execDir); + if s ~= 0 + error('FrankaRobotServer:StartFailed', 'Failed to start server'); end - - if ~exist(pidFile, 'file') - error('PID file was not created within timeout period'); - end - + pause(0.5); obj.pid = str2double(fileread(pidFile)); obj.outputFid = fopen(obj.logFile, 'r'); end - if obj.pid <= 0 - error('Failed to start franka_robot_server process'); + if isempty(obj.pid) || obj.pid <= 0 || isnan(obj.pid) + error('FrankaRobotServer:StartFailed', 'Failed to get server PID'); end end function stop(obj) - if ~isempty(obj.pid) && obj.isRunning() + if obj.isRunning() + % Use pgrep + xargs kill (avoids $() which gets expanded locally for SSH) + % Use [f] trick so pgrep doesn't match itself + pattern = sprintf('[f]ranka_robot_server.*%s', obj.ServerPort); + killCmd = sprintf('pgrep -f ''%s'' | xargs kill 2>/dev/null', pattern); if obj.isRemote - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -o StrictHostKeyChecking=no -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - system([sshCmd ' "kill ' num2str(obj.pid) '"']); + obj.ssh(killCmd); else - system(['kill ', num2str(obj.pid)]); + system(killCmd); end + pause(0.3); end obj.cleanup(); end function running = isRunning(obj) - if isempty(obj.pid) - running = false; - return; - end + % Use pgrep to find process by name and port (more robust than PID) + % Use [f] trick so pgrep doesn't match itself + pattern = sprintf('[f]ranka_robot_server.*%s', obj.ServerPort); if obj.isRemote - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - [status, ~] = system([sshCmd ' "ps -p ' num2str(obj.pid) '"']); + [s, ~] = obj.ssh(sprintf('pgrep -f ''%s'' > /dev/null 2>&1', pattern)); else - [status, ~] = system(['ps -p ', num2str(obj.pid)]); + [s, ~] = system(sprintf('pgrep -f ''%s'' > /dev/null 2>&1', pattern)); end - running = (status == 0); + running = (s == 0); end function lines = getOutput(obj) lines = {}; if obj.isRemote - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - [~, output] = system([sshCmd ' "cat ' obj.logFile '"']); - if ~isempty(output) - lines = strsplit(output, '\n'); + [~, out] = obj.ssh(['cat ' obj.logFile ' 2>/dev/null']); + if ~isempty(out) + lines = strsplit(out, '\n'); end - else - if ~isempty(obj.outputFid) && ~feof(obj.outputFid) - while ~feof(obj.outputFid) - line = fgetl(obj.outputFid); - if ischar(line) - lines{end+1} = line; - end + elseif ~isempty(obj.outputFid) && ~feof(obj.outputFid) + while ~feof(obj.outputFid) + line = fgetl(obj.outputFid); + if ischar(line) + lines{end+1} = line; %#ok end end end @@ -231,187 +142,173 @@ function delete(obj) end function setupRemoteWorkspace(obj) - % Sets up the remote workspace by creating directories and copying necessary files + remoteDir = obj.remoteDir(); + remoteMatlabWs = obj.remoteMatlabWsDir(); + scpOpts = struct('recursive', false, 'nothrow', false); + scpOptsR = struct('recursive', true, 'nothrow', false); - if obj.isWindows - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - fullRemoteMatlabWs = ['/home/' obj.Username '/franka_matlab_ws/libfranka_arm/build']; - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - scpCmd = ['scp.exe -P ' obj.SSHPort]; - else - remoteDir = '~/franka_matlab_ws/franka_robot_server/bin_arm'; - remoteMatlabWs = '~/franka_matlab_ws/libfranka_arm/build'; - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - scpCmd = ['scp -P ' obj.SSHPort]; - end - execPath = fullfile(obj.execDir,'franka_robot_server'); + obj.ssh(['mkdir -p ' remoteDir]); + obj.ssh(['mkdir -p ' remoteMatlabWs]); - % Create remote directory - if obj.isWindows - system([sshCmd ' "mkdir -p ' fullRemoteDir '"']); - else - system([sshCmd ' "mkdir -p ' remoteDir '"']); - end + execPath = fullfile(obj.execDir, 'franka_robot_server'); + franka_toolbox_scp(execPath, [':' remoteDir '/'], ... + obj.Username, obj.ServerIP, obj.SSHPort, scpOpts); - % Copy executable to remote machine - if obj.isWindows - [status, ~] = system([scpCmd ' ' strrep(execPath,'\','/') ' ' obj.Username '@' obj.ServerIP ':' fullRemoteDir '/']); - else - [status, ~] = system([scpCmd ' ' strrep(execPath,'\','/') ' ' obj.Username '@' obj.ServerIP ':' remoteDir '/']); - end - if status ~= 0 - error('Failed to copy executable to remote machine'); - end - - % Copy libfranka_arm folder to remote machine - libfrankaPath = fullfile(franka_toolbox_installation_path_get(), 'libfranka_arm','build','usr'); - if obj.isWindows - system([sshCmd ' "mkdir -p ' fullRemoteMatlabWs '"']); - [status, ~] = system([scpCmd ' -r ' strrep(libfrankaPath,'\','/') ' ' obj.Username '@' obj.ServerIP ':' fullRemoteMatlabWs '/']); - else - system([sshCmd ' "mkdir -p ' remoteMatlabWs '"']); - [status, ~] = system([scpCmd ' -r ' strrep(libfrankaPath,'\','/') ' ' obj.Username '@' obj.ServerIP ':' remoteMatlabWs '/']); - end - if status ~= 0 - error('Failed to copy libfranka_arm folder to remote machine'); - end + libfrankaPath = fullfile(franka_toolbox_installation_path_get(), ... + ['libfranka' obj.archSuffix], 'build', 'usr'); + franka_toolbox_scp(libfrankaPath, [':' remoteMatlabWs '/'], ... + obj.Username, obj.ServerIP, obj.SSHPort, scpOptsR); end function deleteRemoteWorkspace(obj) - % Deletes the remote workspace directory and all its contents if obj.isRemote - if obj.isWindows - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws']; - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - else - remoteDir = '~/franka_matlab_ws'; - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - end - - % Stop the server if it's running obj.stop(); - - % Delete the remote workspace - if obj.isWindows - [status, ~] = system([sshCmd ' "rm -rf ' fullRemoteDir '"']); - else - [status, ~] = system([sshCmd ' "rm -rf ' remoteDir '"']); - end - if status ~= 0 - warning('Failed to delete remote workspace directory'); - end - else - warning('deleteRemoteWorkspace only applies to remote configurations'); + obj.ssh(['rm -rf ' obj.remoteWsRoot()]); end end - % Getter methods - function value = getUsername(obj) - value = obj.Username; - end - - function value = getServerIp(obj) - value = obj.ServerIP; - end - - function value = getSshPort(obj) - value = obj.SSHPort; - end - - function value = getServerPort(obj) - value = obj.ServerPort; - end - - % Setter methods - function setUsername(obj, value) - validateattributes(value, {'char', 'string'}, {'nonempty'}); - obj.Username = value; - end - - function setServerIp(obj, value) - validateattributes(value, {'char', 'string'}, {'nonempty'}); - obj.ServerIP = value; - end - - function setSshPort(obj, value) - validateattributes(value, {'char', 'string'}, {'nonempty'}); - obj.SSHPort = value; - end - + function value = getUsername(obj), value = obj.Username; end + function value = getServerIp(obj), value = obj.ServerIP; end + function value = getSshPort(obj), value = obj.SSHPort; end + function value = getServerPort(obj), value = obj.ServerPort; end + function setServerPort(obj, value) - validateattributes(value, {'char', 'string'}, {'nonempty'}); - obj.ServerPort = value; + obj.ServerPort = char(value); end - - function cleanupRemote(obj) - % Public method to cleanup the remote workspace using instance properties - if obj.isRunning() - warning('Cannot cleanup remote workspace while server is running'); - return; + end + + methods (Access = private) + function path = pidPath(obj) + if obj.isRemote + path = sprintf('%s/franka_server_%s.pid', obj.remoteDir(), obj.ServerPort); + else + path = fullfile(obj.execDir, sprintf('franka_server_%s.pid', obj.ServerPort)); end - + end + + function path = logPath(obj) if obj.isRemote - FrankaRobotServer.cleanupRemoteWorkspace(obj.Username, obj.ServerIP, obj.SSHPort); + path = sprintf('%s/franka_server_%s.log', obj.remoteDir(), obj.ServerPort); else - warning('cleanupRemote only applies to remote configurations'); + path = fullfile(obj.execDir, sprintf('franka_server_%s.log', obj.ServerPort)); end end - end - - methods (Access = private) - function cleanup(obj) + + function cleanupOrphan(obj) + % Check for stale PID file and kill if it's our process + pidFile = obj.pidPath(); if obj.isRemote - if obj.isWindows - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - fullRemoteDir = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin_arm']; - system([sshCmd ' "rm -f ' fullRemoteDir '/output.log ' fullRemoteDir '/pidfile"']); + [s, pidStr] = obj.ssh(['cat ' pidFile ' 2>/dev/null']); + if s == 0 && ~isempty(pidStr) + stalePid = str2double(strtrim(pidStr)); + if ~isnan(stalePid) && obj.isOurProcess(stalePid) + obj.ssh(sprintf('kill -9 %d 2>/dev/null', stalePid)); + pause(0.2); + end + end + else + if exist(pidFile, 'file') + stalePid = str2double(fileread(pidFile)); + if ~isnan(stalePid) && obj.isOurProcess(stalePid) + system(sprintf('kill -9 %d 2>/dev/null', stalePid)); + pause(0.2); + end + end + end + end + + function isOurs = isOurProcess(obj, pid) + % Check if PID belongs to franka_robot_server + if obj.isRemote + [s, out] = obj.ssh(sprintf('cat /proc/%d/cmdline 2>/dev/null', pid)); + isOurs = (s == 0) && contains(out, 'franka_robot_server'); + else + cmdFile = sprintf('/proc/%d/cmdline', pid); + if exist(cmdFile, 'file') + isOurs = contains(fileread(cmdFile), 'franka_robot_server'); else - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' obj.SSHPort ' ' obj.Username '@' obj.ServerIP]; - remoteDir = '~/franka_matlab_ws/franka_robot_server/bin_arm'; - system([sshCmd ' "rm -f ' remoteDir '/output.log ' remoteDir '/pidfile"']); + isOurs = false; end + end + end + + function cleanup(obj) + pidFile = obj.pidPath(); + if obj.isRemote + obj.ssh(sprintf('rm -f %s %s', pidFile, obj.logFile)); else if ~isempty(obj.outputFid) fclose(obj.outputFid); obj.outputFid = []; end - if exist(obj.logFile, 'file') - delete(obj.logFile); - end - if exist(fullfile(obj.execDir, 'pidfile'), 'file') - delete(fullfile(obj.execDir, 'pidfile')); - end + if exist(pidFile, 'file'), delete(pidFile); end + if exist(obj.logFile, 'file'), delete(obj.logFile); end end obj.pid = []; end - end - - methods (Static) - function cleanupRemoteWorkspace(username, serverIP, sshPort) - % Static method to cleanup the remote workspace - % Parameters: - % username - SSH username for remote connection - % serverIP - IP address of remote server - % sshPort - SSH port number (as string) + + function [status, output] = ssh(obj, cmd) + [status, output] = franka_toolbox_ssh_exec(cmd, obj.Username, ... + obj.ServerIP, obj.SSHPort); + end + + function arch = detectRemoteArch(obj) + % Validate connection with user-friendly error + opts = struct('nothrow', true, 'timeout', 5); + [status, ~] = franka_toolbox_ssh_exec('echo ok', obj.Username, ... + obj.ServerIP, obj.SSHPort, opts); + if status ~= 0 + error('FrankaRobotServer:ConnectionFailed', ... + 'Cannot connect to %s@%s:%s\nPlease verify the ServerIP, SSHPort, and Username are correct.', ... + obj.Username, obj.ServerIP, obj.SSHPort); + end - isWindows = ispc(); - if isWindows - fullRemoteDir = ['/home/' username '/franka_matlab_ws']; - sshCmd = ['ssh.exe -o ConnectTimeout=5 -o BatchMode=yes -p ' sshPort ' ' username '@' serverIP]; + [~, out] = obj.ssh('uname -m'); + out = strtrim(out); + if any(strcmp(out, {'aarch64', 'arm64', 'armv8l'})) + arch = '_arm'; else - remoteDir = '~/franka_matlab_ws'; - sshCmd = ['ssh -o ConnectTimeout=5 -o BatchMode=yes -p ' sshPort ' ' username '@' serverIP]; + arch = ''; end - - % Delete the remote workspace - if isWindows - [status, ~] = system([sshCmd ' "rm -rf ' fullRemoteDir '"']); + end + + function path = remoteDir(obj) + if obj.isWindows + path = ['/home/' obj.Username '/franka_matlab_ws/franka_robot_server/bin' obj.archSuffix]; else - [status, ~] = system([sshCmd ' "rm -rf ' remoteDir '"']); + path = ['~/franka_matlab_ws/franka_robot_server/bin' obj.archSuffix]; end - if status ~= 0 - warning('Failed to delete remote workspace directory'); + end + + function path = remoteMatlabWsDir(obj) + if obj.isWindows + path = ['/home/' obj.Username '/franka_matlab_ws/libfranka' obj.archSuffix '/build']; + else + path = ['~/franka_matlab_ws/libfranka' obj.archSuffix '/build']; end end + + function path = remoteWsRoot(obj) + if obj.isWindows + path = ['/home/' obj.Username '/franka_matlab_ws']; + else + path = '~/franka_matlab_ws'; + end + end + end + + methods (Static) + function cleanupRemoteWorkspace(username, serverIP, sshPort) + if ispc() + root = ['/home/' username '/franka_matlab_ws']; + else + root = '~/franka_matlab_ws'; + end + franka_toolbox_ssh_exec(['rm -rf ' root], username, serverIP, sshPort); + end end -end +end + +function result = ternary(cond, t, f) + if cond, result = t; else, result = f; end +end diff --git a/franka_robot_server/bin.tar.gz b/franka_robot_server/bin.tar.gz deleted file mode 100644 index cb431da..0000000 Binary files a/franka_robot_server/bin.tar.gz and /dev/null differ diff --git a/franka_robot_server/bin_arm.tar.gz b/franka_robot_server/bin_arm.tar.gz deleted file mode 100644 index 3df7e53..0000000 Binary files a/franka_robot_server/bin_arm.tar.gz and /dev/null differ diff --git a/franka_robot_server/franka_robot_server_build.m b/franka_robot_server/franka_robot_server_build.m index f56a5b0..a122fea 100644 --- a/franka_robot_server/franka_robot_server_build.m +++ b/franka_robot_server/franka_robot_server_build.m @@ -38,17 +38,18 @@ function franka_robot_server_build(varargin) % Define remote installation path remote_installation_path = '~/franka_matlab'; + sshOpts = struct('verbose', true, 'nothrow', false); + scpOpts = struct('recursive', true, 'verbose', true, 'nothrow', false); + % Check if remote directory exists before removing - [~, cmdout] = franka_toolbox_remote_system_cmd('ls -d ~/franka_matlab 2>/dev/null || echo "Directory does not exist"', '', p.Results.user, p.Results.ip, p.Results.port,true); - if ~contains(cmdout, 'Directory does not exist') - franka_toolbox_remote_system_cmd('rm -rf ~/franka_matlab', '', p.Results.user, p.Results.ip, p.Results.port,true); + [status, ~] = franka_toolbox_ssh_exec('ls -d ~/franka_matlab 2>/dev/null', ... + p.Results.user, p.Results.ip, p.Results.port); + if status == 0 + franka_toolbox_ssh_exec('rm -rf ~/franka_matlab', ... + p.Results.user, p.Results.ip, p.Results.port, sshOpts); end - franka_toolbox_remote_system_cmd(... - ['mkdir -p ', remote_installation_path], ... - '', ... % No working directory needed for mkdir - p.Results.user, ... - p.Results.ip, ... - p.Results.port,true); + franka_toolbox_ssh_exec(['mkdir -p ', remote_installation_path], ... + p.Results.user, p.Results.ip, p.Results.port, sshOpts); % Copy only the necessary folders to remote machine folders_to_copy = {'common', 'franka_robot_server','libfranka_arm'}; @@ -64,8 +65,9 @@ function franka_robot_server_build(varargin) for i = 1:length(folders_to_copy) source_path = fullfile(installation_path, folders_to_copy{i}); - remote_path = [remote_installation_path, '/', folders_to_copy{i}]; - franka_toolbox_foder_remote_cp(source_path, p.Results.user, p.Results.ip, remote_path, p.Results.port,true); + remote_path = [':' remote_installation_path, '/', folders_to_copy{i}]; + franka_toolbox_scp(source_path, remote_path, ... + p.Results.user, p.Results.ip, p.Results.port, scpOpts); end % Execute remote build @@ -74,21 +76,21 @@ function franka_robot_server_build(varargin) fprintf('Starting remote build process...\n'); % Create and navigate to build directory - franka_toolbox_remote_system_cmd('mkdir -p build', remote_build_path, p.Results.user, p.Results.ip, p.Results.port, true); + franka_toolbox_ssh_exec(['mkdir -p ' remote_build_path '/build'], ... + p.Results.user, p.Results.ip, p.Results.port, sshOpts); % Configure CMake - cmake_cmd = sprintf('cmake -DCMAKE_BUILD_TYPE=%s -DFranka_DIR="%s" -DFRANKA_FOLDER="libfranka_arm" -DBIN_FOLDER="bin_arm" ..', ... - build_type, remote_franka_dir); - franka_toolbox_remote_system_cmd(cmake_cmd, fullfile(remote_build_path, 'build'), ... - p.Results.user, p.Results.ip, p.Results.port, true); + cmake_cmd = sprintf('cd %s/build && cmake -DCMAKE_BUILD_TYPE=%s -DFranka_DIR="%s" -DFRANKA_FOLDER="libfranka_arm" -DBIN_FOLDER="bin_arm" ..', ... + remote_build_path, build_type, remote_franka_dir); + franka_toolbox_ssh_exec(cmake_cmd, p.Results.user, p.Results.ip, p.Results.port, sshOpts); % Build the project - franka_toolbox_remote_system_cmd('cmake --build . --config Release -j$(nproc)', ... - fullfile(remote_build_path, 'build'), p.Results.user, p.Results.ip, p.Results.port, true); + build_cmd = sprintf('cd %s/build && cmake --build . --config Release -j$(nproc)', remote_build_path); + franka_toolbox_ssh_exec(build_cmd, p.Results.user, p.Results.ip, p.Results.port, sshOpts); % Add executable permissions to the built server - franka_toolbox_remote_system_cmd('chmod +x build/franka_robot_server', ... - remote_build_path, p.Results.user, p.Results.ip, p.Results.port, true); + franka_toolbox_ssh_exec(['chmod +x ' remote_build_path '/build/franka_robot_server'], ... + p.Results.user, p.Results.ip, p.Results.port, sshOpts); if ~isfolder(fullfile(installation_path,'franka_robot_server','bin_arm')) mkdir(fullfile(installation_path,'franka_robot_server','bin_arm')); @@ -98,7 +100,10 @@ function franka_robot_server_build(varargin) unzip(fullfile(installation_path,'franka_robot_server','bin_arm.zip'),fullfile(installation_path,'franka_robot_server')); end - franka_toolbox_foder_from_remote_cp(fullfile('~','franka_matlab','franka_robot_server','build','franka_robot_server'),fullfile(installation_path,'franka_robot_server','bin_arm'),p.Results.user, p.Results.ip, p.Results.port,true); + % Copy built executable from remote + franka_toolbox_scp(':~/franka_matlab/franka_robot_server/build/franka_robot_server', ... + fullfile(installation_path,'franka_robot_server','bin_arm'), ... + p.Results.user, p.Results.ip, p.Results.port, scpOpts); % Replace system tar with MATLAB tar for remote build tar(fullfile(installation_path,'franka_robot_server','bin_arm.tar.gz'), ... @@ -135,25 +140,15 @@ function franka_robot_server_build(varargin) fprintf('Configuring CMake...\n'); cmake_cmd = sprintf('cmake -DCMAKE_BUILD_TYPE=%s -DFranka_DIR="%s" -DFRANKA_FOLDER="libfranka" -DBIN_FOLDER="bin" ..', ... build_type, frankaDir); - [status, cmdout] = franka_toolbox_system_cmd(cmake_cmd, build_dir); - - if status ~= 0 - error('CMake configuration failed:\n%s', cmdout); - end + opts = struct('nothrow', false); + franka_toolbox_local_exec(cmake_cmd, build_dir, opts); % Build the project fprintf('Building project...\n'); - [status, cmdout] = franka_toolbox_system_cmd('cmake --build . --config Release', build_dir); - - if status ~= 0 - error('Build failed:\n%s', cmdout); - end + franka_toolbox_local_exec('cmake --build . --config Release', build_dir, opts); % Add executable permissions to the built server - [status, cmdout] = franka_toolbox_system_cmd('chmod +x franka_robot_server', build_dir); - if status ~= 0 - error('Failed to set executable permissions:\n%s', cmdout); - end + franka_toolbox_local_exec('chmod +x franka_robot_server', build_dir, opts); % Pack if ~isfolder(fullfile(installation_path,'franka_robot_server','bin')) @@ -177,4 +172,4 @@ function franka_robot_server_build(varargin) disp('=== Build completed successfully ==='); -end \ No newline at end of file +end diff --git a/franka_robot_server/include/franka_robot_server/franka_robot_rpc_service.hpp b/franka_robot_server/include/franka_robot_server/franka_robot_rpc_service.hpp index 5090f5b..56c6775 100644 --- a/franka_robot_server/include/franka_robot_server/franka_robot_rpc_service.hpp +++ b/franka_robot_server/include/franka_robot_server/franka_robot_rpc_service.hpp @@ -11,7 +11,8 @@ class FrankaRobotRPCServiceImpl final : public RPCService::Server { public: - FrankaRobotRPCServiceImpl(); + FrankaRobotRPCServiceImpl() = default; + explicit FrankaRobotRPCServiceImpl(uint16_t port) : server_port_(port) {} kj::Promise automaticErrorRecovery( capnp::CallContext context) override; @@ -71,7 +72,34 @@ class FrankaRobotRPCServiceImpl final : public RPCService::Server { kj::Promise vacuumGripperStop( capnp::CallContext context) override; + // Impedance control + kj::Promise setJointImpedance( + capnp::CallContext context) override; + + kj::Promise setCartesianImpedance( + capnp::CallContext context) override; + + // Guiding mode + kj::Promise setGuidingMode( + capnp::CallContext context) override; + + // Frame transformations + kj::Promise setK( + capnp::CallContext context) override; + + kj::Promise setEE( + capnp::CallContext context) override; + + // Robot control + kj::Promise stopRobot( + capnp::CallContext context) override; + + // Health check + kj::Promise ping( + capnp::CallContext context) override; + private: + uint16_t server_port_ = 0; // Stored for ping response std::unique_ptr robot_; std::unique_ptr model_; std::unique_ptr gripper_; diff --git a/franka_robot_server/interface/rpc.capnp b/franka_robot_server/interface/rpc.capnp index 31d8499..f6c0769 100644 --- a/franka_robot_server/interface/rpc.capnp +++ b/franka_robot_server/interface/rpc.capnp @@ -115,4 +115,21 @@ interface RPCService { vacuumGripperVacuum @14 (controlPoint :UInt8, timeout :UInt32, profile :UInt8) -> (success :Bool); vacuumGripperDropOff @15 (timeout :UInt32) -> (success :Bool); vacuumGripperStop @16 () -> (success :Bool); + + # Impedance control + setJointImpedance @19 (kTheta :List(Float64)) -> (success :Bool); # 7 elements + setCartesianImpedance @20 (kX :List(Float64)) -> (success :Bool); # 6 elements + + # Guiding mode + setGuidingMode @21 (guidingMode :List(Bool), elbow :Bool) -> (success :Bool); # 6 booleans + 1 boolean + + # Frame transformations + setK @22 (eeTK :List(Float64)) -> (success :Bool); # 16 elements (4x4 matrix) + setEE @23 (neTEe :List(Float64)) -> (success :Bool); # 16 elements (4x4 matrix) + + # Robot control + stopRobot @24 () -> (success :Bool); + + # Health check - returns server timestamp for liveness verification + ping @25 () -> (timestamp :UInt64, port :UInt16); } \ No newline at end of file diff --git a/franka_robot_server/src/frame_transformations.cpp b/franka_robot_server/src/frame_transformations.cpp new file mode 100644 index 0000000..4191a3a --- /dev/null +++ b/franka_robot_server/src/frame_transformations.cpp @@ -0,0 +1,77 @@ +#include "franka_robot_server/franka_robot_rpc_service.hpp" +#include +#include +#include + +kj::Promise FrankaRobotRPCServiceImpl::setK( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + auto params = context.getParams(); + auto ee_t_k = params.getEeTK(); + + if (ee_t_k.size() != 16) { + KJ_FAIL_REQUIRE("EE_T_K transformation matrix must have exactly 16 elements"); + } + + try { + // Convert to std::array + std::array EE_T_K{}; + for (size_t i = 0; i < 16; i++) { + EE_T_K[i] = ee_t_k[i]; + } + + // Set stiffness frame transformation + robot_->setK(EE_T_K); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to set EE_T_K transformation", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + +kj::Promise FrankaRobotRPCServiceImpl::setEE( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + auto params = context.getParams(); + auto ne_t_ee = params.getNeTEe(); + + if (ne_t_ee.size() != 16) { + KJ_FAIL_REQUIRE("NE_T_EE transformation matrix must have exactly 16 elements"); + } + + try { + // Convert to std::array + std::array NE_T_EE{}; + for (size_t i = 0; i < 16; i++) { + NE_T_EE[i] = ne_t_ee[i]; + } + + // Set end effector frame transformation + robot_->setEE(NE_T_EE); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to set NE_T_EE transformation", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + diff --git a/franka_robot_server/src/franka_robot_rpc_service.cpp b/franka_robot_server/src/franka_robot_rpc_service.cpp index 903aff5..da16df3 100644 --- a/franka_robot_server/src/franka_robot_rpc_service.cpp +++ b/franka_robot_server/src/franka_robot_rpc_service.cpp @@ -3,10 +3,8 @@ #include #include -FrankaRobotRPCServiceImpl::FrankaRobotRPCServiceImpl() { - // Do not initialize robot, model, gripper, or vacuum_gripper here. - // Initialization will occur in initializeRobot(). -} +// Note: Constructor is defaulted or takes port parameter (see header). +// Robot, model, gripper, and vacuum_gripper are initialized via their respective init methods. kj::Promise FrankaRobotRPCServiceImpl::initializeRobot( capnp::CallContext context) { diff --git a/franka_robot_server/src/franka_robot_server.cpp b/franka_robot_server/src/franka_robot_server.cpp index c6e3695..e9db7ac 100644 --- a/franka_robot_server/src/franka_robot_server.cpp +++ b/franka_robot_server/src/franka_robot_server.cpp @@ -18,7 +18,8 @@ int main(int argc, char* argv[]) { int port = std::stoi(argv[2]); // Initialize the RPC server with the provided IP address and port - capnp::EzRpcServer server(kj::heap(), ipAddress, port); + // Pass port to service impl for ping response + capnp::EzRpcServer server(kj::heap(static_cast(port)), ipAddress, port); auto& waitScope = server.getWaitScope(); diff --git a/franka_robot_server/src/guiding_mode.cpp b/franka_robot_server/src/guiding_mode.cpp new file mode 100644 index 0000000..9700a10 --- /dev/null +++ b/franka_robot_server/src/guiding_mode.cpp @@ -0,0 +1,42 @@ +#include "franka_robot_server/franka_robot_rpc_service.hpp" +#include +#include +#include + +kj::Promise FrankaRobotRPCServiceImpl::setGuidingMode( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + auto params = context.getParams(); + auto guiding_mode = params.getGuidingMode(); + bool elbow = params.getElbow(); + + if (guiding_mode.size() != 6) { + KJ_FAIL_REQUIRE("Guiding mode array must have exactly 6 elements"); + } + + try { + // Convert to std::array + std::array guiding_mode_arr{}; + for (size_t i = 0; i < 6; i++) { + guiding_mode_arr[i] = guiding_mode[i]; + } + + // Set guiding mode + robot_->setGuidingMode(guiding_mode_arr, elbow); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to set guiding mode", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + diff --git a/franka_robot_server/src/impedance_control.cpp b/franka_robot_server/src/impedance_control.cpp new file mode 100644 index 0000000..3269ded --- /dev/null +++ b/franka_robot_server/src/impedance_control.cpp @@ -0,0 +1,77 @@ +#include "franka_robot_server/franka_robot_rpc_service.hpp" +#include +#include +#include + +kj::Promise FrankaRobotRPCServiceImpl::setJointImpedance( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + auto params = context.getParams(); + auto k_theta = params.getKTheta(); + + if (k_theta.size() != 7) { + KJ_FAIL_REQUIRE("Joint impedance array must have exactly 7 elements"); + } + + try { + // Convert to std::array + std::array K_theta{}; + for (size_t i = 0; i < 7; i++) { + K_theta[i] = k_theta[i]; + } + + // Set joint impedance + robot_->setJointImpedance(K_theta); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to set joint impedance", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + +kj::Promise FrankaRobotRPCServiceImpl::setCartesianImpedance( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + auto params = context.getParams(); + auto k_x = params.getKX(); + + if (k_x.size() != 6) { + KJ_FAIL_REQUIRE("Cartesian impedance array must have exactly 6 elements"); + } + + try { + // Convert to std::array + std::array K_x{}; + for (size_t i = 0; i < 6; i++) { + K_x[i] = k_x[i]; + } + + // Set Cartesian impedance + robot_->setCartesianImpedance(K_x); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to set Cartesian impedance", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + diff --git a/franka_robot_server/src/ping.cpp b/franka_robot_server/src/ping.cpp new file mode 100644 index 0000000..11560aa --- /dev/null +++ b/franka_robot_server/src/ping.cpp @@ -0,0 +1,19 @@ +#include "franka_robot_server/franka_robot_rpc_service.hpp" +#include + +kj::Promise FrankaRobotRPCServiceImpl::ping( + capnp::CallContext context) { + + auto results = context.getResults(); + + // Get current timestamp in milliseconds since epoch + auto now = std::chrono::system_clock::now(); + auto duration = now.time_since_epoch(); + auto millis = std::chrono::duration_cast(duration).count(); + + results.setTimestamp(static_cast(millis)); + results.setPort(server_port_); + + return kj::READY_NOW; +} + diff --git a/franka_robot_server/src/stop_robot.cpp b/franka_robot_server/src/stop_robot.cpp new file mode 100644 index 0000000..8ff999b --- /dev/null +++ b/franka_robot_server/src/stop_robot.cpp @@ -0,0 +1,27 @@ +#include "franka_robot_server/franka_robot_rpc_service.hpp" +#include +#include + +kj::Promise FrankaRobotRPCServiceImpl::stopRobot( + capnp::CallContext context) { + + if (!robot_) { + KJ_FAIL_REQUIRE("Robot not initialized"); + } + + try { + // Stop all currently running motions + robot_->stop(); + + auto results = context.getResults(); + results.setSuccess(true); + + } catch (const franka::Exception& e) { + KJ_LOG(ERROR, "Failed to stop robot", e.what()); + auto results = context.getResults(); + results.setSuccess(false); + } + + return kj::READY_NOW; +} + diff --git a/franka_toolbox_simulink_library/CMakeLists.txt b/franka_simulink_library/CMakeLists.txt similarity index 100% rename from franka_toolbox_simulink_library/CMakeLists.txt rename to franka_simulink_library/CMakeLists.txt diff --git a/franka_toolbox_simulink_library/blocks/apply_control.m b/franka_simulink_library/blocks/apply_control.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/apply_control.m rename to franka_simulink_library/blocks/apply_control.m diff --git a/franka_toolbox_simulink_library/blocks/apply_control.tlc b/franka_simulink_library/blocks/apply_control.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/apply_control.tlc rename to franka_simulink_library/blocks/apply_control.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_coriolis.m b/franka_simulink_library/blocks/get_coriolis.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_coriolis.m rename to franka_simulink_library/blocks/get_coriolis.m diff --git a/franka_toolbox_simulink_library/blocks/get_coriolis.tlc b/franka_simulink_library/blocks/get_coriolis.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_coriolis.tlc rename to franka_simulink_library/blocks/get_coriolis.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_duration_period.cpp b/franka_simulink_library/blocks/get_duration_period.cpp similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_duration_period.cpp rename to franka_simulink_library/blocks/get_duration_period.cpp diff --git a/franka_toolbox_simulink_library/blocks/get_duration_period.tlc b/franka_simulink_library/blocks/get_duration_period.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_duration_period.tlc rename to franka_simulink_library/blocks/get_duration_period.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_gravity.m b/franka_simulink_library/blocks/get_gravity.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_gravity.m rename to franka_simulink_library/blocks/get_gravity.m diff --git a/franka_toolbox_simulink_library/blocks/get_gravity.tlc b/franka_simulink_library/blocks/get_gravity.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_gravity.tlc rename to franka_simulink_library/blocks/get_gravity.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_gripper_state.cpp b/franka_simulink_library/blocks/get_gripper_state.cpp similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_gripper_state.cpp rename to franka_simulink_library/blocks/get_gripper_state.cpp diff --git a/franka_toolbox_simulink_library/blocks/get_gripper_state.tlc b/franka_simulink_library/blocks/get_gripper_state.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_gripper_state.tlc rename to franka_simulink_library/blocks/get_gripper_state.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_jacobian.m b/franka_simulink_library/blocks/get_jacobian.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_jacobian.m rename to franka_simulink_library/blocks/get_jacobian.m diff --git a/franka_toolbox_simulink_library/blocks/get_jacobian.tlc b/franka_simulink_library/blocks/get_jacobian.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_jacobian.tlc rename to franka_simulink_library/blocks/get_jacobian.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_mass.m b/franka_simulink_library/blocks/get_mass.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_mass.m rename to franka_simulink_library/blocks/get_mass.m diff --git a/franka_toolbox_simulink_library/blocks/get_mass.tlc b/franka_simulink_library/blocks/get_mass.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_mass.tlc rename to franka_simulink_library/blocks/get_mass.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_pose.m b/franka_simulink_library/blocks/get_pose.m similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_pose.m rename to franka_simulink_library/blocks/get_pose.m diff --git a/franka_toolbox_simulink_library/blocks/get_pose.tlc b/franka_simulink_library/blocks/get_pose.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_pose.tlc rename to franka_simulink_library/blocks/get_pose.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_robot_state.cpp b/franka_simulink_library/blocks/get_robot_state.cpp similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_robot_state.cpp rename to franka_simulink_library/blocks/get_robot_state.cpp diff --git a/franka_toolbox_simulink_library/blocks/get_robot_state.tlc b/franka_simulink_library/blocks/get_robot_state.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_robot_state.tlc rename to franka_simulink_library/blocks/get_robot_state.tlc diff --git a/franka_toolbox_simulink_library/blocks/get_vacuum_gripper_state.cpp b/franka_simulink_library/blocks/get_vacuum_gripper_state.cpp similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_vacuum_gripper_state.cpp rename to franka_simulink_library/blocks/get_vacuum_gripper_state.cpp diff --git a/franka_toolbox_simulink_library/blocks/get_vacuum_gripper_state.tlc b/franka_simulink_library/blocks/get_vacuum_gripper_state.tlc similarity index 100% rename from franka_toolbox_simulink_library/blocks/get_vacuum_gripper_state.tlc rename to franka_simulink_library/blocks/get_vacuum_gripper_state.tlc diff --git a/franka_toolbox_simulink_library/blocks/local_utils.cpp b/franka_simulink_library/blocks/local_utils.cpp similarity index 100% rename from franka_toolbox_simulink_library/blocks/local_utils.cpp rename to franka_simulink_library/blocks/local_utils.cpp diff --git a/franka_toolbox_simulink_library/blocks/local_utils.h b/franka_simulink_library/blocks/local_utils.h similarity index 100% rename from franka_toolbox_simulink_library/blocks/local_utils.h rename to franka_simulink_library/blocks/local_utils.h diff --git a/franka_toolbox_simulink_library/blocks/makecfg.m b/franka_simulink_library/blocks/makecfg.m similarity index 97% rename from franka_toolbox_simulink_library/blocks/makecfg.m rename to franka_simulink_library/blocks/makecfg.m index d430b84..11c235c 100644 --- a/franka_toolbox_simulink_library/blocks/makecfg.m +++ b/franka_simulink_library/blocks/makecfg.m @@ -63,7 +63,7 @@ function makecfg(objBuildInfo) if rt_main_src_idx objBuildInfo.Src.Files(rt_main_src_idx) = []; end - addSourceFiles(objBuildInfo, 'rt_main.cpp', fullfile(franka_toolbox_installation_path_get(),'franka_toolbox_simulink_library','rtw','src')); + addSourceFiles(objBuildInfo, 'rt_main.cpp', fullfile(franka_toolbox_installation_path_get(),'franka_simulink_library','rtw','src')); end addIncludePaths(objBuildInfo,... diff --git a/franka_toolbox_simulink_library/franka_toolbox_simulink_library.slx b/franka_simulink_library/franka_simulink_library.slx similarity index 100% rename from franka_toolbox_simulink_library/franka_toolbox_simulink_library.slx rename to franka_simulink_library/franka_simulink_library.slx diff --git a/franka_toolbox_simulink_library/franka_toolbox_simulink_library_mex.m b/franka_simulink_library/franka_simulink_library_mex.m similarity index 79% rename from franka_toolbox_simulink_library/franka_toolbox_simulink_library_mex.m rename to franka_simulink_library/franka_simulink_library_mex.m index 0159686..ad2e3f4 100644 --- a/franka_toolbox_simulink_library/franka_toolbox_simulink_library_mex.m +++ b/franka_simulink_library/franka_simulink_library_mex.m @@ -1,4 +1,4 @@ -function franka_toolbox_simulink_library_mex() +function franka_simulink_library_mex() % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved % This file is subject to the terms and conditions defined in the file % 'LICENSE' , which is part of this package @@ -7,7 +7,7 @@ function franka_toolbox_simulink_library_mex() installation_path = franka_toolbox_installation_path_get(); % Set paths - simulink_lib_path = fullfile(installation_path, 'franka_toolbox_simulink_library'); + simulink_lib_path = fullfile(installation_path, 'franka_simulink_library'); build_dir = fullfile(simulink_lib_path, 'build'); target_dir = fullfile(simulink_lib_path, 'bin'); bin_zip = fullfile(simulink_lib_path, 'bin.zip'); @@ -31,35 +31,24 @@ function franka_toolbox_simulink_library_mex() try % Configure CMake with verbose output disp('Configuring CMake...'); + opts = struct('nothrow', false); if ispc % For Windows, specify Visual Studio generator with verbose output - [status, output] = franka_toolbox_system_cmd('cmake -G "Visual Studio 17 2022" -A x64 ..', build_dir); - if status ~= 0 - error('CMake configuration failed:\n%s', output); - end + [~, output] = franka_toolbox_local_exec('cmake -G "Visual Studio 17 2022" -A x64 ..', build_dir, opts); disp(output); % Build using CMake with verbose output disp('Building library...'); - [status, output] = franka_toolbox_system_cmd('cmake --build . --config Release --verbose', build_dir); - if status ~= 0 - error('Build failed:\n%s', output); - end + [~, output] = franka_toolbox_local_exec('cmake --build . --config Release --verbose', build_dir, opts); disp(output); else % For Unix systems (Linux/macOS) - [status, output] = franka_toolbox_system_cmd('cmake ..', build_dir); - if status ~= 0 - error('CMake configuration failed:\n%s', output); - end + [~, output] = franka_toolbox_local_exec('cmake ..', build_dir, opts); disp(output); % Build using CMake with verbose output disp('Building library...'); - [status, output] = franka_toolbox_system_cmd('cmake --build . --verbose', build_dir); - if status ~= 0 - error('Build failed:\n%s', output); - end + [~, output] = franka_toolbox_local_exec('cmake --build . --verbose', build_dir, opts); disp(output); end diff --git a/franka_toolbox_simulink_library/rtw/src/rt_main.cpp b/franka_simulink_library/rtw/src/rt_main.cpp similarity index 100% rename from franka_toolbox_simulink_library/rtw/src/rt_main.cpp rename to franka_simulink_library/rtw/src/rt_main.cpp diff --git a/franka_toolbox_simulink_library/scripts/FrankaGripperControlModes.m b/franka_simulink_library/scripts/FrankaGripperControlModes.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaGripperControlModes.m rename to franka_simulink_library/scripts/FrankaGripperControlModes.m diff --git a/franka_toolbox_simulink_library/scripts/FrankaRobotCollisionThresholds.m b/franka_simulink_library/scripts/FrankaRobotCollisionThresholds.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaRobotCollisionThresholds.m rename to franka_simulink_library/scripts/FrankaRobotCollisionThresholds.m diff --git a/franka_toolbox_simulink_library/scripts/FrankaRobotControlModes.m b/franka_simulink_library/scripts/FrankaRobotControlModes.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaRobotControlModes.m rename to franka_simulink_library/scripts/FrankaRobotControlModes.m diff --git a/franka_toolbox_simulink_library/scripts/FrankaRobotLoadInertia.m b/franka_simulink_library/scripts/FrankaRobotLoadInertia.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaRobotLoadInertia.m rename to franka_simulink_library/scripts/FrankaRobotLoadInertia.m diff --git a/franka_toolbox_simulink_library/scripts/FrankaRobotSettings.m b/franka_simulink_library/scripts/FrankaRobotSettings.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaRobotSettings.m rename to franka_simulink_library/scripts/FrankaRobotSettings.m diff --git a/franka_toolbox_simulink_library/scripts/FrankaVacuumGripperControlModes.m b/franka_simulink_library/scripts/FrankaVacuumGripperControlModes.m similarity index 100% rename from franka_toolbox_simulink_library/scripts/FrankaVacuumGripperControlModes.m rename to franka_simulink_library/scripts/FrankaVacuumGripperControlModes.m diff --git a/franka_toolbox_simulink_library/slblocks.m b/franka_simulink_library/slblocks.m similarity index 85% rename from franka_toolbox_simulink_library/slblocks.m rename to franka_simulink_library/slblocks.m index 864ebcd..390b43c 100644 --- a/franka_toolbox_simulink_library/slblocks.m +++ b/franka_simulink_library/slblocks.m @@ -2,7 +2,7 @@ % Copyright (c) 2023 Franka Robotics GmbH - All Rights Reserved % This file is subject to the terms and conditions defined in the file % 'LICENSE' , which is part of this package -Browser.Library = 'franka_toolbox_simulink_library'; +Browser.Library = 'franka_simulink_library'; Browser.Name = 'Franka Robotics Simulink Support Package'; blkStruct.Browser = Browser; end \ No newline at end of file diff --git a/franka_toolbox.prj b/franka_toolbox.prj index 3866e28..c9e2f4e 100644 --- a/franka_toolbox.prj +++ b/franka_toolbox.prj @@ -1,5 +1,5 @@ - + Franka Toolbox for MATLAB Franka Robotics GmbH info@franka.de @@ -7,7 +7,7 @@ The Franka Toolbox for MATLAB contains a set of Matlab & Simulink libraries for interfacing the Franka Robot. ${PROJECT_ROOT}/data/images/franka_robot_no_ee_4_3.png - 3.1.0 + 4.0.0 ${PROJECT_ROOT}/Franka Toolbox for MATLAB.mltbx MATLAB @@ -22,10 +22,10 @@ 14 - 24.1 - 24.1 - 24.1 - 24.1 + 9.14 + 10.7 + 5.6 + 9.9 36861653-1081-46df-aedd-6ae570ddebac @@ -92,31 +92,33 @@ franka_toolbox_dist_make.m ${PROJECT_ROOT} + ${PROJECT_ROOT}/.dockerignore + ${PROJECT_ROOT}/.gitattributes ${PROJECT_ROOT}/.github - ${PROJECT_ROOT}/LICENCE + ${PROJECT_ROOT}/LICENSE ${PROJECT_ROOT}/README.md ${PROJECT_ROOT}/common ${PROJECT_ROOT}/config ${PROJECT_ROOT}/data ${PROJECT_ROOT}/dependencies - ${PROJECT_ROOT}/dist ${PROJECT_ROOT}/doc + ${PROJECT_ROOT}/docker ${PROJECT_ROOT}/docs ${PROJECT_ROOT}/examples ${PROJECT_ROOT}/franka_robot ${PROJECT_ROOT}/franka_robot_server - ${PROJECT_ROOT}/franka_toolbox_simulink_library + ${PROJECT_ROOT}/franka_simulink_library ${PROJECT_ROOT}/scripts - /home/kvasios/franka_matlab/Franka Toolbox for MATLAB.mltbx + /home/kvasios/franka_toolbox_for_matlab/Franka Toolbox for MATLAB.mltbx - /usr/local/MATLAB/R2024a + /usr/local/MATLAB/R2023a diff --git a/franka_toolbox_dist_make.m b/franka_toolbox_dist_make.m index 8d6d41a..5050a62 100644 --- a/franka_toolbox_dist_make.m +++ b/franka_toolbox_dist_make.m @@ -1,20 +1,58 @@ -function franka_toolbox_dist_make() - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package +function franka_toolbox_dist_make(options) + %FRANKA_TOOLBOX_DIST_MAKE Create distribution package for Franka Toolbox + % + % franka_toolbox_dist_make() - CI-safe packaging mode: preserve generated artifacts + % franka_toolbox_dist_make('Mode', 'local') - Local packaging with git clean + % franka_toolbox_dist_make('Mode', 'ci', 'OutputName', 'franka') - Specify output name + % + % Options: + % Mode - 'ci' (default) or 'local' + % OutputName - Base name for output file (default: 'franka') + % Output will be: dist/.mltbx + % + % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE' , which is part of this package + + arguments + options.Mode {mustBeMember(options.Mode, {'local', 'ci'})} = 'ci' + options.OutputName {mustBeTextScalar} = 'franka' + end + + ci_mode = strcmp(options.Mode, 'ci'); + output_name = options.OutputName; %% Clean-up env - rm_dir('dist'); - addpath(genpath('../franka_matlab')); + % In CI mode, preserve existing .mltbx files (for multi-package builds) + if ci_mode && exist('dist', 'dir') + fprintf('CI mode: preserving existing .mltbx files in dist/\n'); + % Only remove franka_matlab subfolder, keep .mltbx files + rm_dir(fullfile('dist', 'franka_matlab')); + else + rm_dir('dist'); + end + + % Add parent franka_matlab if it exists (for local development) + parent_franka_matlab = '../franka_matlab'; + has_parent_franka = exist(parent_franka_matlab, 'dir'); + if has_parent_franka + addpath(genpath(parent_franka_matlab)); + end % Get the current directory as project root (since this script is in the root) project_root = pwd; % Check if we're in a git repository before running git clean - if exist(fullfile(project_root, '.git'), 'dir') + % Skip in CI mode to preserve downloaded artifacts + if ~ci_mode && exist(fullfile(project_root, '.git'), 'dir') + fprintf('Running git clean (local mode)...\n'); system(['cd ',project_root,' && git clean -ffxd']); else - fprintf('Not in a git repository, skipping git clean\n'); + if ci_mode + fprintf('CI mode: skipping git clean to preserve artifacts\n'); + else + fprintf('Not in a git repository, skipping git clean\n'); + end end %% Copy the Project @@ -34,24 +72,31 @@ function franka_toolbox_dist_make() rm_dir(fullfile(target_dir,'cmake')); rm_dir(fullfile(target_dir,'libfranka')); rm_dir(fullfile(target_dir,'libfranka_arm')); - delete(fullfile(target_dir,'.gitignore')); - delete(fullfile(target_dir,'CHANGELOG.md')); - delete(fullfile(target_dir,'README.md')); - delete(fullfile(target_dir,'LICENCE')); - delete(fullfile(target_dir,'franka_toolbox_dist_make.m')); + rm_dir(fullfile(target_dir,'docker')); + rm_dir(fullfile(target_dir,'.github')); + safe_delete(fullfile(target_dir,'.gitignore')); + safe_delete(fullfile(target_dir,'CHANGELOG.md')); + safe_delete(fullfile(target_dir,'README.md')); + safe_delete(fullfile(target_dir,'LICENCE')); + safe_delete(fullfile(target_dir,'LICENSE')); + safe_delete(fullfile(target_dir,'franka_toolbox_dist_make.m')); remove_all_files_of_type_recursively('.asv',target_dir,{''}); - %% Remove build artifacts (duplicate line removed) - %% Make the Franka Toolbox for MATLAB addpath(genpath(fullfile(project_root,'dist'))); - rmpath(genpath('../franka_matlab')); + if has_parent_franka + rmpath(genpath(parent_franka_matlab)); + end addpath(genpath(fullfile(project_root,'dist'))); - matlab.addons.toolbox.packageToolbox(fullfile(target_dir,'franka_toolbox.prj'),fullfile(project_root,'dist','franka')) + fprintf('Packaging toolbox as %s.mltbx...\n', output_name); + matlab.addons.toolbox.packageToolbox(fullfile(target_dir,'franka_toolbox.prj'),fullfile(project_root,'dist',output_name)) + fprintf('Toolbox packaged successfully: dist/%s.mltbx\n', output_name); - addpath(genpath('../franka_matlab')); + if has_parent_franka + addpath(genpath(parent_franka_matlab)); + end rmpath(genpath(fullfile(project_root,'dist'))); end @@ -90,6 +135,12 @@ function rm_dir(dir) end end +function safe_delete(filepath) +if exist(filepath,'file') + delete(filepath); +end +end + function files = find_all_files_of_type_in_directory_recursively(file_type,directory) files = dir(fullfile(directory,'**',['*',file_type])); end diff --git a/franka_toolbox_simulink_library/bin.zip b/franka_toolbox_simulink_library/bin.zip deleted file mode 100644 index 9e7ca9e..0000000 Binary files a/franka_toolbox_simulink_library/bin.zip and /dev/null differ diff --git a/generate_cartesian_pose_motion b/generate_cartesian_pose_motion deleted file mode 100755 index fd5d5cd..0000000 Binary files a/generate_cartesian_pose_motion and /dev/null differ diff --git a/scripts/binaries_build/franka_toolbox_binaries_all_build.m b/scripts/binaries_build/franka_toolbox_binaries_all_build.m index acd630d..47fe000 100644 --- a/scripts/binaries_build/franka_toolbox_binaries_all_build.m +++ b/scripts/binaries_build/franka_toolbox_binaries_all_build.m @@ -1,27 +1,103 @@ -function franka_toolbox_binaries_all_build(user,ip,port) - % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package +function franka_toolbox_binaries_all_build(varargin) + %FRANKA_TOOLBOX_BINARIES_ALL_BUILD Build all Franka Toolbox binaries + % + % franka_toolbox_binaries_all_build() + % Build host MEX files. On Linux, also builds target binaries using Docker. + % + % franka_toolbox_binaries_all_build('docker') + % Build host MEX files and target binaries using Docker (Linux only). + % + % franka_toolbox_binaries_all_build(user, ip, port) + % Legacy mode: Build host MEX files and target binaries using remote Jetson. + % + % This function orchestrates the complete build process: + % 1. Builds Simulink S-function MEX files (host) + % 2. Builds MATLAB FrankaRobot MEX files (host) + % 3. On Linux: Builds target server binaries (using Docker or remote) + % + % Docker Build (Recommended): + % - Builds both x86_64 and ARM64 target binaries locally + % - No remote Jetson machine required + % - Requires Docker to be installed and running + % + % Legacy Remote Build: + % - Requires SSH access to a Jetson device + % - Builds ARM64 binaries on the remote machine + % + % After building, run franka_toolbox_dist_make() for packaging. + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package - % This needs to be run x1 by Ubuntu Host PC - % with Jetson connected and user,ip, port provided - % and 1x with Windows Host PC (no Jetson connected) - % run franka_toolbox_dist_make(); when done for packaging the - % distribution + % Parse arguments + use_docker = true; % Default to Docker + user = ''; + ip = ''; + port = '22'; + + if nargin == 1 + if ischar(varargin{1}) && strcmpi(varargin{1}, 'docker') + use_docker = true; + elseif islogical(varargin{1}) + use_docker = varargin{1}; + else + error('Invalid argument. Use ''docker'' or franka_toolbox_binaries_all_build(user, ip, port)'); + end + elseif nargin == 3 + use_docker = false; + user = varargin{1}; + ip = varargin{2}; + port = varargin{3}; + elseif nargin > 0 && nargin ~= 3 + error('Invalid number of arguments. Use franka_toolbox_binaries_all_build() or franka_toolbox_binaries_all_build(user, ip, port)'); + end - % Build Simulink & MATLAB libs - franka_toolbox_simulink_library_mex(); + fprintf('\n'); + fprintf('==============================================\n'); + fprintf('Franka Toolbox Build - All Binaries\n'); + fprintf('==============================================\n'); + fprintf('\n'); + + % Build Simulink & MATLAB libs (host) + fprintf('=== Building Host MEX Files ===\n\n'); + + fprintf('Building Simulink library MEX files...\n'); + franka_simulink_library_mex(); + + fprintf('\nBuilding FrankaRobot MEX files...\n'); franka_robot_mex(); - % Additionally if in linux build targets & dependencies - + % Build target binaries (Linux only) if isunix() - franka_toolbox_binaries_target_local_build(); - - if nargin == 3 - franka_toolbox_binaries_target_remote_build(user,ip,port); - end + fprintf('\n=== Building Target Binaries ===\n\n'); + if use_docker + fprintf('Using Docker for target builds (both amd64 and arm64)...\n\n'); + franka_toolbox_binaries_target_docker_build('all'); + else + % Legacy mode with remote Jetson + fprintf('Building local x86_64 target...\n'); + franka_toolbox_binaries_target_local_build(false); + + if ~isempty(user) && ~isempty(ip) + fprintf('\nBuilding remote ARM64 target...\n'); + franka_toolbox_binaries_target_remote_build(user, ip, port, false); + end + end + else + fprintf('\n'); + fprintf('Note: Target binaries can only be built on Linux.\n'); + fprintf('Run this function on a Linux machine to build target binaries,\n'); + fprintf('or use the Docker build script directly.\n'); end + fprintf('\n'); + fprintf('==============================================\n'); + fprintf('Build Complete!\n'); + fprintf('==============================================\n'); + fprintf('\n'); + fprintf('Next step: Run franka_toolbox_dist_make() to create the distribution package.\n'); + fprintf('\n'); + end \ No newline at end of file diff --git a/scripts/binaries_build/franka_toolbox_binaries_target_docker_build.m b/scripts/binaries_build/franka_toolbox_binaries_target_docker_build.m new file mode 100644 index 0000000..f959c6e --- /dev/null +++ b/scripts/binaries_build/franka_toolbox_binaries_target_docker_build.m @@ -0,0 +1,128 @@ +function franka_toolbox_binaries_target_docker_build(arch) + %FRANKA_TOOLBOX_BINARIES_TARGET_DOCKER_BUILD Build target binaries using Docker + % + % franka_toolbox_binaries_target_docker_build() - Build for both architectures + % franka_toolbox_binaries_target_docker_build('amd64') - Build for x86_64 only + % franka_toolbox_binaries_target_docker_build('arm64') - Build for ARM64 only + % franka_toolbox_binaries_target_docker_build('all') - Build for both architectures + % + % This function uses Docker containers to build the target binaries + % (franka_robot_server and common library) for Linux targets. + % + % Output files: + % - common/bin.zip and common/bin_arm.zip + % - franka_robot_server/bin.tar.gz and franka_robot_server/bin_arm.tar.gz + % - dependencies/libfranka.zip and dependencies/libfranka_arm.zip + % + % Prerequisites: + % - Docker must be installed and running + % - On Linux, user must be in docker group or use sudo + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package + + if nargin < 1 + arch = 'all'; + end + + % Validate architecture argument + valid_archs = {'amd64', 'arm64', 'all'}; + if ~ismember(arch, valid_archs) + error('Invalid architecture: %s. Must be one of: %s', arch, strjoin(valid_archs, ', ')); + end + + % Get installation path + installation_path = franka_toolbox_installation_path_get(); + docker_dir = fullfile(installation_path, 'docker'); + build_script = fullfile(docker_dir, 'build.sh'); + + % Check Docker is available + fprintf('Checking Docker availability...\n'); + [status, ~] = system('docker --version'); + if status ~= 0 + error(['Docker is not installed or not in PATH.\n' ... + 'Please install Docker and ensure it is running.']); + end + + [status, ~] = system('docker info'); + if status ~= 0 + error(['Docker daemon is not running.\n' ... + 'Please start the Docker service.']); + end + + % Check build script exists + if ~isfile(build_script) + error('Docker build script not found at: %s', build_script); + end + + % Make build script executable (Unix only) + if isunix() + system(['chmod +x "', build_script, '"']); + % Also make all scripts executable + system(['chmod +x "', fullfile(docker_dir, 'scripts', '*.sh'), '"']); + end + + % Read libfranka version + libfranka_ver = readcell(fullfile(installation_path, 'config', 'libfranka_ver.csv')); + libfranka_ver = libfranka_ver{1}; + + fprintf('\n'); + fprintf('==============================================\n'); + fprintf('Franka Toolbox Docker Build\n'); + fprintf('==============================================\n'); + fprintf('Architecture: %s\n', arch); + fprintf('Libfranka Version: %s\n', libfranka_ver); + fprintf('Docker Directory: %s\n', docker_dir); + fprintf('==============================================\n'); + fprintf('\n'); + + % Build command + cmd = sprintf('cd "%s" && bash build.sh %s --libfranka %s', ... + docker_dir, arch, libfranka_ver); + + fprintf('Running Docker build...\n'); + fprintf('Command: %s\n\n', cmd); + + % Execute build + [status, output] = system(cmd, '-echo'); + + if status ~= 0 + error('Docker build failed with exit code %d:\n%s', status, output); + end + + fprintf('\n'); + fprintf('==============================================\n'); + fprintf('Build completed successfully!\n'); + fprintf('==============================================\n'); + + % List output files + fprintf('\nOutput files:\n'); + + if strcmp(arch, 'amd64') || strcmp(arch, 'all') + fprintf('\n x86_64 (amd64):\n'); + check_and_print_file(fullfile(installation_path, 'common', 'bin.zip')); + check_and_print_file(fullfile(installation_path, 'franka_robot_server', 'bin.tar.gz')); + check_and_print_file(fullfile(installation_path, 'dependencies', 'libfranka.zip')); + end + + if strcmp(arch, 'arm64') || strcmp(arch, 'all') + fprintf('\n ARM64 (arm64):\n'); + check_and_print_file(fullfile(installation_path, 'common', 'bin_arm.zip')); + check_and_print_file(fullfile(installation_path, 'franka_robot_server', 'bin_arm.tar.gz')); + check_and_print_file(fullfile(installation_path, 'dependencies', 'libfranka_arm.zip')); + end + + fprintf('\n'); +end + +function check_and_print_file(filepath) + if isfile(filepath) + info = dir(filepath); + fprintf(' ✓ %s (%.1f KB)\n', filepath, info.bytes/1024); + else + fprintf(' ✗ %s (not found)\n', filepath); + end +end + + diff --git a/scripts/binaries_build/franka_toolbox_binaries_target_local_build.m b/scripts/binaries_build/franka_toolbox_binaries_target_local_build.m index d3f8f33..eac6072 100644 --- a/scripts/binaries_build/franka_toolbox_binaries_target_local_build.m +++ b/scripts/binaries_build/franka_toolbox_binaries_target_local_build.m @@ -1,5 +1,28 @@ -function franka_toolbox_binaries_target_local_build() - +function franka_toolbox_binaries_target_local_build(use_docker) + %FRANKA_TOOLBOX_BINARIES_TARGET_LOCAL_BUILD Build target binaries for local x86_64 Linux + % + % franka_toolbox_binaries_target_local_build() - Build using Docker (recommended) + % franka_toolbox_binaries_target_local_build(true) - Build using Docker + % franka_toolbox_binaries_target_local_build(false) - Build natively (legacy) + % + % This function builds the target binaries (franka_robot_server and + % common library) for the local Linux x86_64 system. + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + + if nargin < 1 + use_docker = true; % Default to Docker build + end + + if use_docker + fprintf('Building target binaries using Docker...\n'); + franka_toolbox_binaries_target_docker_build('amd64'); + return; + end + + % Legacy native build (requires local dependencies) + fprintf('Building target binaries natively (legacy mode)...\n'); + libfranka_ver = readcell('libfranka_ver.csv'); libfranka_ver = libfranka_ver{1}; @@ -9,10 +32,12 @@ function franka_toolbox_binaries_target_local_build() franka_toolbox_libfranka_build(libfranka_ver,true); % common - franka_toolbox_common_build(); + franka_common_build(); % FrankaRobot() server franka_robot_server_build(); + else + error('Native build is only supported on Unix systems. Use Docker instead.'); end end \ No newline at end of file diff --git a/scripts/binaries_build/franka_toolbox_binaries_target_remote_build.m b/scripts/binaries_build/franka_toolbox_binaries_target_remote_build.m index 147ca32..63f6b92 100644 --- a/scripts/binaries_build/franka_toolbox_binaries_target_remote_build.m +++ b/scripts/binaries_build/franka_toolbox_binaries_target_remote_build.m @@ -1,7 +1,48 @@ -function franka_toolbox_binaries_target_remote_build(user,ip,port) - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package +function franka_toolbox_binaries_target_remote_build(user, ip, port, use_docker) + %FRANKA_TOOLBOX_BINARIES_TARGET_REMOTE_BUILD Build target binaries for ARM64 (Jetson) + % + % franka_toolbox_binaries_target_remote_build() - Build using Docker (recommended) + % franka_toolbox_binaries_target_remote_build(user, ip, port) - Build on remote machine (legacy) + % franka_toolbox_binaries_target_remote_build(user, ip, port, false) - Build on remote machine (legacy) + % franka_toolbox_binaries_target_remote_build('', '', '', true) - Build using Docker + % + % For Docker builds, the user/ip/port arguments are ignored. + % Docker cross-compilation builds ARM64 binaries locally without needing + % a remote Jetson machine. + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package + + % Determine if Docker should be used + if nargin == 0 + % No arguments = use Docker + use_docker = true; + elseif nargin == 1 && islogical(user) + % Single logical argument = docker flag + use_docker = user; + elseif nargin < 4 + % Legacy call with user/ip/port but no docker flag + use_docker = false; + end + + if use_docker + fprintf('Building ARM64 target binaries using Docker cross-compilation...\n'); + fprintf('Note: No remote machine required - building locally in Docker.\n\n'); + franka_toolbox_binaries_target_docker_build('arm64'); + return; + end + + % Legacy remote build (requires SSH access to Jetson) + if nargin < 3 || isempty(user) || isempty(ip) + error(['Remote build requires user, ip, and port arguments.\n' ... + 'Usage: franka_toolbox_binaries_target_remote_build(user, ip, port)\n' ... + 'Or use Docker build: franka_toolbox_binaries_target_remote_build()']); + end + + if nargin < 3 || isempty(port) + port = '22'; + end fprintf('Starting remote build process...\n'); fprintf('Target: %s@%s:%s\n', user, ip, port); @@ -17,7 +58,7 @@ function franka_toolbox_binaries_target_remote_build(user,ip,port) % common fprintf('\n=== Building common components ===\n'); - franka_toolbox_common_build(user,ip,port); + franka_common_build(user,ip,port); % FrankaRobot() server fprintf('\n=== Building FrankaRobot server ===\n'); diff --git a/scripts/binaries_build/franka_toolbox_libfranka_build.m b/scripts/binaries_build/franka_toolbox_libfranka_build.m index 98f2713..5a49c73 100644 --- a/scripts/binaries_build/franka_toolbox_libfranka_build.m +++ b/scripts/binaries_build/franka_toolbox_libfranka_build.m @@ -54,19 +54,22 @@ libfranka_path = fullfile(installation_path, folder_name); libfranka_build_path = fullfile(libfranka_path,'build'); - franka_toolbox_system_cmd(['git clone --recursive https://github.com/frankarobotics/libfranka ',folder_name],installation_path,true); - franka_toolbox_system_cmd(['git checkout ',libfranka_version],libfranka_path,true); - franka_toolbox_system_cmd('git submodule update',libfranka_path,true); + + % Execute git commands with verbose output + opts = struct('verbose', true, 'nothrow', false); + franka_toolbox_local_exec(['git clone --recursive https://github.com/frankarobotics/libfranka ', folder_name], installation_path, opts); + franka_toolbox_local_exec(['git checkout ', libfranka_version], libfranka_path, opts); + franka_toolbox_local_exec('git submodule update', libfranka_path, opts); % Check if build directory exists and remove it if exist(libfranka_build_path, 'dir') rmdir(libfranka_build_path, 's'); end - franka_toolbox_system_cmd('mkdir build',libfranka_path); + franka_toolbox_local_exec('mkdir build', libfranka_path); if ~clone_only - franka_toolbox_system_cmd('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_PREFIX_PATH="/opt/openrobots/lib/cmake" ..',libfranka_build_path,true); - franka_toolbox_system_cmd('cmake --build .',libfranka_build_path,true); + franka_toolbox_local_exec('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_PREFIX_PATH="/opt/openrobots/lib/cmake" ..', libfranka_build_path, opts); + franka_toolbox_local_exec('cmake --build .', libfranka_build_path, opts); % libfranka runtime dependencies bundle franka_toolbox_libfranka_deps_bundle(); @@ -75,4 +78,4 @@ franka_toolbox_libfranka_pack(); end -end \ No newline at end of file +end diff --git a/scripts/binaries_build/franka_toolbox_libfranka_remote_build.m b/scripts/binaries_build/franka_toolbox_libfranka_remote_build.m index ad8bc6d..ea3828e 100644 --- a/scripts/binaries_build/franka_toolbox_libfranka_remote_build.m +++ b/scripts/binaries_build/franka_toolbox_libfranka_remote_build.m @@ -34,23 +34,27 @@ franka_toolbox_libfranka_build(libfranka_version,true,true,'libfranka_arm'); fprintf('Cleaning remote libfranka directory...\n'); - franka_toolbox_remote_system_cmd('rm -rf libfranka','~',user,ip,port,true); + sshOpts = struct('verbose', true, 'nothrow', false); + franka_toolbox_ssh_exec('rm -rf ~/libfranka', user, ip, port, sshOpts); fprintf('Copying libfranka to remote machine...\n'); - franka_toolbox_foder_remote_cp(['"',libfranka_path,'"'],user,ip,'~/libfranka',port,true); + scpOpts = struct('recursive', true, 'verbose', true, 'nothrow', false); + franka_toolbox_scp(libfranka_path, ':~/libfranka', user, ip, port, scpOpts); fprintf('Running CMake configuration on remote machine...\n'); - franka_toolbox_remote_system_cmd('cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_PREFIX_PATH="/opt/openrobots/lib/cmake" ..','~/libfranka/build',user,ip,port,true); + cmake_cmd = 'cd ~/libfranka/build && cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DCMAKE_PREFIX_PATH="/opt/openrobots/lib/cmake" ..'; + franka_toolbox_ssh_exec(cmake_cmd, user, ip, port, sshOpts); fprintf('Building libfranka on remote machine...\n'); - franka_toolbox_remote_system_cmd('cmake --build .','~/libfranka/build',user,ip,port,true); + franka_toolbox_ssh_exec('cd ~/libfranka/build && cmake --build .', user, ip, port, sshOpts); fprintf('Cleaning up local libfranka_arm directory...\n'); rmdir(libfranka_path,'s'); end fprintf('Copying built libfranka from remote machine...\n'); - franka_toolbox_foder_from_remote_cp(fullfile('~','libfranka'),['"',libfranka_path,'"'],user,ip,port,true); + scpOpts = struct('recursive', true, 'verbose', true, 'nothrow', false); + franka_toolbox_scp(':~/libfranka', libfranka_path, user, ip, port, scpOpts); fprintf('Bundling libfranka runtime dependencies...\n'); franka_toolbox_libfranka_deps_bundle(user,ip,port); diff --git a/scripts/franka_toolbox_install.m b/scripts/franka_toolbox_install.m index 30fac5a..f92613a 100644 --- a/scripts/franka_toolbox_install.m +++ b/scripts/franka_toolbox_install.m @@ -3,7 +3,7 @@ function franka_toolbox_install() % This function handles the complete installation process of the Franka Toolbox, % including binary unpacking and Simulink configuration. % - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved + % Copyright (c) 2026 Franka Robotics GmbH - All Rights Reserved % This file is subject to the terms and conditions defined in the file % 'LICENSE', which is part of this package @@ -16,6 +16,14 @@ function franka_toolbox_install() % Remove any existing installation franka_toolbox_uninstall(); + binaryGroups = getBinaryGroups(); + if hasMissingBinaries(binaryGroups) + warning('franka_toolbox_install:MissingBinaries', '%s', ... + getMissingBinariesWarningMessage(binaryGroups)); + fprintf('\nFranka Toolbox installation unsuccessful or partially incomplete\n\n'); + return; + end + % Extract binary files unpackBinaries(); @@ -34,29 +42,161 @@ function unpackBinaries() installation_path = franka_toolbox_installation_path_get(); % Unzip Simulink binaries - franka_toolbox_simulink_library = fullfile(installation_path, 'franka_toolbox_simulink_library'); - unzip(fullfile(franka_toolbox_simulink_library,'bin.zip'), ... - fullfile(franka_toolbox_simulink_library,'blocks')); + franka_simulink_library = fullfile(installation_path, 'franka_simulink_library'); + tryUnzip(fullfile(franka_simulink_library,'bin.zip'), ... + fullfile(franka_simulink_library,'blocks')); % Unpack common binaries - unzip(fullfile(installation_path, 'common', 'bin.zip'), ... - fullfile(installation_path, 'common')); - unzip(fullfile(installation_path, 'common', 'bin_arm.zip'), ... - fullfile(installation_path, 'common')); + tryUnzip(fullfile(installation_path, 'common', 'bin.zip'), ... + fullfile(installation_path, 'common')); + tryUnzip(fullfile(installation_path, 'common', 'bin_arm.zip'), ... + fullfile(installation_path, 'common'), false); % Unpack MATLAB library binaries matlab_robot_server_path = fullfile(installation_path, 'franka_robot_server'); - untar(fullfile(matlab_robot_server_path, 'bin.tar.gz'), matlab_robot_server_path); - untar(fullfile(matlab_robot_server_path, 'bin_arm.tar.gz'), matlab_robot_server_path); + tryUntar(fullfile(matlab_robot_server_path, 'bin.tar.gz'), matlab_robot_server_path); + tryUntar(fullfile(matlab_robot_server_path, 'bin_arm.tar.gz'), matlab_robot_server_path, false); matlab_lib_path = fullfile(installation_path, 'franka_robot'); - unzip(fullfile(matlab_lib_path, 'bin.zip'), matlab_lib_path); - addpath(fullfile(matlab_lib_path, 'bin')); + tryUnzip(fullfile(matlab_lib_path, 'bin.zip'), matlab_lib_path); + matlab_bin_path = fullfile(matlab_lib_path, 'bin'); + if ~isfolder(matlab_bin_path) + error('Required MATLAB binaries could not be unpacked from: %s', ... + fullfile(matlab_lib_path, 'bin.zip')); + end + addpath(matlab_bin_path); % Unpack dependencies deps_path = fullfile(installation_path, 'dependencies'); - unzip(fullfile(deps_path, 'libfranka.zip'), installation_path); - unzip(fullfile(deps_path, 'libfranka_arm.zip'), installation_path); + tryUnzip(fullfile(deps_path, 'libfranka.zip'), installation_path); + tryUnzip(fullfile(deps_path, 'libfranka_arm.zip'), installation_path, false); + end + + function tryUnzip(archivePath, destPath, warnIfMissing) + % Attempts to unzip an archive, optionally warns if file not found + if nargin < 3 + warnIfMissing = true; + end + + if ~isfile(archivePath) + if warnIfMissing + warning('Archive not found, skipping: %s', archivePath); + end + return; + end + try + unzip(archivePath, destPath); + catch ME + error('Failed to unzip %s: %s', archivePath, ME.message); + end + end + + function tryUntar(archivePath, destPath, warnIfMissing) + % Attempts to untar an archive, optionally warns if file not found + if nargin < 3 + warnIfMissing = true; + end + + if ~isfile(archivePath) + if warnIfMissing + warning('Archive not found, skipping: %s', archivePath); + end + return; + end + try + untar(archivePath, destPath); + catch ME + error('Failed to untar %s: %s', archivePath, ME.message); + end + end + + function binaryGroups = getBinaryGroups() + installation_path = franka_toolbox_installation_path_get(); + binaryGroups = struct( ... + 'missingLabel', { ... + 'Missing MATLAB host archives', ... + 'Missing x86_64 target archives (bin)', ... + 'Missing ARM64 target archives (bin_arm)' ... + }, ... + 'foundLabel', { ... + 'Found MATLAB host archives', ... + 'Found x86_64 target archives (bin)', ... + 'Found ARM64 target archives (bin_arm)' ... + }, ... + 'requiredPaths', { ... + { ... + fullfile(installation_path, 'franka_simulink_library', 'bin.zip'), ... + fullfile(installation_path, 'franka_robot', 'bin.zip') ... + }, ... + { ... + fullfile(installation_path, 'common', 'bin.zip'), ... + fullfile(installation_path, 'franka_robot_server', 'bin.tar.gz'), ... + fullfile(installation_path, 'dependencies', 'libfranka.zip') ... + }, ... + { ... + fullfile(installation_path, 'common', 'bin_arm.zip'), ... + fullfile(installation_path, 'franka_robot_server', 'bin_arm.tar.gz'), ... + fullfile(installation_path, 'dependencies', 'libfranka_arm.zip') ... + } ... + } ... + ); + + for i = 1:numel(binaryGroups) + [binaryGroups(i).missingPaths, binaryGroups(i).foundPaths] = ... + splitArchivesByAvailability(binaryGroups(i).requiredPaths); + end + end + + function [missingArchives, foundArchives] = splitArchivesByAvailability(requiredArchives) + missingMask = ~cellfun(@isfile, requiredArchives); + missingArchives = requiredArchives(missingMask); + foundArchives = requiredArchives(~missingMask); + end + + function tf = hasMissingBinaries(binaryGroups) + tf = any(arrayfun(@(group) ~isempty(group.missingPaths), binaryGroups)); + end + + function message = getMissingBinariesWarningMessage(binaryGroups) + installation_path = franka_toolbox_installation_path_get(); + missingSections = cell(0, 1); + foundSections = cell(0, 1); + for i = 1:numel(binaryGroups) + if ~isempty(binaryGroups(i).missingPaths) + relativeMissingArchives = cellfun( ... + @(path) strrep(path, [installation_path filesep], ''), ... + binaryGroups(i).missingPaths, 'UniformOutput', false); + missingSections{end + 1} = sprintf('%s:\n - %s\n', ... + binaryGroups(i).missingLabel, ... + strjoin(relativeMissingArchives, '\n - ')); %#ok + end + + if isempty(binaryGroups(i).foundPaths) + continue; + end + relativeFoundArchives = cellfun( ... + @(path) strrep(path, [installation_path filesep], ''), ... + binaryGroups(i).foundPaths, 'UniformOutput', false); + foundSections{end + 1} = sprintf('%s:\n - %s\n', ... + binaryGroups(i).foundLabel, ... + strjoin(relativeFoundArchives, '\n - ')); %#ok + end + missingList = strjoin(missingSections, '\n'); + if isempty(foundSections) + foundList = ' - none'; + else + foundList = strjoin(foundSections, '\n'); + end + + message = sprintf([ ... + ['One or more binaries were not found. The Toolbox won''t function partially or at all ' ... + 'depending on the binaries found (check warning message below).\n\n'] ... + '%s\n' ... + 'Found archives:\n%s\n\n' ... + 'Please\n\n' ... + '1. In case the official mtlbx release has been installed and the source code has been cloned please make sure that the source code is not found in path.\n' ... + '2. If you''ve cloned the source code please make sure you''ve built the missing x86_64 and/or ARM64 binaries with ./build.sh [amd64|arm64] --libfranka , and then run franka_robot_mex() and franka_simulink_library_mex().' ... + ], missingList, foundList); end function configureSimulink() diff --git a/scripts/libfranka_handle/franka_toolbox_libfranka_system_installation_check.m b/scripts/libfranka_handle/franka_toolbox_libfranka_system_installation_check.m index f836afa..a7da24c 100644 --- a/scripts/libfranka_handle/franka_toolbox_libfranka_system_installation_check.m +++ b/scripts/libfranka_handle/franka_toolbox_libfranka_system_installation_check.m @@ -24,7 +24,7 @@ if ~remote [~, r] = system(ld_search_cmd); else - [~, r] = franka_toolbox_remote_system_cmd(ld_search_cmd,'~',user,ip,port); + [~, r] = franka_toolbox_ssh_exec(ld_search_cmd, user, ip, port); end if ~isempty(r) @@ -34,4 +34,4 @@ end end -end \ No newline at end of file +end diff --git a/scripts/packaging/franka_toolbox_libfranka_deps_bundle.m b/scripts/packaging/franka_toolbox_libfranka_deps_bundle.m index 05f181d..81b08d8 100644 --- a/scripts/packaging/franka_toolbox_libfranka_deps_bundle.m +++ b/scripts/packaging/franka_toolbox_libfranka_deps_bundle.m @@ -23,25 +23,32 @@ function franka_toolbox_libfranka_deps_bundle(user,ip,port) ' --executable=',fullfile(libfranka_path,'libfranka.so'),... ' --library=',fullfile(libfranka_path,libfranka)]; - franka_toolbox_system_cmd(cmd,'.',true); + opts = struct('verbose', true); + franka_toolbox_local_exec(cmd, '.', opts); end else % Check if linuxdeploy has been extracted on remote system % If not, install it first - [~, cmdout] = franka_toolbox_remote_system_cmd('ls -d ~/franka-dev-tools/squashfs-root 2>/dev/null || echo "Directory does not exist"', '', user, ip, port); - if contains(cmdout, 'Directory does not exist') + [status, ~] = franka_toolbox_ssh_exec('ls -d ~/franka-dev-tools/squashfs-root 2>/dev/null', user, ip, port); + if status ~= 0 % linuxdeploy not extracted, install it franka_toolbox_linuxdeploy_install(user, ip, port); end libfranka_remote_path = '\$HOME/libfranka/build'; - cmd = ['./AppRun',... + cmd = ['cd ~/franka-dev-tools/squashfs-root && ./AppRun',... ' --appdir=',libfranka_remote_path,... ' --executable=',fullfile(libfranka_remote_path,'libfranka.so'),... ' --library=',fullfile(libfranka_remote_path,libfranka)]; - franka_toolbox_remote_system_cmd(cmd,'~/franka-dev-tools/squashfs-root',user,ip,port,true); - franka_toolbox_foder_from_remote_cp(fullfile(libfranka_remote_path,'usr'),fullfile(franka_toolbox_installation_path_get(),'libfranka_arm','build'),user,ip,port,true); + sshOpts = struct('verbose', true, 'nothrow', false); + franka_toolbox_ssh_exec(cmd, user, ip, port, sshOpts); + + % Copy usr folder from remote + scpOpts = struct('recursive', true, 'verbose', true, 'nothrow', false); + franka_toolbox_scp(':~/libfranka/build/usr', ... + fullfile(franka_toolbox_installation_path_get(),'libfranka_arm','build'), ... + user, ip, port, scpOpts); end -end \ No newline at end of file +end diff --git a/scripts/packaging/franka_toolbox_linuxdeploy_get.m b/scripts/packaging/franka_toolbox_linuxdeploy_get.m index 37dd331..48c1cd8 100644 --- a/scripts/packaging/franka_toolbox_linuxdeploy_get.m +++ b/scripts/packaging/franka_toolbox_linuxdeploy_get.m @@ -30,9 +30,7 @@ % Download on-the-fly into tmp if missing if ~isfile(appimage_path{i}) cmd = ['wget -q -O ', appimage_name, ' ', url, ' && chmod +x ', appimage_name]; - franka_toolbox_system_cmd(cmd, base_tmp); + franka_toolbox_local_exec(cmd, base_tmp); end end end - - diff --git a/scripts/packaging/franka_toolbox_linuxdeploy_install.m b/scripts/packaging/franka_toolbox_linuxdeploy_install.m index 5394757..2b04f16 100644 --- a/scripts/packaging/franka_toolbox_linuxdeploy_install.m +++ b/scripts/packaging/franka_toolbox_linuxdeploy_install.m @@ -12,10 +12,14 @@ function franka_toolbox_linuxdeploy_install(user,ip,port) linuxdeploy_appimage = franka_toolbox_linuxdeploy_get(); aarch64_path = linuxdeploy_appimage{2}; - franka_toolbox_remote_system_cmd(['rm -rf ','~/franka-dev-tools',' && mkdir -p ','~/franka-dev-tools'],'~',user,ip,port); - franka_toolbox_foder_remote_cp(aarch64_path,user,ip,'~/franka-dev-tools/linuxdeploy-aarch64.AppImage',port); - franka_toolbox_remote_system_cmd('./linuxdeploy-aarch64.AppImage --appimage-extract','~/franka-dev-tools',user,ip,port); + sshOpts = struct('nothrow', false); + franka_toolbox_ssh_exec('rm -rf ~/franka-dev-tools && mkdir -p ~/franka-dev-tools', user, ip, port, sshOpts); + + scpOpts = struct('nothrow', false); + franka_toolbox_scp(aarch64_path, ':~/franka-dev-tools/linuxdeploy-aarch64.AppImage', user, ip, port, scpOpts); + + franka_toolbox_ssh_exec('cd ~/franka-dev-tools && ./linuxdeploy-aarch64.AppImage --appimage-extract', user, ip, port, sshOpts); end -end \ No newline at end of file +end diff --git a/scripts/system/franka_toolbox_foder_from_remote_cp.m b/scripts/system/franka_toolbox_foder_from_remote_cp.m deleted file mode 100644 index 2ecb9b0..0000000 --- a/scripts/system/franka_toolbox_foder_from_remote_cp.m +++ /dev/null @@ -1,24 +0,0 @@ -function [status, cmdout] = franka_toolbox_foder_from_remote_cp(folder,destination_path,user,ip,port,verbose) - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package - - if nargin < 5 - port = '22'; - end - - if nargin < 6 - verbose = false; - end - - if verbose - [status, cmdout] = franka_toolbox_system_cmd(['scp -o BatchMode=yes -o ConnectTimeout=10 ','-P ',port,' -r ',user,'@',ip,':',folder,' ', destination_path],'.',true); - else - [status, cmdout] = franka_toolbox_system_cmd(['scp -o BatchMode=yes -o ConnectTimeout=10 ','-P ',port,' -r ',user,'@',ip,':',folder,' ', destination_path],'.',false); - end - - % Check for errors - if status ~= 0 - error('Failed to copy folder from remote with status %d:\n%s', status, cmdout); - end -end \ No newline at end of file diff --git a/scripts/system/franka_toolbox_foder_remote_cp.m b/scripts/system/franka_toolbox_foder_remote_cp.m deleted file mode 100644 index 611c370..0000000 --- a/scripts/system/franka_toolbox_foder_remote_cp.m +++ /dev/null @@ -1,35 +0,0 @@ -function [status, cmdout] = franka_toolbox_foder_remote_cp(folder,user,ip,destination,port,verbose) - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package - - if nargin < 6 - verbose = false; - end - - if nargin < 4 - destination = '~/'; - port = '22'; - end - - if nargin < 5 - port = '22'; - end - - scp_cmd = ['scp -o BatchMode=yes -o ConnectTimeout=10 ','-P ',port,' -r ',folder,' ',user,'@',ip,':',destination]; - - if verbose - fprintf('Executing remote copy command:\n%s\n', scp_cmd); - end - - [status, cmdout] = franka_toolbox_system_cmd(scp_cmd,'.',verbose); - - if verbose - fprintf('Copy command output:\n%s\n', cmdout); - end - - % Check for errors - if status ~= 0 - error('Failed to copy folder with status %d:\n%s', status, cmdout); - end -end \ No newline at end of file diff --git a/scripts/system/franka_toolbox_local_exec.m b/scripts/system/franka_toolbox_local_exec.m new file mode 100644 index 0000000..519a57f --- /dev/null +++ b/scripts/system/franka_toolbox_local_exec.m @@ -0,0 +1,81 @@ +function [status, output] = franka_toolbox_local_exec(cmd, path, options) + %FRANKA_TOOLBOX_LOCAL_EXEC Execute a command locally in a specified directory + % + % [status, output] = franka_toolbox_local_exec(cmd) + % [status, output] = franka_toolbox_local_exec(cmd, path) + % [status, output] = franka_toolbox_local_exec(cmd, path, options) + % + % Inputs: + % cmd - Command to execute + % path - Directory to execute command in (default: '.') + % options - Struct with optional fields: + % .verbose - Echo command output (default: false) + % .nothrow - Don't throw error on failure (default: true) + % + % Outputs: + % status - Exit status of the command (0 = success) + % output - Command output string + % + % Notes: + % - On Unix systems, LD_LIBRARY_PATH is cleared to avoid conflicts + % with MATLAB's bundled libraries during compilation + % - The function uses pushd/popd to change directories safely + % + % Examples: + % % Run cmake in build directory + % [s, o] = franka_toolbox_local_exec('cmake ..', '/path/to/build'); + % + % % Run with verbose output + % opts.verbose = true; + % franka_toolbox_local_exec('make', '/path/to/project', opts); + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package + + % Default arguments + if nargin < 2 || isempty(path) + path = '.'; + end + + if nargin < 3 + options = struct(); + end + + % Parse options with defaults + verbose = false; + nothrow = true; + + if isfield(options, 'verbose') + verbose = options.verbose; + end + if isfield(options, 'nothrow') + nothrow = options.nothrow; + end + + % Build the command with directory change + if isunix() + % Clear LD_LIBRARY_PATH to avoid conflicts with MATLAB's bundled libraries + full_cmd = sprintf('LD_LIBRARY_PATH="" && pushd "%s" && %s && popd', path, cmd); + else + % Windows - just change directory + full_cmd = sprintf('pushd "%s" && %s && popd', path, cmd); + end + + % Execute command + if verbose + [status, output] = system(full_cmd, '-echo'); + else + [status, output] = system(full_cmd); + end + + output = strtrim(output); + + % Handle errors + if status ~= 0 && ~nothrow + error('FrankaToolbox:LocalExec:Failed', ... + 'Command failed (status %d) in directory: %s\nCommand: %s\nOutput: %s', ... + status, path, cmd, output); + end +end + diff --git a/scripts/system/franka_toolbox_remote_system_cmd.m b/scripts/system/franka_toolbox_remote_system_cmd.m deleted file mode 100644 index 59dc955..0000000 --- a/scripts/system/franka_toolbox_remote_system_cmd.m +++ /dev/null @@ -1,45 +0,0 @@ -function [s, r] = franka_toolbox_remote_system_cmd(cmd, path, user, ip, port, verbose) - % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package - - if nargin < 6 - verbose = false; - end - - if nargin < 5 - port = '22'; - end - - % Convert path to use forward slashes for SSH compatibility - path = strrep(path, '\', '/'); - - % Check if running on Windows - if ispc - % Use full path to OpenSSH if it exists in Windows - ssh_cmd = 'C:\Windows\System32\OpenSSH\ssh.exe'; - if ~exist(ssh_cmd, 'file') - error('OpenSSH not found. Please install OpenSSH for Windows or add it to System32/OpenSSH.'); - end - else - ssh_cmd = 'ssh'; - end - - full_cmd = ['LD_LIBRARY_PATH='''' && cd "', path, '" && ', cmd]; - ssh_cmd_full = ['"', ssh_cmd, '" -o BatchMode=yes -o ConnectTimeout=10 ', user, '@', ip, ' -p ', port, ' "', full_cmd, '"']; - - if verbose - fprintf('Executing remote command:\n%s\n', full_cmd); - end - - if verbose - [s, r] = system(ssh_cmd_full, '-echo'); - else - [s, r] = system(ssh_cmd_full); - end - - % Check for errors - if s ~= 0 - error('Remote command failed with status %d on %s@%s:%s\nCommand: %s\nOutput: %s', s, user, ip, port, full_cmd, r); - end -end \ No newline at end of file diff --git a/scripts/system/franka_toolbox_scp.m b/scripts/system/franka_toolbox_scp.m new file mode 100644 index 0000000..6a2a9c8 --- /dev/null +++ b/scripts/system/franka_toolbox_scp.m @@ -0,0 +1,136 @@ +function [status, output] = franka_toolbox_scp(source, destination, user, ip, port, options) + %FRANKA_TOOLBOX_SCP Copy files/folders to or from a remote machine via SCP + % + % [status, output] = franka_toolbox_scp(source, destination, user, ip) + % [status, output] = franka_toolbox_scp(source, destination, user, ip, port) + % [status, output] = franka_toolbox_scp(source, destination, user, ip, port, options) + % + % Inputs: + % source - Source path (local or remote with ':' prefix for remote) + % destination - Destination path (local or remote with ':' prefix for remote) + % user - SSH username + % ip - IP address or hostname of remote machine + % port - SSH port (default: '22') + % options - Struct with optional fields: + % .recursive - Copy directories recursively (default: false) + % .verbose - Echo command output (default: false) + % .nothrow - Don't throw error on failure (default: false) + % .timeout - Connection timeout in seconds (default: 10) + % + % For remote paths, use ':' prefix to indicate remote: + % ':remote/path' means the path is on the remote machine + % 'local/path' means the path is on the local machine + % + % Examples: + % % Copy local file to remote + % franka_toolbox_scp('/local/file.txt', ':/remote/dir/', 'user', '192.168.1.1'); + % + % % Copy remote folder to local (recursive) + % opts.recursive = true; + % franka_toolbox_scp(':/remote/folder', '/local/dest/', 'user', '192.168.1.1', '22', opts); + % + % % Copy local folder to remote (recursive, non-throwing) + % opts.recursive = true; + % opts.nothrow = true; + % [s, o] = franka_toolbox_scp('/local/folder', ':/remote/', 'user', '192.168.1.1', '22', opts); + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package + + % Default arguments + if nargin < 5 || isempty(port) + port = '22'; + end + + if nargin < 6 + options = struct(); + end + + % Parse options with defaults + recursive = false; + verbose = false; + nothrow = false; + timeout = 10; + + if isfield(options, 'recursive') + recursive = options.recursive; + end + if isfield(options, 'verbose') + verbose = options.verbose; + end + if isfield(options, 'nothrow') + nothrow = options.nothrow; + end + if isfield(options, 'timeout') + timeout = options.timeout; + end + + % Build SCP command based on platform + if ispc() + scp_exe = 'scp.exe'; + else + scp_exe = 'scp'; + end + + % Build flags + flags = sprintf('-o ConnectTimeout=%d -o BatchMode=yes -P %s', timeout, port); + if recursive + flags = [flags ' -r']; + end + + % Process source and destination paths + source_path = processPath(source, user, ip); + dest_path = processPath(destination, user, ip); + + % Quote paths to handle spaces + source_path = quotePath(source_path); + dest_path = quotePath(dest_path); + + % Build full SCP command + scp_cmd = sprintf('%s %s %s %s', scp_exe, flags, source_path, dest_path); + + if verbose + fprintf('Executing SCP command:\n %s\n', scp_cmd); + end + + % Execute command + if verbose + [status, output] = system(scp_cmd, '-echo'); + else + [status, output] = system(scp_cmd); + end + + output = strtrim(output); + + % Handle errors + if status ~= 0 && ~nothrow + error('FrankaToolbox:SCP:Failed', ... + 'SCP command failed (status %d)\nCommand: %s\nOutput: %s', ... + status, scp_cmd, output); + end +end + +function path = processPath(inputPath, user, ip) + % Convert path notation: ':path' means remote, 'path' means local + if startsWith(inputPath, ':') + % Remote path - prepend user@ip: + remotePath = inputPath(2:end); % Remove leading ':' + remotePath = strrep(remotePath, '\', '/'); % Normalize path separators + path = sprintf('%s@%s:%s', user, ip, remotePath); + else + % Local path - normalize separators for command line + path = strrep(inputPath, '\', '/'); + end +end + +function quotedPath = quotePath(path) + % Quote path to handle spaces and special characters + % Don't double-quote if already quoted + if ~startsWith(path, '"') + quotedPath = ['"' path '"']; + else + quotedPath = path; + end +end + diff --git a/scripts/system/franka_toolbox_ssh_exec.m b/scripts/system/franka_toolbox_ssh_exec.m new file mode 100644 index 0000000..527ce7e --- /dev/null +++ b/scripts/system/franka_toolbox_ssh_exec.m @@ -0,0 +1,90 @@ +function [status, output] = franka_toolbox_ssh_exec(cmd, user, ip, port, options) + %FRANKA_TOOLBOX_SSH_EXEC Execute a command on a remote machine via SSH + % + % [status, output] = franka_toolbox_ssh_exec(cmd, user, ip) + % [status, output] = franka_toolbox_ssh_exec(cmd, user, ip, port) + % [status, output] = franka_toolbox_ssh_exec(cmd, user, ip, port, options) + % + % Inputs: + % cmd - Command to execute on remote machine + % user - SSH username + % ip - IP address or hostname of remote machine + % port - SSH port (default: '22') + % options - Struct with optional fields: + % .verbose - Echo command output (default: false) + % .nothrow - Don't throw error on failure (default: true) + % .timeout - Connection timeout in seconds (default: 5) + % + % Outputs: + % status - Exit status of the command (0 = success) + % output - Command output string + % + % Examples: + % % Check if file exists (non-throwing) + % [s, ~] = franka_toolbox_ssh_exec('test -f /path/file', 'user', '192.168.1.1'); + % + % % Run command and throw on failure + % opts.nothrow = false; + % franka_toolbox_ssh_exec('ls -la', 'user', '192.168.1.1', '22', opts); + % + % Copyright (c) 2025 Franka Robotics GmbH - All Rights Reserved + % This file is subject to the terms and conditions defined in the file + % 'LICENSE', which is part of this package + + % Default arguments + if nargin < 4 || isempty(port) + port = '22'; + end + + if nargin < 5 + options = struct(); + end + + % Parse options with defaults + verbose = false; + nothrow = true; + timeout = 5; + + if isfield(options, 'verbose') + verbose = options.verbose; + end + if isfield(options, 'nothrow') + nothrow = options.nothrow; + end + if isfield(options, 'timeout') + timeout = options.timeout; + end + + % Build SSH command based on platform + if ispc() + % Windows - use OpenSSH + ssh_exe = 'ssh.exe'; + else + ssh_exe = 'ssh'; + end + + % Build the full SSH command with standard options + ssh_cmd = sprintf('%s -o ConnectTimeout=%d -o BatchMode=yes -p %s %s@%s "%s"', ... + ssh_exe, timeout, port, user, ip, cmd); + + if verbose + fprintf('Executing SSH command:\n %s\n', cmd); + end + + % Execute command - use evalc to suppress any MATLAB-generated messages + if verbose + [status, output] = system(ssh_cmd, '-echo'); + else + evalc('[status, output] = system(ssh_cmd)'); + end + + output = strtrim(output); + + % Handle errors + if status ~= 0 && ~nothrow + error('FrankaToolbox:SSHExec:Failed', ... + 'SSH command failed (status %d) on %s@%s:%s\nCommand: %s\nOutput: %s', ... + status, user, ip, port, cmd, output); + end +end + diff --git a/scripts/system/franka_toolbox_system_cmd.m b/scripts/system/franka_toolbox_system_cmd.m deleted file mode 100644 index cf8a400..0000000 --- a/scripts/system/franka_toolbox_system_cmd.m +++ /dev/null @@ -1,29 +0,0 @@ -function [status, cmdout] = franka_toolbox_system_cmd(cmd,path,echo) - % Copyright (c) 2024 Franka Robotics GmbH - All Rights Reserved - % This file is subject to the terms and conditions defined in the file - % 'LICENSE' , which is part of this package - - if nargin < 2 - path = '.'; - echo = false; - end - - if nargin < 3 - echo = false; - end - - if isunix() - if echo - [status, cmdout] = system(['LD_LIBRARY_PATH="" && pushd "',path,'" && ',cmd, '&& popd'], '-echo'); - else - [status, cmdout] = system(['LD_LIBRARY_PATH="" && pushd "',path,'" && ',cmd, '&& popd']); - end - elseif ispc() - if echo - [status, cmdout] = system(['pushd "',path,'" && ',cmd, '&& popd'], '-echo'); - else - [status, cmdout] = system(['pushd "',path,'" && ',cmd, '&& popd']); - end - end - -end \ No newline at end of file