Cubic Doggo 06R High Mobility is an upgrade from the base Cubic Doggo. It receives several mechanical improvements and applies an IMU for balance. The upgraded CADv2 files can be found in GitHub directory/Printables.
Demos: wobbly IMU (Reddit/Reddit), standing IMU tunning (Reddit/Reddit), leg lifting (Reddit), walking with IMU (Reddit)
- Left: the following shows the high-lift mode available after all the mechanical improvements. The black notepad in the center is 1cm in height, and the robot is able to land on it with all 4 of its feet. However, you can see that landing off the notepad gives a rather high impact load to the landing leg.
- Right: walking with IMU balance correction based on BNO055, controlled using PID (technically on kP to avoid diverging oscillation). It's walking down from the notepad because the current IMU system cannot support the high-lift mode. Although the effect is not obvious, the robot is constantly trying to balance itself after standing up and during walking.
- Note also that a servo (or servos) from the robot's back-right leg is likely damaged/performance-reduced from dimension collisions during the test using the spring/compliant feet (see Reddit)
- Support on the 1st servos on both sides
- Support on the 3rd servos. These joints always broke first
- Spring-like feet (Reddit)
- Smaller feet tip. Swinging less mass should cause less wobbling
- Start using PLA+
Also updated the initial leg position such that the center of mass is optimized at trotting in place.
| device | models | count | specification |
|---|---|---|---|
| IMU | Adafruit BNO055 [ADA2472] | 1 | in-system Kalman Filter, but rate capped at 100 Hz |
| IMU | BMI088 | 1 | rate available at 200 Hz after ROS2 imu_filter_madgwick |
| IMU | ICM-42688-P | 1 | rate available at 200-400 Hz after ROS2 imu_filter_madgwick |
So far, only BNO055 is tested. Future incorporation with BMI088 or ICM-42688-P will likely improve the IMU performance: the current PID oscillation is potentially caused by delayed IMU correction at the BNO055 readout rate set of 50 Hz.
Following link. It's a bit outdated, so do change up a bit and actually connect via I2C instead for RaspPi > 3:
- Vin to 1
- GND to 9
- SDA to 3
- SCL to 5
- RST to 11
Then do:
sudo apt update && sudo apt install -y i2c-tools gpiod libgpiod-dev
sudo gpiodetect
sudo gpioset gpiochip4 17=1 # enable reset pin
sudo i2cdetect -y 1 # if 28 lights up, it's alive
sudo usermod -aG i2c $USER
sudo usermod -aG dialout $USER
sudo reboot
i2cdetect -y 1
To test BNO055 without ROS:
sudo apt install -y python3-venv python3-pip python3-lgpio
cd CubicDoggo_06R/python_test_scripts
python3 -m venv env
source env/bin/activate
pip install adafruit-circuitpython-bno055 rpi-lgpio
python bno_test.py
To test BNO055 with ROS:
cd CubicDoggo_06R
colcon build
source install/setup.bash
ros2 run my_robot_peripheral imu_bno055_node
# on another terminal
ros2 topic echo /imu/euler # show IMU content
ros2 topic hz /imu/euler # show IMU read speed
ls /dev/input/js*
# output: /dev/input/js0
# otherwise do: sudo jstest /dev/input/js0
ros2 run joy joy_enumerate_devices
# if no device found
sudo usermod -aG input $USER
sudo reboot
cd CubicDoggo_06R
colcon build
source install/setup.bash
ros2 run joy joy_node
# on another terminal
ros2 run my_robot_controller cubic_doggo_joy_control
# on yet another terminal
ros2 node info /cubic_doggo_joy_control
ros2 topic info /joy --verbose
ros2 topic echo /joy
for d in /sys/class/hwmon/hwmon*; do echo -n "$d: "; cat "$d/name"; done # find the correct path for power alarm
# update /home/cubicdoggo/Documents/CubicDoggo_06R/src/my_robot_bringup/launch/cubic_doggo.with_lifecycle.launch.py
echo timer | sudo tee /sys/class/leds/ACT/trigger # trigger LED first
sudo chmod -R 777 /sys/class/leds/ACT/ # enable LED to indicate battery condition
sudo chmod -R 777 /sys/class/hwmon/hwmon*/ # enable reading of power alarm/voltage
# test the node
echo 0 > /tmp/fake_alarm # create a fake alarm by
ros2 run my_robot_peripheral rasp_pi_peripheral_node --ros-args -p alarm_path:=/tmp/fake_alarm
echo 1 > /tmp/fake_alarm # create a fake alarm by
Follow this GitHub section to set up the servos.
Run the following to view the moving parts:
cd CubicDoggo_06R
colcon build
source install/setup.bash
ros2 launch my_robot_description cubic_doggo.rviz.launch.xacro.py
Notice that the placement of the first servo is different between the front and back legs.
Under: CubicDoggo_06R/src/my_robot_description/urdf/cubic_doggo.ros2_control.xacro
- use
<plugin>mock_components/GenericSystem</plugin>if just want rViz - use
<<plugin>cubic_doggo_namespace/HardwareInterfaceU2D2_cubic_doggo</plugin>if want to control hardware
Under: CubicDoggo_06R/src/my_robot_bringup/launch/cubic_doggo.with_lifecycle.launch.py
-
choose from:
executable="cubic_doggo_lifecycle"for no IMUexecutable="cubic_doggo_lifecycle_imuNode"for standing with an IMU nodeexecutable="cubic_doggo_lifecycle_imuController"for standing with an IMU controller usingros2_controlexecutable="cubic_doggo_lifecycle_imuController_walking"for standing/walking with an IMU controller usingros2_controlexecutable="cubic_doggo_lifecycle_imuController_walkingToggle"to start with no IMU, but can toggle on for standing/walking with an IMU controller usingros2_control
-
comment out
imu_nodeunless usingexecutable="cubic_doggo_lifecycle_imuNode" -
comment out
rviz_nodeif don't want rViz to show -
comment out
joy_driver_nodeandjoy_controller_nodeif don't want the joystick controller -
comment out
peripheral_nodeif don't want warning from low power
Start the robot (skip launching rviz_node, joy_driver_node, or joy_controller_node if needed):
cd CubicDoggo_06R
colcon build
source install/setup.bash
# comment in rvis node in CubicDoggo_06R/src/my_robot_bringup/launch/cubic_doggo.with_lifecycle.launch.py
ros2 launch my_robot_bringup cubic_doggo.with_lifecycle.launch.py
# on another terminal
ros2 topic pub -1 /leg_set_named example_interfaces/msg/String "{data: "rest"}"
ros2 topic pub -1 /leg_set_named example_interfaces/msg/String "{data: "stand"}"
ros2 topic pub -1 /leg_set_named example_interfaces/msg/String "{data: "sit"}"
ros2 topic pub -1 /leg_set_named example_interfaces/msg/String "{data: "bow"}"
ros2 topic pub -1 /leg_set_joint example_interfaces/msg/Float64MultiArray "{data: [0, 3.14, 3.14, 3.54]}"
ros2 topic pub -1 /leg_set_pose my_robot_interface/msg/CubicDoggoLegPoseTarget "{leg_index: 0, x: -0.092, y: 0.053, z: 0.135}"
ros2 service call /leg_walk_toggle std_srvs/srv/SetBool "{data: true}"
ros2 service call /leg_walk_toggle std_srvs/srv/SetBool "{data: false}"
Check out all the gait/IMU options under CubicDoggo_06R/src/my_robot_commander/src/cubic_doggo_lifecycle_imuController_walking.cpp:
# rclcpp::get_logger("").set_level(rclcpp::Logger::Level::Warn); // for silencing output
...
# aw_strat.i_max = 0.05;
# aw_strat.i_min = -0.05;
# double kP = 0.0016, kI = 0.0001, kD = 0.0; // NOTE: PID
# double corr_v_thres = 3.0, corr_d_thres = 1.0; // threshold on pitch/roll and their vel
# double corr_z_limit_kD = 0.01, corr_z_limit = 0.025; // limit on +/- z
# double pitch_shift = 0.643, roll_shift = -2.917; // basic on-support reference
# auto loop_rate = rclcpp::WallRate(update_rate_); // Hz, for consistent loop rate
# double maxVelScale = 1.0, maxAccScale = 1.0;
# int waypoint_N_walk = 40; // number of waypoints for each cycle
# double waypoint_dt_walk = 0.5/double(update_rate_); // second for each waypoint, to match loop rate
# int waypoint_N_stand = 1; // standing require immidiate reaction
# double waypoint_dt_stand = 1.0/double(update_rate_); // standing require faster trajectory
# double IK_bufferTime = 0.10; // time at end of cycle buffer for IK calc
# double swing_fraction = 0.32; // creep < 0.25 < stable trot < 0.5 < trot
# double lift = 0.02, x_stride_max = 0.02, y_stride_max = 0.03, x_shift = 0.0, y_shift = -0.007;
chmod +x /home/cubicdoggo/Documents/CubicDoggo_06R/start_robot.sh
sudo cp /home/cubicdoggo/Documents/CubicDoggo_06R/robot_startup.service /etc/systemd/system/robot_startup.service
sudo chmod 644 /etc/systemd/system/robot_startup.service
sudo systemctl enable robot_startup.service # now will start at reboot
sudo systemctl daemon-reload # reload whenever there is a change
# sudo systemctl restart robot_startup.service # restart, even if is rrunning
# sudo systemctl stop robot_startup.service # stop right now, kill all relevant nodes
# sudo systemctl disable robot_startup.service # disable at reboot
# journalctl -u robot_startup.service -n 100 > start_robot_output.txt # to check the output
# journalctl -u robot_startup.service -f
Run the robot first:
cd CubicDoggo_06R
colcon build
source install/setup.bash
ros2 launch my_robot_bringup cubic_doggo.with_lifecycle.launch.py
# on another terminal for joint states
ros2 topic echo /joint_states
# on another terminal for IMU
ros2 topic echo /imu/euler
To track the values (remember to connect the RaspPi to a monitor):
# on another terminal for plotjuggler
sudo apt install ros-jazzy-plotjuggler-ros
ros2 run plotjuggler plotjuggler
# load CubicDoggo_06R/monitor_plot.xml
# or starting over again:
## start => locate the following
### /imu/data
### /imu/euler
### /joint_states
## OK
## on the left panel, expand /imu and /joint_states and drag the variable to the center for live plotting
Note that to enable plotting, the robot needs an HDMI cable to connect to the monitor, a mouse, a keyboard, and a direct power source (it's rather energy-consuming). The controller is still remote. This means extra weights are placed on the robot, and so what's being plotted is not exactly equivalent to battery mode. The photo shows the connections:
Configuration (update the code if direction differs):
roll, negative when tilting forward
pitch, negative when tilting leftward
Checking loop speed for latency, including IK calculation and IMU readout:
ros2 topic hz /joint_states
Standing up and then changing to a slope, with Kp parameter only. Never correct to the exact values:
Standing up and then changing to a slope, with full PID parameters. Large oscillations:
Walking gait, trotting in place:
Walking gait with IMU, trotting in place:
High-lift walking gait, trotting in place:
- Dr Jon EA: Pico & Pico 2, The BNO055: At Last, an IMU I Can Tame Quickly - C/C++ (YouTube)
- ROS1 Packages for CHAMP Quadruped Controller (GitHub) => node based IMU control with classical walk gait
- Spot Micro (GitHub, link) => node based IMU control with classical walk gait
- Pupper V3 (link) => ros2_control based IMU control with RL walk gait