Broadcast robot state to tf2, and other improvements (mike4192#36)

* WIP publishing robot state * WIP transforms * WIP tf2 transforms * Working robot tf2 broadcasting * Working odometry * Updated submodule pointer to cpp kinematics library * Updated config file with more comments * Optional publishing of odometry by parameter, selectable 8 and 4 phase gaits * Documentation update, photos, gait merge fix * Note about alternate gait * Thingverse link and lidar mount image * Changelog update, additional documentation, and new animation * formatting adjust * Modified parameters for trot gait
curui · Jan 8, 2021 · dff0080 · dff0080
1 parent aeffd08
commit dff0080
Show file tree

Hide file tree

Showing 23 changed files with 607 additions and 111 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,6 +7,20 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+## [0.3.0] - 2021-1-07
+
+### Added
+- Publishing of robot state and joints via tf2
+- Open loop calculated odometry by integrating rate commands 
+- Additional documentation about hardware
+- Links to 3d printed parts for mounting lidar scanner
+
+### Changed
+- Merged alternate gait into master, configurable by parameters
+
+#### Deprecated
+- Deprecating alternate-gait branch
+
 ## [0.2.0] - 2021-1-01
 
 ### Changed

diff --git a/README.md b/README.md
@@ -1,6 +1,8 @@
 # Spot Micro Quadruped Project
 
 ![Spot Micro Walking](assets/spot_micro_walking.gif)
+![RVIZ](assets/rviz_animation.gif)
+
 
 Video of robot: https://www.youtube.com/watch?v=S-uzWG9Z-5E
 
@@ -14,35 +16,23 @@ Video of robot: https://www.youtube.com/watch?v=S-uzWG9Z-5E
 ## Overview
 This project is the source code for a Spot Micro quadruped, a 4 legged open source robot. This code is capable of keyboard control of a 3d printed spot micro robot with sit, stand, angle command, and walk capability. The software is implemented on a Raspberry Pi 3B running Ubuntu 16.04 with ROS Kinetic installed.
 
-The software is composed of C++ and python nodes in a ROS framework.
+The software is composed of C++ and Python nodes in a ROS framework.
 
 #### Hardware:
-The frame utilized is the Thingverse Spot Micro frame developed by KDY0523. See the thingverse page below for additional details for assembly hardware. The files for cls6336hv servos were printed which also fit the hv5523mg servos I used.
-https://www.thingiverse.com/thing:3445283
+The frame utilized is the Thingverse Spot Micro frame developed by KDY0523. See [the thingverse page](https://www.thingiverse.com/thing:3445283) for additional details for assembly hardware. The files for cls6336hv servos were printed which also fit the hv5523mg servos I used.
 
 Component List:
 * Computer: Raspberry Pi 3B 
 * Servo control board: PCA9685, controlled via i2c
 * Servos: 12 x PDI-HV5523MG
-* LCD Panel: 16x2 i2c LCD panel
+* LCD Panel: 16x2 i2c LCD panel (Optional)
 * Battery: 2s 4000 mAh Lipo, direct connection to servo board for servo power
 * UBEC: HKU5 5V/5A ubec, used as 5v voltage regulator to power raspberry pi, lcd panel, pca9685 control board.
+* Lidar: RPLidar A1
+* Custom 3d printed parts for mounts and reinforcements
+
+More information about the hardware, including the additional custom 3d printed parts, coordinate system information, and sample hardware installation photos, can be found in the [additional hardware description](docs/additional_hardware_description.md) document.
 
-Servos are connected in the following order to the PCA 9685 control board:
-1. Right front knee
-2. Right front shoulder
-3. Right front hip
-4. Right back knee
-5. Right back shoulder
-6. Right back hip
-7. Left back knee
-8. Left back shoulder
-9. Left back hip
-10. Left front knee
-11. Left front shoulder
-12. left front hip
-
-A custom shoulder assembly was created with an additional piece to provide more reinforcement to the shoulder axis. The modified shoulder assembly parts can be found [at this thingverse page.](https://www.thingiverse.com/thing:4591999) Additionally, a plain center mounting platform and two convenience platforms for the RPI 3 and PCA9685 boards can be found [at this thingverse page](https://www.thingiverse.com/thing:4596267). The latter are affixed to the main platform by double sided foam tape.
 
 #### Software:
 This repo is structured as a catkin workspace in a ROS Kinetic envivornment on Ubuntu 16.04. The software may not work or compile outside this environment. Raspberry pi images preloaded with Ubuntu 16.04 and a ROS Kinetic installation can be found via ubiquity robotics. [See ubiquity robotics webpage](https://downloads.ubiquityrobotics.com/) for download, setup, and wifi setup instructions. It is suggested to also install ROS Kinetic on a Ubuntu 16.04 linux installation/dual boot/virtual machine on a PC for development and for running control nodes.
@@ -95,7 +85,7 @@ CMakeFiles/Makefile2:2343: recipe for target 'ros-i2cpwmboard/CMakeFiles/i2cpwm_
 ```
 
 #### Note on Walking Gaits
-The gait implemented on master is a 8 phase gait that incorporates body movement which helps maintain balance and stability. An alternate trot gait, where the diagonal legs move simultaneously, can achieve faster walking speeds, but is less stable and requires careful positioning of the robot's center of mass. The trot gait is the one depicted in the animation at the top of this document, and can be found on the branch to this project titled `alternate_gait`. The 8 phase gait can be observed in the linked Youtube video.
+The default gait implemented is a 8 phase gait that incorporates body movement which helps maintain balance and stability. An alternate trot gait, where the diagonally opposite legs move simultaneously, can achieve faster walking speeds, but is less stable and requires careful positioning of the robot's center of mass. The trot gait is the one depicted in the animation at the top of this document. See the `spot_micro_motion_cmd` node's config file for information on how to switch to the trot gait. The 8 phase gait can be observed in the linked Youtube video.
 
 ## General Instructions
 This section attemps to be a full set of instructions to get a spot micro robot calibrated and running with this code.
@@ -114,8 +104,9 @@ Open at least two terminal wndows, with at least one ssh'ed to the raspberry pi.
         * `debug_mode:=true`: Overrides the `debug_mode` parameter to true. Useful in combination with `run_standalone` for running or debugging the motion command node on a PC instead of the RPi
         * `run_lcd:=true`: Runs the lcd monitor node to display simple state information on a LCD monitor if installed. Only works running on a RPi
     * Command line arguments for `keyboard_command.launch`:
+        * `run_rviz:=true`: Starts RVIZ and displays a 3d model of the robot with state update in real time.
         * `run_plot:=true`: Runs python plotting node to display a stick figure wireframe model of the spot micro robot state in real time. Must be run on a PC. Requires updated matplot lib python library (matplotlib 2.2.5) and updated numpy library (numpy 1.16.6).
-        * `run_rviz:=true`: (**NOT YET FUNCTIONAL**) Starts RVIZ and displays a 3d model of the robot with state update in real time. Future capability.
+
 
 Stopping and exiting the keyboard command launch file may require typing `quit` and pressing `Ctrl-C`, as well as closing any plot windows if plotting was utilized. 
 
@@ -160,16 +151,21 @@ A yaml confguration file is used for holding various software configuration sett
 
 * **servo_move_keyboard**: A python node that can be used in conjuction with the i2cpwm_board node to manually command an individual servo via keyboard controls. Can be used for servo calibration to build the servo configuration dictionary.
 
-* **spot_micro_rviz**: **WORK IN PROGRESS**. A node to launch RVIZ and show a visualization of the spot micro model, as well as mapping and navigational elements in the future. The `show_and_move_model` launch file can be launched standalone to show a manually moveable spot micro model via GUI sliders. 
+* **spot_micro_rviz**: A node to launch RVIZ and show a visualization of the spot micro model, as well as mapping and navigational elements in the future. The `show_and_move_model` launch file can be launched standalone to show a manually moveable spot micro model via GUI sliders. 
 
 Note that the servo control node `i2cpwm_board` should only be commanded by one node at one time. Thus `spot_micro_motion_command` and `servo_move_keyboard` should be run exclusionary; only one should ever run at one time.
 
 ## Additional Project Components
 #### URDF Model
-The project contains a URDF model of the spot micro platform, along with a custom set of stl files  for visualization. The URDF file was pulled from Florian Wilk's repo, noted at the end of this README, and modified to change the coordinate system orientation, and the dimensions to match dimensions of my spot micro robot. Currently this urdf file is **only** used for RVIZ visualization of the spot micro model. This URDF model should not be treated as perfectly accurate representation of the robot's geometry, nor should the STL files for visualization be used for 3d printing. Use the noted Thingverse files instead. 
+The project contains a URDF model of the spot micro platform, along with a custom set of stl files  for visualization. The URDF file was pulled from Florian Wilk's repo, noted at the end of this README, and modified to change the coordinate system orientation, and the dimensions to match dimensions of my spot micro robot. Currently this urdf file is **only** used for RVIZ visualization of the spot micro model. This URDF model should not be treated as perfectly accurate representation of the robot's geometry, nor should the STL files in this repo be used for 3d printing. Use the noted Thingverse files instead. 
 
 The URDF model is defined as a `xacro` file, which is a way to define urdf file using macros to automate certain generative actions. The xacro file is located in the `spot_micro_rviz/urdf` directory. A urdf file can be generated from the `.xacro` file for inspection or use, if needed, via running `xacro` after sourcing a ROS development environment. 
 
+#### TF2 Publishing and Odometry
+Robot state transforms are published via TF2. Some primary frames of interest are `base_footprint` and `base_link`. `base_footprint` is a coordinate frame at zero height at the base of the robot frame. `base_link` is the coordinate frame fixed to the body center of the robot, and moves and rotates with body motion. 
+
+An odometry frame, `odom`, is optionally available and can be enabled via a configurable parameter in the `spot_micro_motion_cmd.yaml` file. If enabled, `odom` is parent to the `base_footprint` frame.  **Note that odometry is grossly inaccurate and not calibrated whatsoever**. It is a pure integration of robot rate commands and thus drifts unbounded with errors over time. It is provided for any useful purpose it may serve.
+
 
 
 ## Future Work

diff --git a/assets/custom_shoulder_assembly.jpg b/assets/custom_shoulder_assembly.jpg
diff --git a/assets/kinematic_coord_system.jpg b/assets/kinematic_coord_system.jpg
diff --git a/assets/lidar_mount.jpg b/assets/lidar_mount.jpg
diff --git a/assets/robot_bottom.jpg b/assets/robot_bottom.jpg
diff --git a/assets/robot_top.jpg b/assets/robot_top.jpg
diff --git a/assets/robot_top_no_servos.jpg b/assets/robot_top_no_servos.jpg
diff --git a/assets/rpi_platform.jpg b/assets/rpi_platform.jpg
diff --git a/assets/rviz_animation.gif b/assets/rviz_animation.gif
diff --git a/assets/tf2_coord_system.jpg b/assets/tf2_coord_system.jpg
diff --git a/docs/additional_hardware_description.md b/docs/additional_hardware_description.md
@@ -0,0 +1,51 @@
+# Additional Hardware Description
+This document provides some addition description of the harware for the spot micro robot.
+
+* [Custom 3d Printed Parts](#custom-3d-printed-parts)
+* [Coordinate Frames](#coordinate-frames)
+* [Sample Hardware Install Photos](#sample-hardware-installation-photos)
+
+
+## Custom 3d Printed Parts
+
+Several custom 3d printed pieces were created to expand on KDY's original design. These include a custom shoulder assembly to provide additional reinforcement, and several mounting platforms for convenience. 
+
+The modified shoulder assembly, shown below, includes an additional plastic piece to provide additional reinforcement to the shoulder axis. It requires printing an antire set of shoulder joint parts, which can be found [at this thingverse page](https://www.thingiverse.com/thing:4591999). Two sets must be printed mirrored for the opposite side's legs. The modified shoulder will require an additional 8x M3x10 screws, 8x M3 nuts, and 4x F625zz bearings in total for assembly.
+
+
+![Custom Shoulder Assembly](../assets/custom_shoulder_assembly.jpg)
+
+A plain center body platform, and two convenience platforms for mounting the RPI 3 and PCA9685 boards can be found [at this thingverse page](https://www.thingiverse.com/thing:4596267). The Raspberry Pi platform is shown below. It can be adhered to the center platform by double sided foam tape. Small wood screws can be used are used t attach the RPi3 and PCA9685 to these platforms.
+
+![RPI platform](../assets/rpi_platform.jpg)
+
+A custom platform and mount adapter for a RPLidar A1 can be found at [at this thingverse page](https://www.thingiverse.com/thing:4713180). Design courtesy of Maggie Mathiue. Like the platforms above, the base can be adhered to the center body platform by double sided foam tape. The mount adapter is attached to a RPLiadr A1 by 4x M2.5x8 screws and to the bottom platform by small wood screws.
+
+![lidar mount](../assets/lidar_mount.jpg)
+
+
+
+## Coordinate Frames
+There are many coordinate frames on the spot micro frame (one at each joint!), but some of the more important frames are described here.
+
+
+#### Kinematics Coordinate Frame
+With regard to kinematics of the robot frame, the coordinate frame is oriented as follows: X positive forward, Y positive up, Z positive left. This frame was is only relevant if working on the kinematic calculations for the robot. It is the same coordinate frame as used in the paper sourced for the inverse kinematic calculations for this project ("Inverse Kinematic Analysis Of A Quadruped Robot").
+
+![Kinematic coordinate system](../assets/kinematic_coord_system.jpg)
+
+#### TF2 Coordinate Frame 
+The TF2 coordinate frames is the base robot body coordinate frame used for all transforms published to TF2 within the ROS framework. This is the robot coordinate frame of interest with regard to mapping and navigation. This frame is oriented as follows: X positive forward, Y positive left, Z positive up.
+
+![tf2 coordinate system](../assets/tf2_coord_system.jpg)
+
+
+## Sample Hardware Install Photos
+The following photos a sample installation of components on the spot micro frame.
+
+
+![Robot top no servos](../assets/robot_top_no_servos.jpg)
+
+![tf2 coordinate system](../assets/robot_top.jpg)
+
+![tf2 coordinate system](../assets/robot_bottom.jpg)
diff --git a/docs/servo_calibration.md b/docs/servo_calibration.md
@@ -1,6 +1,7 @@
 # Servo Calibration Guide
 
-This document provides a comprehensive guide to calibrate servos on a spot micro frame and create a cooresponding servo configuration ditionary the ROS control software requires. A spreadhseet named `servo_calibration_spreadsheet.ods` is included in this repo in the `docs` directory as an aid for calculating the servo configuration values.
+This document provides a comprehensive guide to install and calibrate servos on a spot micro frame, and create a cooresponding servo configuration the ROS motion command node requires. A spreadhseet named `servo_calibration_spreadsheet.ods` is included in this repo in the `docs` directory as an aid for calculating the servo configuration values.
+
 
 The servo configuration dictionary is contained within the configuration file `spot_micro_motion_cmd.yaml` and holds servo configuration values as shown below:
 ```yaml
@@ -34,6 +35,20 @@ Servo's are defined by a abbreviation and number referring to their location and
 
 
 ### Servo Installation
+Servos must be connected in the following order to the PCA 9685 control board:
+1. Right front knee
+2. Right front shoulder
+3. Right front hip
+4. Right back knee
+5. Right back shoulder
+6. Right back hip
+7. Left back knee
+8. Left back shoulder
+9. Left back hip
+10. Left front knee
+11. Left front shoulder
+12. left front hip
+
 It is reccomended to install servos in the spot micro frame when they are powered and commanded to their center position. The joint which a servo is installed in should be approximately positioned at it's "nuetral" stance, the position about which most leg motion will occur. This ensures maximum servo travel will be available around the typical joint command angles. The two figures below roughly depict the joint orientations for which servo's should be installed when at their center position. 
 
 ![Side View Neutral Positions](../assets/1_robot_right_links.png)

diff --git a/spot_micro_motion_cmd/CMakeLists.txt b/spot_micro_motion_cmd/CMakeLists.txt
@@ -12,6 +12,9 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   geometry_msgs
   i2cpwm_board
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 add_subdirectory(libs/spot_micro_kinematics_cpp)
@@ -107,7 +110,7 @@ add_subdirectory(libs/spot_micro_kinematics_cpp)
 catkin_package(
   INCLUDE_DIRS include/spot_micro_motion_cmd
 #  LIBRARIES spot_micro_motion_cmd
-  CATKIN_DEPENDS roscpp std_msgs geometry_msgs
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs tf2 tf2_ros
 #  DEPENDS system_lib
 )
 
@@ -146,7 +149,7 @@ target_link_libraries(smfsm
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(${PROJECT_NAME}_node src/spot_micro_motion_cmd.cpp src/spot_micro_motion_cmd_node.cpp)
+add_executable(${PROJECT_NAME}_node src/spot_micro_motion_cmd.cpp src/spot_micro_motion_cmd_node.cpp src/utils.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the