diff --git a/website/docs/api-reference/cli.mdx b/website/docs/api-reference/cli.mdx new file mode 100644 index 00000000..a9c7eb6f --- /dev/null +++ b/website/docs/api-reference/cli.mdx @@ -0,0 +1,500 @@ +--- +title: OpenArm CAN CLI +sidebar_position: 4 +--- + +# OpenArm CAN CLI Tool + +`openarm-can-cli` is a command-line tool for configuring and monitoring DaMiao motors over CAN/CAN-FD interfaces. + +--- + +## Prerequisites + +### Install + +```bash +sudo apt install -y libopenarm-can-dev openarm-can-utils +``` + +For manual installation or other platforms, see the [build guide](https://github.com/enactic/openarm_can/tree/main/dev). + +Verify the socketcan interface is available: + +```bash +ip link show | grep can +``` + +--- + +## Quick Start + +### 1. Configure the CAN interface + +Before communicating with motors, configure the CAN interface baudrate to match your motors. + +```bash +# Default: 1Mbps nominal, 5Mbps data, CAN-FD +openarm-can-cli -i can0 can_configure + +# Classic CAN (1Mbps only, no FD) +openarm-can-cli -i can0 can_configure --no-fd +``` + +### 2. Discover connected motors + +```bash +openarm-can-cli -i can0 discover +``` + +### 3. Enable motors and check status + +```bash +# Enable all arm motors (IDs 1-8) +openarm-can-cli -i can0 enable + +# Monitor motor status +openarm-can-cli -i can0 monitor +``` + +--- + +## Global Options + +| Option | Description | Default | +|--------|-------------|---------| +| `-i, --interface` | SocketCAN interface | `can0` | +| `-h, --help` | Print help message | | + +The `-i` option applies to all subcommands: + +```bash +openarm-can-cli -i can1 monitor +``` + +--- + +## Subcommands + +### `can_configure` — Configure CAN Interface + +Set up the SocketCAN interface baudrate and mode. + +```bash +openarm-can-cli -i can0 can_configure [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-b, --bitrate` | Nominal (arbitration) bitrate | `1000000` | +| `-d, --dbitrate` | Data phase bitrate (CAN-FD) | `5000000` | +| `--sp` | Sample point for nominal phase | `0.75` | +| `--dsp` | Sample point for data phase | `0.75` | +| `--dsjw` | Data SJW | `2` | +| `--rm` | Auto-restart time (ms) | `100` | +| `--no-fd` | Disable CAN-FD mode | | + +**Examples:** + +```bash +# Default (1Mbps / 5Mbps FD) +openarm-can-cli -i can0 can_configure + +# 8Mbps data rate +openarm-can-cli -i can0 can_configure -d 8000000 --dsp 0.6 --dsjw 1 + +# Classic CAN (no FD) +openarm-can-cli -i can0 can_configure --no-fd +``` + +:::note +When `-i` is not specified, `can_configure` applies to all interfaces (`can0`–`can3`). +::: + +--- + +### `discover` — Scan for Motors + +Scan the CAN bus for connected motors. + +```bash +openarm-can-cli -i can0 discover [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-m, --max-id` | Maximum motor ID to scan | `16` | +| `--full-scan` | Scan all 12 baudrates | Fast scan (1M/5M/8M/10M) | + +**Examples:** + +```bash +# Fast scan (recommended) +openarm-can-cli -i can0 discover + +# Full scan (slower but more thorough) +openarm-can-cli -i can0 discover --full-scan + +# Scan up to ID 32 +openarm-can-cli -i can0 discover -m 32 +``` + +**Example output:** + +``` +========================================================= + DISCOVERY SUMMARY (Total: 8 motors found) +--------------------------------------------------------- +Send ID Recv ID Internal Baudrate Setting +--------------------------------------------------------- +0x01 0x11 5 Mbps (FD) (Code: 9) +0x02 0x12 5 Mbps (FD) (Code: 9) +... +========================================================= +⚠️ WARNING: CAN interface is now configured at 10 Mbps (FD). + Run 'can_configure' to restore the baudrate for your motors. +``` + +:::note +After `discover`, always run `can_configure` to restore the correct baudrate before using other commands. +::: + +--- + +### `show_param` — Read Motor Parameters + +Read all internal parameters from motors. + +```bash +openarm-can-cli -i can0 show_param [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-a, --arm` | Read from arm motors (IDs 1-8) | enabled | +| `--id` | Specific motor IDs | | + +**Examples:** + +```bash +# Read parameters from all arm motors +openarm-can-cli -i can0 show_param + +# Read from specific motors +openarm-can-cli -i can0 show_param --id 1,2,3 +openarm-can-cli -i can0 show_param --id 1 2 3 +``` + +**Example output:** + +``` +================================================== + MOTOR ID: 0x1 (Response ID: 0x11) +================================================== +Parameter R/W Type Range Value +-------------------------------------------------------------------------------- +Master ID RW uint32 [0, 0x7FF] 17 +Motor (ESC) ID RW uint32 [0, 0x7FF] 1 +Control Mode RW uint32 [0, 4] 1 (MIT) +CAN Baudrate RW uint32 [0, 9] 9 (5M) +... +``` + +--- + +### `write_param` — Write Motor Parameter + +Write a value to a specific motor register. + +```bash +openarm-can-cli -i can0 write_param -c -r -v [--save] +``` + +| Option | Description | +|--------|-------------| +| `-c, --id` | Target motor ID (required) | +| `-r, --rid` | Register ID (required) | +| `-v, --value` | Value to write (required) | +| `--save` | Save to flash (⚠️ limit: ~10,000 cycles) | + +**Example:** + +```bash +# Set control mode to MIT (RID=10, value=1) +openarm-can-cli -i can0 write_param -c 1 -r 10 -v 1 +``` + +--- + +### `enable` / `disable` — Motor Power Control + +Enable or disable motor torque output. + +```bash +openarm-can-cli -i can0 enable [OPTIONS] +openarm-can-cli -i can0 disable [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-a, --arm` | All arm motors (IDs 1-8) | enabled | +| `--id` | Specific motor IDs | | + +**Examples:** + +```bash +# Enable all arm motors +openarm-can-cli -i can0 enable + +# Enable specific motors +openarm-can-cli -i can0 enable --id 1,2,3 + +# Disable all arm motors +openarm-can-cli -i can0 disable +``` + +--- + +### `monitor` — Live Telemetry Dashboard + +Display real-time motor position, velocity, torque, and temperature. + +```bash +openarm-can-cli -i can0 monitor [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-a, --arm` | Monitor arm motors (IDs 1-8) | enabled | +| `--id` | Specific motor IDs | | +| `-t, --tick` | Update interval (ms) | `100` | +| `-d, --duration` | Total duration (ms) | `6000` | + +**Examples:** + +```bash +# Monitor all arm motors for 6 seconds +openarm-can-cli -i can0 monitor + +# Monitor specific motors +openarm-can-cli -i can0 monitor --id 1,2,3 + +# Monitor for 30 seconds with 50ms update interval +openarm-can-cli -i can0 monitor -d 30000 -t 50 +``` + +--- + +### `change_baud` — Change Motor Baudrate + +Change the internal CAN baudrate of a motor. + +```bash +openarm-can-cli -i can0 change_baud -b -c [--save] +``` + +| Option | Description | +|--------|-------------| +| `-b, --baudrate` | Target baudrate (required) | +| `-c, --canid` | Motor ID (required) | +| `--save` | Save to flash (⚠️ limit: ~10,000 cycles) | + +**Supported baudrates:** + +| Code | Baudrate | +|------|----------| +| 0 | 125K | +| 1 | 200K | +| 2 | 250K | +| 3 | 500K | +| 4 | 1M | +| 5 | 2M | +| 6 | 2.5M | +| 7 | 3.2M | +| 8 | 4M | +| 9 | 5M | +| 10 | 8M | +| 11 | 10M | + +**Example:** + +```bash +# 1. First, configure the interface to match the motor's CURRENT baudrate +openarm-can-cli -i can0 can_configure -d 5000000 # if motor is currently at 5Mbps + +# 2. Change motor baudrate and save +openarm-can-cli -i can0 change_baud -b 8000000 -c 3 --save + +# 3. Power cycle the motor + +# 4. Reconfigure interface to the NEW baudrate +openarm-can-cli -i can0 can_configure -d 8000000 +``` + +:::danger +- Always configure the CAN interface to match the motor's **current** baudrate before running this command. If they don't match, the command will not reach the motor. +- The `--save` flag writes to flash memory. Flash memory has a **hard limit of ~10,000 write cycles**. Do not run `--save` in a loop or repeatedly. Exceeding this limit will permanently damage the motor's flash memory. +- After saving, a **power cycle is required** for the new baudrate to take effect. +::: + +--- + +### `change_id` — Change Motor CAN ID + +Change the Slave ID and Master ID of a motor. + +```bash +openarm-can-cli -i can0 change_id -c -s [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-c, --current` | Current Slave ID (required) | | +| `-s, --new-slave` | New Slave ID (required) | | +| `-m, --new-master` | New Master ID | `17` | +| `--save` | Save to flash | | + +**Example:** + +```bash +# Change motor ID from 5 to 6 +openarm-can-cli -i can0 change_id -c 5 -s 6 --save +``` + +:::danger +- Always configure the CAN interface to match the motor's **current** baudrate before running this command. +- If two motors on the same bus share the same ID, CAN communication will fail. Make sure all motor IDs on a bus are unique before enabling them. Use `discover` to check current IDs. +- The `--save` flag writes to flash memory. Flash memory has a **hard limit of ~10,000 write cycles**. Do not run `--save` repeatedly. +::: + +--- + +### `set_zero` — Set Motor Zero Position + +```bash +# Set zero for all arm motors +openarm-can-cli -i can0 set_zero + +# Set zero for specific motors +openarm-can-cli -i can0 set_zero --id 1,2 +``` + +--- + +### `clear_error` — Clear Motor Errors + +Reset motor error flags. + +```bash +openarm-can-cli -i can0 clear_error [OPTIONS] +``` + +| Option | Description | Default | +|--------|-------------|---------| +| `-a, --arm` | Clear errors on arm motors (IDs 1-8) | enabled | +| `--id` | Specific motor IDs | | + +--- + +## Common Workflows + +### First-time motor connection + +```bash +# 1. Configure interface (match your motor baudrate) +openarm-can-cli -i can0 can_configure + +# 2. Discover motors and check baudrates +openarm-can-cli -i can0 discover + +# 3. Restore interface to motor baudrate after discover +openarm-can-cli -i can0 can_configure + +# 4. Check motor parameters +openarm-can-cli -i can0 show_param + +# 5. Enable and monitor +openarm-can-cli -i can0 enable +openarm-can-cli -i can0 monitor +``` + +### Change motor baudrate from 5Mbps to 8Mbps + +```bash +# 1. Configure interface to current motor baudrate (5Mbps) +openarm-can-cli -i can0 can_configure -d 5000000 + +# 2. Change motor baudrate +openarm-can-cli -i can0 change_baud -b 8000000 -c 1 --save + +# 3. Power cycle the motor + +# 4. Reconfigure interface to new baudrate +openarm-can-cli -i can0 can_configure -d 8000000 --dsp 0.6 --dsjw 1 +``` + +--- + +## Troubleshooting + +### General Checklist + +Before diving into specific issues, go through this checklist: + +- [ ] Motor power is on +- [ ] CAN cable is connected and not broken +- [ ] CAN interface baudrate matches the motor's internal baudrate +- [ ] Termination resistors (120Ω) are connected at both ends of the bus +- [ ] All motor IDs on the bus are unique +- [ ] CAN driver is loaded + +--- + +### No motors found in `discover` + +1. Go through the general checklist above +2. Try `--full-scan` to scan all baudrates: + ```bash + openarm-can-cli -i can0 discover --full-scan + ``` + +### `show_param` shows no response + +``` +[!] NO RESPONSE FROM MOTOR - possible causes: + - CAN cable not connected + - Motor power not on + - Baudrate mismatch (run 'discover' to find correct baudrate) + - Wrong motor ID +``` + +Run `discover` to find the correct baudrate and motor IDs, then reconfigure the interface: + +```bash +openarm-can-cli -i can0 discover +openarm-can-cli -i can0 can_configure -d +``` + +### BUS OFF errors in `dmesg` + +```bash +dmesg | grep -i "bus-off" +``` + +1. Go through the general checklist above +2. Try adjusting sample point settings: + ```bash + openarm-can-cli -i can0 can_configure -d 8000000 --dsp 0.6 --dsjw 1 + ``` +3. If using high baudrates (8Mbps+), shorten the cable length + +### `change_baud` not taking effect + +1. Go through the general checklist above +2. Ensure interface baudrate matches motor's **current** baudrate before running +3. After `--save`, power cycle the motor +4. Use `candump` to verify frames and responses: + ```bash + candump can0 + ``` + A successful flash save shows a response frame from the motor (e.g. `0x11` for motor ID 1). \ No newline at end of file diff --git a/website/docs/api-reference/controls.md b/website/docs/api-reference/controls.md deleted file mode 100644 index 701c1be2..00000000 --- a/website/docs/api-reference/controls.md +++ /dev/null @@ -1,20 +0,0 @@ ---- -title: Controls -description: ... -sidebar_position: 5 ---- -# Controls - -The details on OpenArm Control will be coming soon! Contribution is welcomed. - -## System Identification - -Coming soon! - -## Gravity Compensation - -Coming soon! - -## Cartesian Space Control - -Coming soon! diff --git a/website/docs/api-reference/description.mdx b/website/docs/api-reference/description.mdx index 7fe9750f..988d1757 100644 --- a/website/docs/api-reference/description.mdx +++ b/website/docs/api-reference/description.mdx @@ -10,8 +10,7 @@ import BlockImage from '@site/src/components/BlockImage'; ## Overview -This [OpenArm description](https://github.com/enactic/openarm_description) package contains URDF/xacro description files for the OpenArm robot system, -supporting both single-arm and bimanual configurations. +This [OpenArm description](https://github.com/enactic/openarm_description) package contains URDF/xacro description files for the OpenArm robot system, supporting both v1.0 and v2.0 hardware in bimanual configuration. ## Table of Contents @@ -22,43 +21,65 @@ supporting both single-arm and bimanual configurations. --- -## Xacro Structure +## Supported Robot Versions + +| Version | `arm_type` values | Configuration style | +|---|---|---| +| OpenArm v1.0 | `v10`, `v1.0`, `v1_0`, `openarm_v10`, `openarm_v1.0` | Per-argument (`ee_type`, ...) | +| OpenArm v2.0 | `v20`, `v2.0`, `v2_0`, `openarm_v20`, `openarm_v2.0` | Preset-based (`robot_preset`) | -The package follows a hierarchical xacro structure with the following organization. +Both versions share the same launch file and entry point. The `arm_type` argument determines which hardware and xacro files are loaded. -### Robot (Overarching Structure) +--- + +## Xacro Structure -- `robot/openarm_robot.xacro` - Main robot macro that orchestrates all components -- `robot/v10.urdf.xacro` - Entry point for v1.0 arm configuration +Assets are organized per robot version under `assets/robot/`: -### Component Structure +``` +assets/ +├── robot/ +│ ├── openarm_v1.0/ +│ │ ├── config/ # Kinematics, joint limits, inertials +│ │ ├── mesh/ # STL and DAE mesh files +│ │ └── urdf/ +│ │ ├── openarm_v10.urdf.xacro # Entry point for v1.0 +│ │ ├── arm/ +│ │ ├── body/ +│ │ ├── ee/ +│ │ └── ros2_control/ +│ └── openarm_v2.0/ +│ ├── config/ +│ │ ├── arm/ # Joint limits, inertials, kinematics +│ │ ├── body/ # Body link parameters +│ │ └── robot_presets/ # Preset YAML files +│ ├── meshes/ # STL and DAE mesh files +│ └── urdf/ +│ ├── openarm_v20.urdf.xacro # Entry point for v2.0 +│ ├── ros2_control/ +│ └── utils/ +└── end_effector/ + ├── parallel_link/ # Used in v1.0 + └── pinch_gripper/ # Used in v2.0 +``` -- `arm/` - Arm kinematics, joints, and links - - `openarm_arm.xacro` - Arm assembly - - `openarm_macro.xacro` - Arm macro definitions -- `body/` - Robot base/body components - - `openarm_body.xacro` - Body assembly - - `openarm_body_macro.xacro` - Body macro definitions -- `ee/` - End-effector components - - `openarm_hand.xacro` - OpenArm hand gripper - - `openarm_hand_macro.xacro` - Hand macro definitions - - `ee_with_one_link.xacro` - Simple single-link end-effector -- `ros2_control/` - ROS2 Control with hardware interfaces - - `openarm.ros2_control.xacro` - Single arm control interface - - `openarm.bimanual.ros2_control.xacro` - Bimanual control interface +### v2.0 Robot Presets -### Configuration Files +v2.0 uses a preset-based system. A single `robot_preset` argument selects the full robot configuration: -- `config/arm/v10/` - v1.0 arm parameters (kinematics, limits, inertials) -- `config/body/v10/` - v1.0 body parameters -- `config/hand/openarm_hand/` - Hand configuration +| Preset | Description | +|---|---| +| `default_bimanual` | Both left and right arms (default) | +| `right_arm` | Right arm only | +| `left_arm` | Left arm only | +| `right_arm_with_pinch_gripper` | Right arm with pinch gripper | +| `left_arm_with_pinch_gripper` | Left arm with pinch gripper | --- ## Installation -To use this package, make sure you have: [ROS2 installed](ros2/install) -Then set up the workspace: +To use this package, make sure you have [ROS2 installed](ros2/install). Then set up the workspace: ```bash # Set up environment variables if you haven't already (customize as needed) @@ -86,130 +107,141 @@ source $ROS_WS/install/setup.bash ## Universal Robot Description Files (URDF) -The coordinate transforms of each link and joint constraints of OpenArm are defined in the URDF. ROS2 uses a XML macro language called xacro to generate these files. +The coordinate transforms of each link and joint constraints of OpenArm are defined in the URDF. ROS2 uses an XML macro language called xacro to generate these files. -### Generate URDF Files +### Visualization -#### Single Arm Configuration +To display the robot in RViz with a simple joint state GUI: -```bash -# Basic single arm with hand -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=false > openarm_single.urdf +:::tip +If you're receiving errors regarding the `.dae` files, try changing mesh formats to `.stl` or `.obj`. +::: -# Single arm without end-effector -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=false ee_type:=none > openarm_single_no_ee.urdf +#### OpenArm v2.0 -# Single arm with ROS2 control -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=false ros2_control:=true can_interface:=can0 > openarm_single_control.urdf -``` +```bash +# Bimanual (default) +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 -#### Bimanual Configuration +# Right arm only +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 robot_preset:=right_arm -```bash -# Basic bimanual setup -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=true > openarm_bimanual.urdf +# Left arm only +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 robot_preset:=left_arm -# Bimanual with ROS2 control -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=true ros2_control:=true left_can_interface:=can1 right_can_interface:=can0 > openarm_bimanual_control.urdf +# Right arm with pinch gripper +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 robot_preset:=right_arm_with_pinch_gripper -# Bimanual with fake hardware for simulation -xacro $ROS_WS/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=true ros2_control:=true use_fake_hardware:=true > openarm_bimanual_sim.urdf -``` +# Left arm with pinch gripper +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 robot_preset:=left_arm_with_pinch_gripper -### Visualization -To display the robot in RViz with a simple joint state GUI: +# Bimanual with grasp frame +ros2 launch openarm_description display_openarm.launch.py arm_type:=v2.0 emit_grasp_frame:=true +``` -:::tip -If you're receiving errors regarding the `.dae` files, try changing mesh formats to `.stl` or `.obj`. -::: + -#### Single Arm +#### OpenArm v1.0 ```bash -# Display single arm with hand -ros2 launch openarm_description display_openarm.launch.py arm_type:=v10 bimanual:=false - -# Display single arm without end-effector -ros2 launch openarm_description display_openarm.launch.py arm_type:=v10 bimanual:=false ee_type:=none +ros2 launch openarm_description display_openarm.launch.py arm_type:=v1.0 ``` - + +### Generate URDF Files -#### Bimanual +#### OpenArm v2.0 ```bash -# Display bimanual configuration -ros2 launch openarm_description display_openarm.launch.py arm_type:=v10 bimanual:=true +# Bimanual +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v2.0/urdf/openarm_v20.urdf.xacro \ + robot_preset:=default_bimanual > openarm_v20_bimanual.urdf -# Display bimanual without end-effectors -ros2 launch openarm_description display_openarm.launch.py arm_type:=v10 bimanual:=true ee_type:=none -``` +# Right arm only +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v2.0/urdf/openarm_v20.urdf.xacro \ + robot_preset:=right_arm > openarm_v20_right_arm.urdf - +# Right arm with pinch gripper +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v2.0/urdf/openarm_v20.urdf.xacro \ + robot_preset:=right_arm_with_pinch_gripper > openarm_v20_right_arm_pinch.urdf +``` -### Exporting URDFs +#### OpenArm v1.0 -Sometimes, it may be necessary to export a URDF for use in other applications. To generate a new URDF file, run the xacro command with arguments. +```bash +# Bimanual +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v1.0/urdf/openarm_v10.urdf.xacro \ + arm_type:=v10 > openarm_v10_bimanual.urdf -To generate a bimanual URDF: -```sh -URDF_OUT=openarm_bimanual.urdf -xacro ~/ros2_ws/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 bimanual:=true > $URDF_OUT -``` +# Bimanual with ROS2 control +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v1.0/urdf/openarm_v10.urdf.xacro \ + arm_type:=v10 ros2_control:=true left_can_interface:=can1 right_can_interface:=can0 > openarm_v10_bimanual_control.urdf -Or a single arm URDF: -```sh -URDF_OUT=openarm.urdf -xacro ~/ros2_ws/src/openarm_description/urdf/robot/v10.urdf.xacro arm_type:=v10 > $URDF_OUT +# Bimanual with fake hardware for simulation +xacro $(ros2 pkg prefix openarm_description)/share/openarm_description/assets/robot/openarm_v1.0/urdf/openarm_v10.urdf.xacro \ + arm_type:=v10 ros2_control:=true use_fake_hardware:=true > openarm_v10_bimanual_sim.urdf ``` --- ## Available Arguments -Below are some configurable variables for the OpenArm robot description that allows customization when generating URDFs with xacro. +Below are some configurable variables for the OpenArm robot description that allow customization when generating URDFs with xacro. + +### Common Arguments + +| Argument | Default | Description | +|---|---|---| +| `arm_type` | `v20` | Robot version. See [Supported Robot Versions](#supported-robot-versions) for accepted values. | +| `rviz_config` | `bimanual.rviz` | RViz config file to load | -### Core Arguments +### v2.0 Arguments -- `arm_type` - Arm version (eg.: "v10") -- `body_type` - Body version (eg.: "v10") -- `ee_type` - End-effector type (default: "openarm_hand", options: "openarm_hand", "none") -- `bimanual` - Enable bimanual configuration (default: false) +| Argument | Default | Description | +|---|---|---| +| `robot_preset` | `default_bimanual` | Robot configuration preset (see preset table above) | +| `collapse_internal_empty_links` | `true` | Collapse empty intermediate links in the URDF | +| `emit_grasp_frame` | `false` | Add a grasp frame link to the end-effector | -### ROS2 Control Arguments +### v1.0 Arguments -- `ros2_control` - Enable ROS2 control hardware interface (default: false) -- `use_fake_hardware` - Use fake hardware for simulation (default: false) -- `fake_sensor_commands` - Enable fake sensor commands (default: false) -- `can_interface` - CAN interface for single arm (default: "can0") -- `left_can_interface` - CAN interface for left arm in bimanual setup (default: "can1") -- `right_can_interface` - CAN interface for right arm in bimanual setup (default: "can0") +| Argument | Default | Description | +|---|---|---| +| `ros2_control` | `false` | Enable ROS2 control hardware interface | +| `use_fake_hardware` | `false` | Use fake hardware for simulation | +| `fake_sensor_commands` | `false` | Enable fake sensor commands | +| `left_can_interface` | `can1` | CAN interface for left arm | +| `right_can_interface` | `can0` | CAN interface for right arm | -### Positioning Arguments +### v1.0 Positioning Arguments -- `parent` - Parent frame (default: "world") -- `xyz` - Position offset (default: "0 0 0") -- `rpy` - Orientation offset (default: "0 0 0") -- `left_arm_base_xyz` - Left arm base position (default: "0.0 0.031 0.698") -- `left_arm_base_rpy` - Left arm base orientation (default: "-1.5708 0 0") -- `right_arm_base_xyz` - Right arm base position (default: "0.0 -0.031 0.698") -- `right_arm_base_rpy` - Right arm base orientation (default: "1.5708 0 0") +| Argument | Default | Description | +|---|---|---| +| `parent` | `world` | Parent frame | +| `xyz` | `0 0 0` | Position offset | +| `rpy` | `0 0 0` | Orientation offset | +| `left_arm_base_xyz` | `0.0 0.031 0.698` | Left arm base position | +| `left_arm_base_rpy` | `-1.5708 0 0` | Left arm base orientation | +| `right_arm_base_xyz` | `0.0 -0.031 0.698` | Right arm base position | +| `right_arm_base_rpy` | `1.5708 0 0` | Right arm base orientation | + +--- -### Running Multiple Robots with ROS Namespaces and ROS_DOMAIN_ID +## Running Multiple Robots with ROS Namespaces and ROS_DOMAIN_ID When running multiple instances of OpenArm, one option is to namespace arms to control multiple sets of arms with one device. For example, when running multiple instances of bimanual, it is possible to namespace packages with: ```sh -ros2 launch openarm_description openarm.bimanual.launch.py arm_prefix:=leader right_can_interface:=can0 left_can_interface:=can1 +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v2.0 arm_prefix:=leader right_can_interface:=can0 left_can_interface:=can1 ``` and ```sh -ros2 launch openarm_description openarm.bimanual.launch.py arm_prefix:=follower right_can_interface:=can2 left_can_interface:=can3 +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v2.0 arm_prefix:=follower right_can_interface:=can2 left_can_interface:=can3 ``` If network isolation is required (e.g. multiple sets of OpenArm teleoperation), the [ROS_DOMAIN_ID](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Domain-ID.html) environment variable can be set. \ No newline at end of file diff --git a/website/docs/api-reference/index.md b/website/docs/api-reference/index.md index 6d43200e..f3612908 100644 --- a/website/docs/api-reference/index.md +++ b/website/docs/api-reference/index.md @@ -56,17 +56,6 @@ Middleware for connecting OpenArm with the broader ROS2 ecosystem. Enables compa --- -### Advanced Controls - -**[Learn more under Controls →](controls)** - -Control guides and algorithms for OpenArm systems. This is an active area of development where we're implementing and testing various control approaches. - -- **In development**: System identification guide, gravity compensation, cartesian space control -- **Help wanted**: Algorithm implementations, validation testing, control theory expertise - ---- - ## 🤝 Join the Community diff --git a/website/docs/api-reference/ros2/control.mdx b/website/docs/api-reference/ros2/control.mdx index 28b29f46..cfb74f99 100644 --- a/website/docs/api-reference/ros2/control.mdx +++ b/website/docs/api-reference/ros2/control.mdx @@ -15,20 +15,27 @@ The `openarm_description` package provides the complete robot model and visual r The [openarm_ros2](https://github.com/enactic/openarm_ros2) repository contains a collection of packages for use with [ros2_control](https://control.ros.org/humble/index.html). -It abstracts away the hardware control to expose the arm as a interface that receives position, velocity, and torque commands and outputs joint states. +It abstracts away the hardware control to expose the arm as an interface that receives position, velocity, and torque commands and outputs joint states. -To get started, clone the openarm_ros2 repository, build the packages, and source the workspace. +To get started, clone the `openarm_ros2` repository, build the packages, and source the workspace. ```sh git clone https://github.com/enactic/openarm_ros2 ~/ros2_ws/src/openarm_ros2 -# You can skip this step if openarm_can is installed system-wide -# You can also skip it when testing with mock hardware only. -(cd ~/ros2_ws/src && vcs import < ./openarm_ros2/openarm.repos) +# Install openarm_can via apt (Ubuntu) +sudo apt install -y libopenarm-can-dev openarm-can-utils +# For manual installation or other platforms, see: +# https://github.com/enactic/openarm_can/tree/main/dev +# Alternatively, build from source via vcs: +# (cd ~/ros2_ws/src && vcs import < ./openarm_ros2/openarm.repos) + +# Install ROS2 dependencies +cd ~/ros2_ws +rosdep install --from-paths src --ignore-src -r -y # If you only want to use mock hardware, # build with the `--packages-ignore openarm_hardware` option. -cd ~/ros2_ws && colcon build +colcon build source ~/ros2_ws/install/setup.bash ``` @@ -46,48 +53,95 @@ Before operating with real hardware, you MUST: **Improper operation can cause serious injury or equipment damage. Always prioritize safety.** ::: - ## 🚧 ROS2 Control Integration -The [openarm_bringup](https://github.com/enactic/openarm_ros2/tree/main/openarm_bringup) package provides launch files that integrates with the ROS2 control framework through the `openarm_bringup` package. This package provides launch files and configuration to start the hardware interface, load controllers, and connect the physical arm to the ROS2 ecosystem. Once launched, you can use standard ROS2 control tools and interfaces to command the arm and receive feedback. +The [openarm_bringup](https://github.com/enactic/openarm_ros2/tree/main/openarm_bringup) package provides launch files that integrate with the ROS2 control framework. This package provides launch files and configuration to start the hardware interface, load controllers, and connect the physical arm to the ROS2 ecosystem. Once launched, you can use standard ROS2 control tools and interfaces to command the arm and receive feedback. -The bringup package supports both mock hardware (for simulation/testing) and real hardware through a hardware plugin. When using real hardware, you'll need to first build the openarmcan library by following the [CAN setup guide](../can). +The bringup package supports both mock hardware (for simulation/testing) and real hardware through a hardware plugin. When using real hardware, you'll need to first install the `openarm_can` library by following the [CAN setup guide](../can). :::warning Hardware Bridge Status The hardware bridging components are currently being updated and may be unstable. The gripper bridging logic is particularly under active development. ::: -Launch the OpenArm with v1.0 configuration and fake hardware: +### Launch + +Both v1.0 and v2.0 are supported via the `arm_type` argument. ```sh -ros2 launch openarm_bringup openarm.launch.py arm_type:=v10 use_fake_hardware:=true +# v2.0 with fake hardware (default) +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v2.0 + +# v2.0 with real hardware +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v2.0 use_fake_hardware:=false right_can_interface:=can0 left_can_interface:=can1 + +# v1.0 with fake hardware +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v1.0 + +# v1.0 with real hardware +ros2 launch openarm_bringup openarm.bimanual.launch.py arm_type:=v1.0 use_fake_hardware:=false right_can_interface:=can0 left_can_interface:=can1 ``` -**launch files** -- `openarm.launch.py` - Single arm configuration +**Launch files** - `openarm.bimanual.launch.py` - Dual arm configuration **Key Parameters** -- `arm_type` - Arm type (default: v10) -- `use_fake_hardware` - Use fake hardware instead of real hardware (default: false) -- `can_interface` - CAN interface to use (default: can0) -- `robot_controller` - Controller type: joint_trajectory_controller or forward_position_controller -When you run the bringup launch files, robot state publisher, controller manager, etc. will be launched. - -After the controllers are successfully launched, you can verify they're working by checking the available actions: +| Argument | Default | Description | +|---|---|---| +| `arm_type` | `openarm_v2.0` | Robot version. Accepts `v1.0`, `v10`, `openarm_v1.0`, `v2.0`, `v20`, `openarm_v2.0`, etc. | +| `use_fake_hardware` | `true` | Use fake hardware instead of real hardware | +| `right_can_interface` | `can0` | CAN interface for the right arm | +| `left_can_interface` | `can1` | CAN interface for the left arm | +| `robot_controller` | `joint_trajectory_controller` | Controller type: `joint_trajectory_controller` or `forward_position_controller` | +| `arm_prefix` | `` | Prefix for topic namespacing (for running multiple robots) | +| `controllers_file` | `openarm_bimanual_controllers.yaml` | Controllers configuration file | + +### Topics + +After launching, the following topics are available: + +| Topic | Type | Description | +|---|---|---| +| `/joint_states` | `sensor_msgs/JointState` | Current joint states for all joints | +| `/dynamic_joint_states` | `control_msgs/DynamicJointState` | Dynamic joint states | +| `/robot_description` | `std_msgs/String` | Robot URDF description | +| `/tf` | `tf2_msgs/TFMessage` | Transform tree | +| `/tf_static` | `tf2_msgs/TFMessage` | Static transforms | +| `/left_joint_trajectory_controller/joint_trajectory` | `trajectory_msgs/JointTrajectory` | **Command**: left arm trajectory input | +| `/right_joint_trajectory_controller/joint_trajectory` | `trajectory_msgs/JointTrajectory` | **Command**: right arm trajectory input | +| `/left_joint_trajectory_controller/state` | `control_msgs/JointTrajectoryControllerState` | Left arm controller state | +| `/right_joint_trajectory_controller/state` | `control_msgs/JointTrajectoryControllerState` | Right arm controller state | +| `/left_gripper_controller/joint_trajectory` | `trajectory_msgs/JointTrajectory` | **Command**: left gripper trajectory input | +| `/right_gripper_controller/joint_trajectory` | `trajectory_msgs/JointTrajectory` | **Command**: right gripper trajectory input | +| `/left_gripper_controller/state` | `control_msgs/JointTrajectoryControllerState` | Left gripper controller state | +| `/right_gripper_controller/state` | `control_msgs/JointTrajectoryControllerState` | Right gripper controller state | + +### Actions + +| Action | Type | Description | +|---|---|---| +| `/left_joint_trajectory_controller/follow_joint_trajectory` | `control_msgs/action/FollowJointTrajectory` | Execute trajectory on left arm | +| `/right_joint_trajectory_controller/follow_joint_trajectory` | `control_msgs/action/FollowJointTrajectory` | Execute trajectory on right arm | +| `/left_gripper_controller/follow_joint_trajectory` | `control_msgs/action/FollowJointTrajectory` | Execute trajectory on left gripper | +| `/right_gripper_controller/follow_joint_trajectory` | `control_msgs/action/FollowJointTrajectory` | Execute trajectory on right gripper | + +### Verifying the Controllers + +After the controllers are successfully launched, you can verify they're working by checking the available topics and actions: ```sh +ros2 topic list ros2 action list ``` To test joint movement, send a simple trajectory command: ```sh -ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory '{trajectory: {joint_names: ["openarm_joint1", "openarm_joint2", "openarm_joint3", "openarm_joint4", "openarm_joint5", "openarm_joint6", "openarm_joint7"], points: [{positions: [0.15, 0.15, 0.15, 0.15, 0.15, 0.15, 0.15], time_from_start: {sec: 3, nanosec: 0}}]}}' +ros2 action send_goal /left_joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory \ + '{trajectory: {joint_names: ["openarm_left_joint1", "openarm_left_joint2", "openarm_left_joint3", "openarm_left_joint4", "openarm_left_joint5", "openarm_left_joint6"], points: [{positions: [0.15, 0.15, 0.15, 0.15, 0.15, 0.15], time_from_start: {sec: 3, nanosec: 0}}]}}' ``` -This command moves all arm joints to a 0.15 radian position over 3 seconds. +This command moves all left arm joints to a 0.15 radian position over 3 seconds. ## 🚧 MoveIt2 Integration @@ -98,22 +152,39 @@ This command moves all arm joints to a 0.15 radian position over 3 seconds. ### Getting Started with MoveIt2 -To use the MoveIt2 integration: +Both v1.0 and v2.0 are supported via the `arm_type` argument. The MoveIt2 demo launch starts the move group, RViz, ROS2 control, and all controllers in one command. -**Launch the MoveIt2 demo:** ```sh -ros2 launch openarm_bimanual_moveit_config demo.launch.py +# v2.0 with fake hardware (default) +ros2 launch openarm_bimanual_moveit_config demo.launch.py arm_type:=v2.0 + +# v2.0 with real hardware +ros2 launch openarm_bimanual_moveit_config demo.launch.py arm_type:=v2.0 use_fake_hardware:=false right_can_interface:=can0 left_can_interface:=can1 + +# v1.0 with fake hardware +ros2 launch openarm_bimanual_moveit_config demo.launch.py arm_type:=v1.0 + +# v1.0 with real hardware +ros2 launch openarm_bimanual_moveit_config demo.launch.py arm_type:=v1.0 use_fake_hardware:=false right_can_interface:=can0 left_can_interface:=can1 ``` - -
+**Key Parameters** + +| Argument | Default | Description | +|---|---|---| +| `arm_type` | `openarm_v2.0` | Robot version. Accepts `v1.0`, `v10`, `openarm_v1.0`, `v2.0`, `v20`, `openarm_v2.0`, etc. | +| `use_fake_hardware` | `true` | Use fake hardware instead of real hardware | +| `right_can_interface` | `can0` | CAN interface for the right arm | +| `left_can_interface` | `can1` | CAN interface for the left arm | +| `robot_controller` | `joint_trajectory_controller` | Controller type: `joint_trajectory_controller` or `forward_position_controller` | + ### Motion Planning - -
+ + -Target positions can be set in the `Joints` tab in the MotionPlanning panel on the left. Alternatively, the targets on the arms can be dragged and rotated to a target pose, or the goal state can be selected from a list of preset keypoints under `Planning` > `Goal State` +Target positions can be set in the `Joints` tab in the MotionPlanning panel on the left. Alternatively, the targets on the arms can be dragged and rotated to a target pose, or the goal state can be selected from a list of preset keypoints under `Planning` > `Goal State`. The `Planning` tab provides a GUI to generate trajectories to reach a goal position. Clicking on `Plan` to preview the path is recommended. @@ -121,13 +192,18 @@ The `Planning` tab provides a GUI to generate trajectories to reach a goal posit The MoveIt2 integration is actively being developed. Check the [openarm_ros2 repository](https://github.com/enactic/openarm_ros2) for the latest updates and features. ::: -## Control gain +## Control Gains The default gain is set to a relatively low value for safety. As a result, the arm may not be able to reach high angles in some cases. The gain can be adjusted by editing the following configuration file and rebuilding. -* Edit [~/ros2_ws/src/openarm_description/config/arm/v10/control_gains.yaml](https://github.com/enactic/openarm_description/blob/main/config/arm/v10/control_gains.yaml). -* Rebuild by `colcon build`. +**v2.0:** +- Edit `assets/robot/openarm_v2.0/config/arm/control/control_gains.yaml` +- Rebuild by `colcon build` + +**v1.0:** +- Edit `assets/robot/openarm_v1.0/config/arm/control_gains.yaml` +- Rebuild by `colcon build` :::danger ⚠️ GAIN TUNING SAFETY Please adjust the gain with great care. **Start with a lower gain and verify the behavior first, then increase it gradually.** If you execute an action with a large position difference while using a high gain, some controllers may generate dangerously high velocities. -::: +::: \ No newline at end of file diff --git a/website/docs/api-reference/ros2/install.mdx b/website/docs/api-reference/ros2/install.mdx index 6732d90e..97de3696 100644 --- a/website/docs/api-reference/ros2/install.mdx +++ b/website/docs/api-reference/ros2/install.mdx @@ -3,6 +3,8 @@ title: Installation sidebar_position: 1 --- +import Tabs from '@theme/Tabs'; +import TabItem from '@theme/TabItem'; import TOCInline from '@theme/TOCInline'; # Installing ROS 2 @@ -22,14 +24,15 @@ Select a [ROS 2 distribution](https://docs.ros.org/en/humble/Releases.html) to i ### Install Here are the steps for Humble and Jazzy. + For other distributions, please follow the installation guide on the website. -:::note Quick Link -Replace `humble` with your chosen ROS 2 distro: https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html + +:::note +Quick Link +Replace `humble` with your chosen ROS 2 distro: [ROS2 Installation Guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html) ::: -import Tabs from '@theme/Tabs'; -import TabItem from '@theme/TabItem'; diff --git a/website/docs/api-reference/setup/2-can-setup.mdx b/website/docs/api-reference/setup/2-can-setup.mdx index 2694fd59..7ee2b146 100644 --- a/website/docs/api-reference/setup/2-can-setup.mdx +++ b/website/docs/api-reference/setup/2-can-setup.mdx @@ -41,10 +41,20 @@ The OpenArm CAN library includes convenient setup commands, example usage: ```bash # For CAN 2.0 -openarm-can-configure-socketcan can0 +openarm-can-configure-socketcan-4-arms # For CAN FD at 5mbps (recommended) -openarm-can-configure-socketcan can0 -fd -b 1000000 -d 5000000 +openarm-can-configure-socketcan-4-arms -fd -b 1000000 -d 5000000 +``` + +Alternatively, use `openarm-can-cli` to configure individual interfaces. For detailed usage, see [CAN CLI Reference ](../cli#can_configure--configure-can-interface). + +```bash +# CAN FD at 5Mbps (recommended) +openarm-can-cli -i can0 can_configure + +# CAN 2.0 +openarm-can-cli -i can0 can_configure --no-fd ``` ### Option 2: setup manually diff --git a/website/docs/api-reference/setup/4-motor-config.mdx b/website/docs/api-reference/setup/4-motor-config.mdx index 2f5833d9..e0a7502a 100644 --- a/website/docs/api-reference/setup/4-motor-config.mdx +++ b/website/docs/api-reference/setup/4-motor-config.mdx @@ -20,49 +20,32 @@ We now need to complete the motor setup process. This involves three key tasks: 2. [Set Zero Position](#step-2-zero-position-calibration) 3. [Verify Motor Communication](#step-3-verify-motor-communication) - ## Step 1: Set Motor Baudrate :::warning -- Be sure to use the CAN interface on 2.0 during the baudrate setup regardless of your target baudrate. Configure it back to your target baudrate after this step is finished. -- Motor parameters has a 10000 time write limits, it should not be run frequently in scripts. +Motor parameters has a 10000 time write limits, it should not be run frequently in scripts. ::: -Set the baudrate for each motor (replace `1` with your motor CAN ID and `5000000` with desired baudrate): +:::note +Before changing the baudrate, make sure the CAN interface is configured to match the motor's **current** baudrate. See [Setup CAN Interface](./can-setup) for details. +::: -Set temporarily: -```bash -openarm-can-change-baudrate --baudrate 5000000 --canid 1 --socketcan can0 -``` -Save permanently: +For detailed usage, see [CAN CLI Reference change_baud](../cli#change_baud--change-motor-baudrate). ```bash -openarm-can-change-baudrate --baudrate 5000000 --canid 1 --socketcan can0 --flash -``` - -**Important:** After completing the baudrate configuration, reconfigure your CAN interface to match the motor's new baudrate setting for proper communication if necessary. +# Set temporarily +openarm-can-cli -i can0 change_baud -b 5000000 -c 1 -**Supported Baudrates:** - -| Option | Baudrate | Description | -|--------|----------|-------------| -| 0 | 125000 | 125 kbps | -| 1 | 200000 | 200 kbps | -| 2 | 250000 | 250 kbps | -| 3 | 500000 | 500 kbps | -| 4 | 1000000 | 1 mbps | -| 5 | 2000000 | 2 mbps | -| 6 | 2500000 | 2.5 mbps | -| 7 | 3200000 | 3.2 mbps | -| 8 | 4000000 | 4 mbps | -| 9 | 5000000 | 5 mbps | +# Save permanently +openarm-can-cli -i can0 change_baud -b 5000000 -c 1 --save +``` ## Step 2: Zero Position Calibration :::info -Physically position the arms to roughly match the zero position shown below before running calibration. +Physically position the arms to roughly match the zero position shown below before running calibration. For grippers, the closed position is used as zero by default. ::: @@ -75,55 +58,66 @@ When setting up in a Leader-Follower configuration, please run the zero position Safety First! The robot will move automatically when calibration starts. Wear safety gear, clear the workspace, and be ready to emergency stop. ::: -The zero position calibration runs **one arm at a time**. -Run the zero position calibration (this defaults to can 0, right arm): +The zero position calibration runs **one arm at a time**. + +Run the zero position calibration (this defaults to can0, right arm): + ```bash +# v2.0 (default) openarm-can-zero-position-calibration + +# v1.0 +openarm-can-zero-position-calibration --robot-version v1 ``` + For specific arms, run: + ```bash -# Leader-side Left -openarm-can-zero-position-calibration --canport can1 --arm_side left_arm -``` -```bash +# Leader-side Left (v2.0) +openarm-can-zero-position-calibration --canport can1 --arm-side left_arm + +# Leader-side Left (v1.0) +openarm-can-zero-position-calibration --canport can1 --arm-side left_arm --robot-version v1 + # Follower-side Right -openarm-can-zero-position-calibration --canport can2 --arm_side right_arm -``` -```bash +openarm-can-zero-position-calibration --canport can2 --arm-side right_arm + # Follower-side Left -openarm-can-zero-position-calibration --canport can3 --arm_side left_arm +openarm-can-zero-position-calibration --canport can3 --arm-side left_arm ``` +
-