├── .gitignore ├── .gitmodules ├── CHANGELOG.md ├── CMakeLists.txt ├── LICENSE ├── README.md ├── assets ├── 10_left_link3_config_step_1.png ├── 11_left_link3_config_step_2.png ├── 12_robot_back_overview.png ├── 13_robot_back_angle_directions.png ├── 14_right_link1_config_step_1.png ├── 15_right_link1_config_step_2.png ├── 16_left_link1_config_step_1.png ├── 17_left_link1_config_step_2.png ├── 1_robot_right_links.png ├── 2_right_straight_links.png ├── 3_right_link_angles_example.png ├── 4_right_link2_config_step_1.png ├── 4_right_link2_config_step_2.png ├── 5_right_link3_config_step_1.png ├── 6_right_link3_config_step_2.png ├── 7_robot_left_overview.png ├── 8_left_link2_config_step_1.png ├── 9_left_link2_config_step_2.png ├── custom_shoulder_assembly.jpg ├── kinematic_coord_system.jpg ├── lidar_mount.jpg ├── robot_bottom.jpg ├── robot_top.jpg ├── robot_top_no_servos.jpg ├── rpi_platform.jpg ├── rviz_animation.gif ├── servo_calibration_spreadsheet.png ├── servo_move_prompt.png ├── spot_micro_slam.gif ├── spot_micro_walking.gif ├── state_machine.png ├── tf2_coord_system.jpg └── walking_and_slam.gif ├── docs ├── additional_hardware_description.md ├── joystick_control.md ├── servo_calibration.md ├── servo_calibration_spreadsheet.ods └── slam_information.md ├── lcd_monitor ├── CMakeLists.txt ├── launch │ └── lcd_monitor.launch ├── package.xml ├── scripts │ └── sm_lcd_node.py ├── setup.py └── src │ └── lcd_monitor │ ├── I2C_LCD_driver.py │ ├── __init__.py │ └── sm_lcd_driver.py ├── servo_move_keyboard ├── CMakeLists.txt ├── launch │ └── keyboard_move.launch ├── package.xml └── scripts │ ├── servoConfigTest.py │ └── servoMoveKeyboard.py ├── spot_micro_joy ├── CMakeLists.txt ├── launch │ └── everything.launch ├── package.xml └── scripts │ └── spotMicroJoystickMove.py ├── spot_micro_keyboard_command ├── CMakeLists.txt ├── launch │ └── keyboard_command.launch ├── package.xml └── scripts │ └── spotMicroKeyboardMove.py ├── spot_micro_launch ├── CMakeLists.txt ├── launch │ ├── keyboard_control_and_rviz.launch │ └── motion_control_and_hector_slam.launch └── package.xml ├── spot_micro_motion_cmd ├── CMakeLists.txt ├── config │ └── spot_micro_motion_cmd.yaml ├── data │ └── usage.txt ├── include │ └── spot_micro_motion_cmd │ │ ├── spot_micro_motion_cmd.h │ │ └── utils.h ├── launch │ └── motion_cmd.launch ├── package.xml └── src │ ├── rate_limited_first_order_filter │ └── rate_limited_first_order_filter.h │ ├── smfsm │ ├── command.h │ ├── spot_micro_idle.cpp │ ├── spot_micro_idle.h │ ├── spot_micro_stand.cpp │ ├── spot_micro_stand.h │ ├── spot_micro_state.cpp │ ├── spot_micro_state.h │ ├── spot_micro_transition_idle.cpp │ ├── spot_micro_transition_idle.h │ ├── spot_micro_transition_stand.cpp │ ├── spot_micro_transition_stand.h │ ├── spot_micro_walk.cpp │ └── spot_micro_walk.h │ ├── spot_micro_motion_cmd.cpp │ ├── spot_micro_motion_cmd_node.cpp │ └── utils.cpp ├── spot_micro_plot ├── CMakeLists.txt ├── launch │ └── start_plotting.launch ├── package.xml └── scripts │ └── spotMicroPlot.py └── spot_micro_rviz ├── CMakeLists.txt ├── launch ├── show_and_move_model_via_gui.launch ├── show_model.launch └── slam.launch ├── package.xml ├── rviz ├── mapping_demo.rviz ├── spot_micro.rviz └── spot_micro_slam.rviz └── urdf ├── gen_urdf.sh ├── spot_micro.urdf.xacro └── stl ├── backpart.stl ├── foot.stl ├── frontpart.stl ├── larm.stl ├── larm_cover.stl ├── lfoot.stl ├── lshoulder.stl ├── mainbody.stl ├── rarm.stl ├── rarm_cover.stl ├── rfoot.stl ├── rplidar_main.STL └── rshoulder.stl /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .vscode/ 3 | *.swp 4 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp"] 2 | path = spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp 3 | url = https://github.com/mike4192/spot_micro_kinematics_cpp.git 4 | [submodule "spot_micro_plot/scripts/spot_micro_kinematics_python"] 5 | path = spot_micro_plot/scripts/spot_micro_kinematics_python 6 | url = https://github.com/mike4192/spot_micro_kinematics_python.git 7 | [submodule "ros-i2cpwmboard"] 8 | path = ros-i2cpwmboard 9 | url = https://gitlab.com/bradanlane/ros-i2cpwmboard.git -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), 6 | and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). 7 | 8 | ## [Unreleased] 9 | 10 | ## [0.3.0] - 2021-1-07 11 | 12 | ### Added 13 | - Publishing of robot state and joints via tf2 14 | - Open loop calculated odometry by integrating rate commands 15 | - Additional documentation about hardware 16 | - Links to 3d printed parts for mounting lidar scanner 17 | 18 | ### Changed 19 | - Merged alternate gait into master, configurable by parameters 20 | 21 | #### Deprecated 22 | - Deprecating alternate-gait branch 23 | 24 | ## [0.2.0] - 2021-1-01 25 | 26 | ### Changed 27 | - Changed robot velocity command from a Vector3 message on a `speed_cmd` topic, to the more ROS conventional Twist message on a `cmd_vel` topic._ This affected `spot_micro_motion_cmd`, `spot_micro_keyboard_command`, and `lcd_monitor` packages 28 | 29 | ### Removed 30 | - Removed deprecated `spot_micro_walk` and `spot_micro_simple_command` python pacakges, as they are obsolete 31 | 32 | ## [0.1.0] - 2020-12-31 33 | 34 | ### Added 35 | - This changelog file 36 | - Launch files for packages that did not have them, and added added command line arguments to launch certain configurations 37 | - `spot_micro_rviz` package, which includes a urdf file defining the spot micro robot geometry. Currently this file and package will only be used for visualization. Not yet functional, reserved for future capability. 38 | 39 | ### Changed 40 | - Converted `i2c-pwmboard` package from a code copy to a git submodule 41 | 42 | ### Deprecated 43 | - `spot_micro_simple_command` and `spot_micro_walk` python packages will be deprecated soon 44 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 mike4192 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /assets/10_left_link3_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/10_left_link3_config_step_1.png -------------------------------------------------------------------------------- /assets/11_left_link3_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/11_left_link3_config_step_2.png -------------------------------------------------------------------------------- /assets/12_robot_back_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/12_robot_back_overview.png -------------------------------------------------------------------------------- /assets/13_robot_back_angle_directions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/13_robot_back_angle_directions.png -------------------------------------------------------------------------------- /assets/14_right_link1_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/14_right_link1_config_step_1.png -------------------------------------------------------------------------------- /assets/15_right_link1_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/15_right_link1_config_step_2.png -------------------------------------------------------------------------------- /assets/16_left_link1_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/16_left_link1_config_step_1.png -------------------------------------------------------------------------------- /assets/17_left_link1_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/17_left_link1_config_step_2.png -------------------------------------------------------------------------------- /assets/1_robot_right_links.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/1_robot_right_links.png -------------------------------------------------------------------------------- /assets/2_right_straight_links.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/2_right_straight_links.png -------------------------------------------------------------------------------- /assets/3_right_link_angles_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/3_right_link_angles_example.png -------------------------------------------------------------------------------- /assets/4_right_link2_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/4_right_link2_config_step_1.png -------------------------------------------------------------------------------- /assets/4_right_link2_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/4_right_link2_config_step_2.png -------------------------------------------------------------------------------- /assets/5_right_link3_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/5_right_link3_config_step_1.png -------------------------------------------------------------------------------- /assets/6_right_link3_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/6_right_link3_config_step_2.png -------------------------------------------------------------------------------- /assets/7_robot_left_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/7_robot_left_overview.png -------------------------------------------------------------------------------- /assets/8_left_link2_config_step_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/8_left_link2_config_step_1.png -------------------------------------------------------------------------------- /assets/9_left_link2_config_step_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/9_left_link2_config_step_2.png -------------------------------------------------------------------------------- /assets/custom_shoulder_assembly.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/custom_shoulder_assembly.jpg -------------------------------------------------------------------------------- /assets/kinematic_coord_system.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/kinematic_coord_system.jpg -------------------------------------------------------------------------------- /assets/lidar_mount.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/lidar_mount.jpg -------------------------------------------------------------------------------- /assets/robot_bottom.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/robot_bottom.jpg -------------------------------------------------------------------------------- /assets/robot_top.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/robot_top.jpg -------------------------------------------------------------------------------- /assets/robot_top_no_servos.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/robot_top_no_servos.jpg -------------------------------------------------------------------------------- /assets/rpi_platform.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/rpi_platform.jpg -------------------------------------------------------------------------------- /assets/rviz_animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/rviz_animation.gif -------------------------------------------------------------------------------- /assets/servo_calibration_spreadsheet.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/servo_calibration_spreadsheet.png -------------------------------------------------------------------------------- /assets/servo_move_prompt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/servo_move_prompt.png -------------------------------------------------------------------------------- /assets/spot_micro_slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/spot_micro_slam.gif -------------------------------------------------------------------------------- /assets/spot_micro_walking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/spot_micro_walking.gif -------------------------------------------------------------------------------- /assets/state_machine.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/state_machine.png -------------------------------------------------------------------------------- /assets/tf2_coord_system.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/tf2_coord_system.jpg -------------------------------------------------------------------------------- /assets/walking_and_slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/assets/walking_and_slam.gif -------------------------------------------------------------------------------- /docs/additional_hardware_description.md: -------------------------------------------------------------------------------- 1 | # Additional Hardware Description 2 | This document provides some addition description of the harware for the spot micro robot. 3 | 4 | * [Custom 3d Printed Parts](#custom-3d-printed-parts) 5 | * [Coordinate Frames](#coordinate-frames) 6 | * [Sample Hardware Install Photos](#sample-hardware-installation-photos) 7 | 8 | 9 | ## Custom 3d Printed Parts 10 | 11 | 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. 12 | 13 | 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. 14 | 15 | 16 | ![Custom Shoulder Assembly](../assets/custom_shoulder_assembly.jpg) 17 | 18 | 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. 19 | 20 | ![RPI platform](../assets/rpi_platform.jpg) 21 | 22 | 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. 23 | 24 | ![lidar mount](../assets/lidar_mount.jpg) 25 | 26 | 27 | 28 | ## Coordinate Frames 29 | There are many coordinate frames on the spot micro frame (one at each joint!), but some of the more important frames are described here. 30 | 31 | 32 | #### Kinematics Coordinate Frame 33 | 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"). 34 | 35 | ![Kinematic coordinate system](../assets/kinematic_coord_system.jpg) 36 | 37 | #### TF2 Coordinate Frame 38 | 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. 39 | 40 | ![tf2 coordinate system](../assets/tf2_coord_system.jpg) 41 | 42 | 43 | ## Sample Hardware Install Photos 44 | The following photos a sample installation of components on the spot micro frame. 45 | 46 | 47 | ![Robot top no servos](../assets/robot_top_no_servos.jpg) 48 | 49 | ![tf2 coordinate system](../assets/robot_top.jpg) 50 | 51 | ![tf2 coordinate system](../assets/robot_bottom.jpg) -------------------------------------------------------------------------------- /docs/joystick_control.md: -------------------------------------------------------------------------------- 1 | # Joystick control 2 | With the spot_micro_joy node it is possible to control the robot by a joystick. The default configuration is set up for 3 | an PS4 controller which kan be connected by bluetooth directly. If you want to use another controler you need at 4 | least 4 axes and 4 buttons. 5 | 6 | To test if you joystick is working you can use the jstest command line this: 7 | ``` 8 | ubuntu@spotmicro:~$ jstest /dev/input/js0 9 | Driver version is 2.1.0. 10 | Joystick (Wireless Controller) has 8 axes (X, Y, Rx, Ry, Z, Rz, Hat0X, Hat0Y) 11 | and 13 buttons (BtnX, BtnY, BtnTL, BtnTR, BtnTR2, BtnSelect, BtnStart, BtnMode, BtnThumbL, BtnThumbR, ?, ?, ?). 12 | Testing ... (interrupt to exit) 13 | Axes: 0: 0 1: 0 2: 0 3: 0 4:-32767 5:-32767 6: 0 7: 0 Buttons: 0:off 1:off 2:off 3:off 4:off 5:off 6:off 7:off 8:off 9:off 10:off 11:off 12:off ^C 14 | ``` 15 | 16 | To check if your controller is woking with ROS, mapped and calibrated correctly you might try to start the joy ROS Node by hand. 17 | If you need to calibrate your joystick, i recommend jstest-gtk. To persist you configuration run jscal-store. 18 | 19 | ``` 20 | ubuntu@spotmicro:~$ rosrun joy joy_node 21 | [ INFO] [1615052346.145750548]: Opened joystick: /dev/input/js0. deadzone_: 0.050000. 22 | ``` 23 | In another teminal take a look into the messages your joy_node emits 24 | ``` 25 | ubuntu@spotmicro:~$ rostopic echo joy 26 | header: 27 | seq: 1 28 | stamp: 29 | secs: 1615052430 30 | nsecs: 954517162 31 | frame_id: '' 32 | axes: [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 33 | buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] 34 | --- 35 | ``` 36 | All axes should send values between -1 and 1. Buttons are 1 or 0. 37 | 38 | Now you can check (and modify if needed) the mappings in ```spotMicroJoystickMove.py``` 39 | 40 | Once done, bring your robot in a save position and start ```roslaunch spot_micro_joy everything.launch``` which will 41 | start everything needed to operate the robot by joystick. 42 | 43 | It is a good idea to start with a low value for transit_angle_rl. Once everything is working fine you might increase it 44 | to get a more agile and responsive doggy :) -------------------------------------------------------------------------------- /docs/servo_calibration_spreadsheet.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/docs/servo_calibration_spreadsheet.ods -------------------------------------------------------------------------------- /docs/slam_information.md: -------------------------------------------------------------------------------- 1 | # SLAM Information 2 | This document provides additional information for running SLAM on a spot micro quadruped through this project. 3 | 4 | * [Required Setup](#required-setup) 5 | * [Generating a Map](#generating-a-map-frames) 6 | * [Saving a Map](#saving-a-map) 7 | * [Recording a Data Log](#recording-a-data-log) 8 | * [Reprocessing Scan Data Through Log Playback](#reprocessing-scan-data-through-log-playback) 9 | 10 | ![Walking and Slam](../assets/walking_and_slam.gif) 11 | Example of robot walking and mapping an environment. 12 | 13 | ## Required Setup 14 | Two prerequisite steps are required for SLAM: 15 | 1. Installing additional ROS packages 16 | 2. Elevating permissions of the USB port the lidar is connected to for it's ROS package 17 | 18 | Two additional ROS packages must be installed, and these must be installed on the Raspberry Pi. It is also reccomended to install these on a Linux host machine for use such as data reprocessing. The required packages are hector_slam and rplidar_ros, which can be installed with the following commands: 19 | ``` 20 | sudo apt-get install ros-kinetic-rplidar-ros 21 | sudo apt-get install ros-kinetic-hector-slam 22 | ``` 23 | See the following [website](http://wiki.ros.org/kinetic/Installation/Ubuntu) for additional information about setting up your sources list if the above commands do not work. 24 | 25 | Elevated permissions will need to be granted to the USB port the RPLidar is connected to. After connecting the RPLidar to one of the RPi's usb ports, find which USB port it is connected to using the following command: 26 | `ls -l /dev |grep ttyUSB` 27 | Elevate that ports authorit with the following command. Example if the port was `USB0`: 28 | `sudo chmod 666 /dev/ttyUSB0` 29 | 30 | This step may or may not need to be done everytime the RPi is power cycled. It seems to be applied permanent at least when using the Ubiquity Robotics RPi Ubuntu images with ROS. See [the following page](https://github.com/robopeak/rplidar_ros/wiki) for more details about setting uo the RPLidar A1 for use with the rplidar_ros package. 31 | 32 | Note that the RPLidar A1 will start rotating once it is plugged into a powered USB port through it's USB to serial board. It is possible to command the lidar to stop so it does not rotate when not in use via it's SDK, but this functionalty is not yet implemented on this project. 33 | 34 | This project assumes a RPLidar A1. If another Lidar is used, then a different lidar driver ROS package will be needed. 35 | 36 | 37 | ## Generating a Map 38 | It is reccomended to use the trot gait for robot motion when mapping with a lidar, as it is slightly smoother, albeit less stable, than the 8 phase gait. 39 | 40 | Open at least two terminal windows, with one ssh'ed to the raspberry pi. I reccomend using a terminal multiplexer such as `tmux` for convenience. Start the following launch files in the respective terminals: 41 | * `roslaunch spot_micro_launch motion_control_and_hector_slam.launch`: Run on the Raspberry Pi. Launches the i2c_pwmboard node, the robot's motion control node, hector_mapping, and the lidar driver node (rplidar_ros). 42 | * `roslaunch spot_micro_launch keyboard_control_and_rviz rviz_slam:=true` Run on a local machine. Launches the keyboard command node for issuing keyboard commands to the spot micro robot in the terminal, as well as rviz with a configuration to display the mapping process. 43 | 44 | After everything is launched, the robot can be manually directed to walk around an environment (such as a room or apartment) through the keyboard command node, and a map will be geerated and shown in real time through RVIZ, along with the robot's current and past positions. 45 | 46 | ## Saving a Map 47 | To save a map, such as to use it in the future for navigational purposes, run the robot until a satisfactory map is shown in RVIZ. Keep all nodes running, open another terminal in a ros environment on either the RPi or local linux host, and run the following command: 48 | `rosrun map_server map_saver -f my_map_filename` 49 | This will generate two files (my_map_filename.pgm and my_map_filename.yaml) in the same directory in which the command was run. A custom name can be provided in place of `my_map_filename`. See the [following website](http://wiki.ros.org/map_server) for more detals about ros map_server. 50 | 51 | Another type of map can be generated which is purely an image and is not useful for navigational purposes. This map can be generated by running the following command in a ros environment: 52 | `rostopic pub syscommand std_msgs/String "savegeotiff"` 53 | 54 | 55 | ## Recording a Data Log 56 | Recording a data log is useful for playing back and inspecting or analyzing data at a later point, as well as for reprocessing data which is described in the next section. 57 | 58 | The record a data log, make a new directory for storing logfiles: 59 | ``` 60 | mkdir ~/bagfiles 61 | cd ~/bagfiles 62 | ``` 63 | And record all rostopics with the following command: 64 | `rosbag record -a` 65 | 66 | Note that log files will be on the order of 1 GB in size per around 10 minutes of operation. 67 | 68 | ## Reprocessing Scan Data Through Log Playback 69 | A data log can be played back to reprocess data. One use case for this is to playback a log file with lidar scan data and regenerate a map again, and being able to use differnet mapping settings or an entirely different mapping package. 70 | 71 | As an example, the steps to playback a recorded dataset through hector slam are: 72 | 1. Playback a log using a command such as `rosbag play `. Additional command line arguments, such as `--topics ...` can be added to specify which topics to playback only. 73 | 2. Start the hector slam node and rviz to visualize the mapping process. This can be done through the following two launch commands: 74 | `roslaunch spot_micro_rviz slam.launch` 75 | `roslaunch spot_micro_launch motion_control_and_hector_slam.launch run_post_proc:=true` 76 | 77 | 78 | -------------------------------------------------------------------------------- /lcd_monitor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lcd_monitor) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | geometry_msgs 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs # Or other packages containing msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES lcd_monitor 108 | # CATKIN_DEPENDS rospy 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/lcd_monitor.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/lcd_monitor_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lcd_monitor.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /lcd_monitor/launch/lcd_monitor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /lcd_monitor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lcd_monitor 4 | 0.0.0 5 | The lcd_monitor package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | std_msgs 54 | rospy 55 | rospy 56 | geometry_msgs 57 | std_msgs 58 | rospy 59 | geometry_msgs 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /lcd_monitor/scripts/sm_lcd_node.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | from lcd_monitor.sm_lcd_driver import main 4 | 5 | # Node program, calls main function in spot_micro_simple_command python package 6 | if __name__ == '__main__': 7 | main() -------------------------------------------------------------------------------- /lcd_monitor/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['lcd_monitor'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # -*- coding: utf-8 -*- 4 | # Original code found at: 5 | # https://gist.github.com/DenisFromHR/cc863375a6e19dce359d 6 | 7 | """ 8 | Compiled, mashed and generally mutilated 2014-2015 by Denis Pleic 9 | Made available under GNU GENERAL PUBLIC LICENSE 10 | 11 | # Modified Python I2C library for Raspberry Pi 12 | # as found on http://www.recantha.co.uk/blog/?p=4849 13 | # Joined existing 'i2c_lib.py' and 'lcddriver.py' into a single library 14 | # added bits and pieces from various sources 15 | # By DenisFromHR (Denis Pleic) 16 | # 2015-02-10, ver 0.1 17 | 18 | """ 19 | 20 | # i2c bus (0 -- original Pi, 1 -- Rev 2 Pi) 21 | I2CBUS = 1 22 | 23 | # LCD Address 24 | ADDRESS = 0x27 25 | 26 | import smbus 27 | from time import sleep 28 | 29 | class i2c_device: 30 | def __init__(self, addr, port=I2CBUS): 31 | self.addr = addr 32 | self.bus = smbus.SMBus(port) 33 | 34 | # Write a single command 35 | def write_cmd(self, cmd): 36 | self.bus.write_byte(self.addr, cmd) 37 | sleep(0.0001) 38 | 39 | # Write a command and argument 40 | def write_cmd_arg(self, cmd, data): 41 | self.bus.write_byte_data(self.addr, cmd, data) 42 | sleep(0.0001) 43 | 44 | # Write a block of data 45 | def write_block_data(self, cmd, data): 46 | self.bus.write_block_data(self.addr, cmd, data) 47 | sleep(0.0001) 48 | 49 | # Read a single byte 50 | def read(self): 51 | return self.bus.read_byte(self.addr) 52 | 53 | # Read 54 | def read_data(self, cmd): 55 | return self.bus.read_byte_data(self.addr, cmd) 56 | 57 | # Read a block of data 58 | def read_block_data(self, cmd): 59 | return self.bus.read_block_data(self.addr, cmd) 60 | 61 | 62 | # commands 63 | LCD_CLEARDISPLAY = 0x01 64 | LCD_RETURNHOME = 0x02 65 | LCD_ENTRYMODESET = 0x04 66 | LCD_DISPLAYCONTROL = 0x08 67 | LCD_CURSORSHIFT = 0x10 68 | LCD_FUNCTIONSET = 0x20 69 | LCD_SETCGRAMADDR = 0x40 70 | LCD_SETDDRAMADDR = 0x80 71 | 72 | # flags for display entry mode 73 | LCD_ENTRYRIGHT = 0x00 74 | LCD_ENTRYLEFT = 0x02 75 | LCD_ENTRYSHIFTINCREMENT = 0x01 76 | LCD_ENTRYSHIFTDECREMENT = 0x00 77 | 78 | # flags for display on/off control 79 | LCD_DISPLAYON = 0x04 80 | LCD_DISPLAYOFF = 0x00 81 | LCD_CURSORON = 0x02 82 | LCD_CURSOROFF = 0x00 83 | LCD_BLINKON = 0x01 84 | LCD_BLINKOFF = 0x00 85 | 86 | # flags for display/cursor shift 87 | LCD_DISPLAYMOVE = 0x08 88 | LCD_CURSORMOVE = 0x00 89 | LCD_MOVERIGHT = 0x04 90 | LCD_MOVELEFT = 0x00 91 | 92 | # flags for function set 93 | LCD_8BITMODE = 0x10 94 | LCD_4BITMODE = 0x00 95 | LCD_2LINE = 0x08 96 | LCD_1LINE = 0x00 97 | LCD_5x10DOTS = 0x04 98 | LCD_5x8DOTS = 0x00 99 | 100 | # flags for backlight control 101 | LCD_BACKLIGHT = 0x08 102 | LCD_NOBACKLIGHT = 0x00 103 | 104 | En = 0b00000100 # Enable bit 105 | Rw = 0b00000010 # Read/Write bit 106 | Rs = 0b00000001 # Register select bit 107 | 108 | class lcd: 109 | #initializes objects and lcd 110 | def __init__(self): 111 | self.lcd_device = i2c_device(ADDRESS) 112 | 113 | self.lcd_write(0x03) 114 | self.lcd_write(0x03) 115 | self.lcd_write(0x03) 116 | self.lcd_write(0x02) 117 | 118 | self.lcd_write(LCD_FUNCTIONSET | LCD_2LINE | LCD_5x8DOTS | LCD_4BITMODE) 119 | self.lcd_write(LCD_DISPLAYCONTROL | LCD_DISPLAYON) 120 | self.lcd_write(LCD_CLEARDISPLAY) 121 | self.lcd_write(LCD_ENTRYMODESET | LCD_ENTRYLEFT) 122 | sleep(0.2) 123 | 124 | 125 | # clocks EN to latch command 126 | def lcd_strobe(self, data): 127 | self.lcd_device.write_cmd(data | En | LCD_BACKLIGHT) 128 | sleep(.0005) 129 | self.lcd_device.write_cmd(((data & ~En) | LCD_BACKLIGHT)) 130 | sleep(.0001) 131 | 132 | def lcd_write_four_bits(self, data): 133 | self.lcd_device.write_cmd(data | LCD_BACKLIGHT) 134 | self.lcd_strobe(data) 135 | 136 | # write a command to lcd 137 | def lcd_write(self, cmd, mode=0): 138 | self.lcd_write_four_bits(mode | (cmd & 0xF0)) 139 | self.lcd_write_four_bits(mode | ((cmd << 4) & 0xF0)) 140 | 141 | # write a character to lcd (or character rom) 0x09: backlight | RS=DR< 142 | # works! 143 | def lcd_write_char(self, charvalue, mode=1): 144 | self.lcd_write_four_bits(mode | (charvalue & 0xF0)) 145 | self.lcd_write_four_bits(mode | ((charvalue << 4) & 0xF0)) 146 | 147 | # put string function with optional char positioning 148 | def lcd_display_string(self, string, line=1, pos=0): 149 | if line == 1: 150 | pos_new = pos 151 | elif line == 2: 152 | pos_new = 0x40 + pos 153 | elif line == 3: 154 | pos_new = 0x14 + pos 155 | elif line == 4: 156 | pos_new = 0x54 + pos 157 | 158 | self.lcd_write(0x80 + pos_new) 159 | 160 | for char in string: 161 | self.lcd_write(ord(char), Rs) 162 | 163 | # clear lcd and set to home 164 | def lcd_clear(self): 165 | self.lcd_write(LCD_CLEARDISPLAY) 166 | self.lcd_write(LCD_RETURNHOME) 167 | 168 | # define backlight on/off (lcd.backlight(1); off= lcd.backlight(0) 169 | def backlight(self, state): # for state, 1 = on, 0 = off 170 | if state == 1: 171 | self.lcd_device.write_cmd(LCD_BACKLIGHT) 172 | elif state == 0: 173 | self.lcd_device.write_cmd(LCD_NOBACKLIGHT) 174 | 175 | # add custom characters (0 - 7) 176 | def lcd_load_custom_chars(self, fontdata): 177 | self.lcd_write(0x40) 178 | for char in fontdata: 179 | for line in char: 180 | self.lcd_write_char(line) 181 | 182 | -------------------------------------------------------------------------------- /lcd_monitor/src/lcd_monitor/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/lcd_monitor/src/lcd_monitor/__init__.py -------------------------------------------------------------------------------- /lcd_monitor/src/lcd_monitor/sm_lcd_driver.py: -------------------------------------------------------------------------------- 1 | 2 | from lcd_monitor import I2C_LCD_driver 3 | import datetime 4 | import time 5 | import rospy 6 | from math import pi 7 | from geometry_msgs.msg import Vector3 8 | from geometry_msgs.msg import Twist 9 | from std_msgs.msg import String 10 | 11 | class SpotMicroLcd(): 12 | ''' Class to encapsulate lcd driver for spot micro robot ''' 13 | 14 | def __init__(self): 15 | '''Constructor''' 16 | self._mylcd = I2C_LCD_driver.lcd() 17 | 18 | self._state_str = 'None' 19 | self._padded_state_str = 'None' 20 | 21 | self._fwd_speed_cmd = 0.0 22 | self._side_speed_cmd = 0.0 23 | self._yaw_rate_cmd = 0.0 24 | 25 | self._phi_cmd = 0.0 26 | self._theta_cmd = 0.0 27 | self._psi_cmd = 0.0 28 | 29 | rospy.init_node('lcd_monitor_node') 30 | 31 | rospy.Subscriber('lcd_vel_cmd',Twist,self.update_speed_cmd) 32 | rospy.Subscriber('lcd_angle_cmd',Vector3,self.update_angle_cmd) 33 | rospy.Subscriber('lcd_state',String,self.update_state_string) 34 | 35 | def update_speed_cmd(self, msg): 36 | ''' Updates speed command attributes''' 37 | self._fwd_speed_cmd = msg.linear.x*100.0 38 | self._side_speed_cmd = msg.linear.y*100.0 39 | self._yaw_rate_cmd = msg.angular.z*180.0/pi 40 | 41 | def update_angle_cmd(self, msg): 42 | ''' Updates angle command attributes''' 43 | self._phi_cmd = msg.x * 180.0/pi 44 | self._theta_cmd = msg.y * 180.0/pi 45 | self._psi_cmd = msg.z * 180.0/pi 46 | 47 | def update_state_string(self, msg): 48 | ''' Updates angle command attributes''' 49 | self._state_str = msg.data 50 | 51 | if self._state_str == "Transit Stand": 52 | self._padded_state_str = "To Stand" 53 | elif self._state_str == "Transit Idle": 54 | self._padded_state_str = "To Idle" 55 | else: 56 | self._padded_state_str = self._state_str 57 | 58 | self._padded_state_str = self._padded_state_str.ljust(16,' ') 59 | 60 | def run(self): 61 | ''' Runs the lcd driver and prints data''' 62 | 63 | # Define the loop rate in Hz 64 | rate = rospy.Rate(3) 65 | 66 | while not rospy.is_shutdown(): 67 | 68 | self._mylcd.lcd_display_string('State: %s'%(self._padded_state_str),1) 69 | 70 | 71 | if self._state_str == "Stand": 72 | self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._phi_cmd, self._theta_cmd, self._psi_cmd),2) 73 | elif self._state_str == "Walk": 74 | self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._fwd_speed_cmd, self._side_speed_cmd, self._yaw_rate_cmd),2) 75 | else: 76 | self._mylcd.lcd_display_string(' ',2) 77 | # Sleep till next loop 78 | rate.sleep() 79 | 80 | 81 | def main(): 82 | sm_lcd_obj = SpotMicroLcd() 83 | sm_lcd_obj.run() 84 | -------------------------------------------------------------------------------- /servo_move_keyboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(servo_move_keyboard) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | i2cpwm_board 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES servo_move_keyboard 107 | # CATKIN_DEPENDS rospy 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/servo_move_keyboard.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/servo_move_keyboard_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_servo_move_keyboard.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /servo_move_keyboard/launch/keyboard_move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | │walk: Start walk mode and keyboard motion control 14 | 15 | 16 | -------------------------------------------------------------------------------- /servo_move_keyboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | servo_move_keyboard 4 | 0.0.0 5 | Package to move servos via keyboard inputs 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | i2cpwm_board 57 | i2cpwm_board 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /servo_move_keyboard/scripts/servoMoveKeyboard.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | """ 4 | Class for testing control of 12 servos. It assumes ros-12cpwmboard has been 5 | installed 6 | """ 7 | import rospy 8 | import sys, select, termios, tty # For terminal keyboard key press reading 9 | from i2cpwm_board.msg import Servo, ServoArray 10 | 11 | # Global variable for number of servos 12 | numServos = 12 13 | 14 | msg = """ 15 | Servo Control Module for 12 Servos. 16 | 17 | Enter one of the following options: 18 | ----------------------------- 19 | quit: stop and quit the program 20 | oneServo: Move one servo manually, all others will be commanded to their center position 21 | allServos: Move all servo's manually together 22 | 23 | Keyboard commands for One Servo Control 24 | --------------------------- 25 | q y 26 | f g j k 27 | z x b n m 28 | 29 | q: Quit current command mode and go back to Option Select 30 | z: Command servo min value 31 | y: Command servo center value 32 | x: Command servo max value 33 | f: Manually decrease servo command value by 10 34 | g: Manually decrease servo command value by 1 35 | j: Manually increase servo command value by 1 36 | k: Manually increase servo command value by 10 37 | b: Save new min command value 38 | n: Save new center command value 39 | m: Save new max command value 40 | 41 | 42 | anything else : Prompt again for command 43 | 44 | 45 | CTRL-C to quit 46 | """ 47 | # Dictionary with anonomous helper functions to execute key commands 48 | keyDict = { 49 | 'q': None, 50 | 'z': lambda x: x.set_value(x._min), 51 | 'y': lambda x: x.set_value(x._center), 52 | 'x': lambda x: x.set_value(x._max), 53 | 'f': lambda x: x.set_value(x.value-10), 54 | 'g': lambda x: x.set_value(x.value-1), 55 | 'j': lambda x: x.set_value(x.value+1), 56 | 'k': lambda x: x.set_value(x.value+10), 57 | 'b': lambda x: x.set_min(x.value), 58 | 'n': lambda x: x.set_center(x.value), 59 | 'm': lambda x: x.set_max(x.value), 60 | } 61 | 62 | validCmds = ['quit','oneServo','allServos'] 63 | 64 | class ServoConvert(): 65 | ''' 66 | ServoConvert Class encapsulates a servo 67 | Servo has a center value, and range, and is commanded by a value between 0 and 4095. 68 | This coorsponds to the duty cycle in a 12 bit pwm cycle. Nominally, a servo is commanded with pulses of 69 | 1 to 2 ms in a 20 ms cycle, with 1.5 ms being the value for center position. 70 | These nominal values would coorespond to integer values of approximately 204, 306, and 409 71 | for 1 ms, 1.5 ms, and 2 ms, respectively 72 | ''' 73 | def __init__(self, id=1, center_value=306, direction=1): 74 | self.value = center_value 75 | self._center = center_value 76 | self._min = 83 77 | self._max = 520 78 | self._dir = direction 79 | self.id = id 80 | 81 | def set_value(self, value_in): 82 | ''' 83 | Set Servo value 84 | Input: Value between 0 and 4095 85 | ''' 86 | if value_in not in range(4096): 87 | print('Servo value not in range [0,4095]') 88 | else: 89 | self.value = value_in 90 | 91 | 92 | def set_center(self,center_val): 93 | ''' 94 | Set Servo center value 95 | Input: Value between 0 and 4095 96 | ''' 97 | if center_val not in range(4096): 98 | print('Servo value not in range [0,4095]') 99 | else: 100 | self._center = center_val 101 | print('Servo %2i center set to %4i'%(self.id+1,center_val)) 102 | 103 | def set_max(self,max_val): 104 | ''' 105 | Set Servo max value 106 | Input: Value between 0 and 4095 107 | ''' 108 | if max_val not in range(4096): 109 | print('Servo value not in range [0,4095]') 110 | else: 111 | self._max = max_val 112 | print('Servo %2i max set to %4i'%(self.id+1,max_val)) 113 | 114 | def set_min(self,min_val): 115 | ''' 116 | Set Servo min value 117 | Input: Value between 0 and 4095 118 | ''' 119 | if min_val not in range(4096): 120 | print('Servo value not in range [0,4095]') 121 | else: 122 | self._min = min_val 123 | print('Servo %2i min set to %4i'%(self.id+1,min_val)) 124 | 125 | class SpotMicroServoControl(): 126 | def __init__(self): 127 | rospy.loginfo("Setting Up the Spot Micro Servo Control Node...") 128 | 129 | # Set up and title the ros node for this code 130 | rospy.init_node('spot_micro_servo_control') 131 | 132 | # Intialize empty servo dictionary 133 | self.servos = {} 134 | 135 | # Create a servo dictionary with 12 ServoConvert objects 136 | # keys: integers 0 through 12 137 | # values: ServoConvert objects 138 | for i in range(numServos): 139 | self.servos[i] = ServoConvert(id=i) 140 | rospy.loginfo("> Servos corrrectly initialized") 141 | 142 | # Create empty ServoArray message with n number of Servos in its array 143 | self._servo_msg = ServoArray() 144 | for i in range(numServos): 145 | self._servo_msg.servos.append(Servo()) 146 | 147 | # Create the servo array publisher 148 | self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) 149 | rospy.loginfo("> Publisher corrrectly initialized") 150 | 151 | rospy.loginfo("Initialization complete") 152 | 153 | # Setup terminal input reading, taken from teleop_twist_keyboard 154 | self.settings = termios.tcgetattr(sys.stdin) 155 | 156 | def send_servo_msg(self): 157 | for servo_key, servo_obj in self.servos.iteritems(): 158 | self._servo_msg.servos[servo_obj.id].servo = servo_obj.id+1 159 | self._servo_msg.servos[servo_obj.id].value = servo_obj.value 160 | #rospy.loginfo("Sending to %s command %d"%(servo_key, servo_obj.value)) 161 | 162 | self.ros_pub_servo_array.publish(self._servo_msg) 163 | 164 | def reset_all_servos_center(self): 165 | ''' 166 | Reset all servos to their center value 167 | ''' 168 | for s in self.servos: 169 | self.servos[s].value = self.servos[s]._center 170 | 171 | def reset_all_servos_off(self): 172 | '''Set all servos to off/freewheel value (pwm of 0)''' 173 | for s in self.servos: 174 | self.servos[s].value = 0 175 | 176 | def getKey(self): 177 | tty.setraw(sys.stdin.fileno()) 178 | select.select([sys.stdin], [], [], 0) 179 | key = sys.stdin.read(1) 180 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) 181 | return key 182 | 183 | def run(self): 184 | 185 | # Set all servos to their center values 186 | self.reset_all_servos_center() 187 | self.send_servo_msg() 188 | 189 | # Prompt user with keyboard command information 190 | # Ability to control individual servo to find limits and center values 191 | # and ability to control all servos together 192 | 193 | while not rospy.is_shutdown(): 194 | print(msg) 195 | userInput = raw_input("Command?: ") 196 | 197 | if userInput not in validCmds: 198 | print('Valid command not entered, try again...') 199 | else: 200 | if userInput == 'quit': 201 | print("Ending program...") 202 | print('Final Servo Values') 203 | print('--------------------') 204 | for i in range(numServos): 205 | print('Servo %2i: Min: %4i, Center: %4i, Max: %4i'%(i,self.servos[i]._min,self.servos[i]._center,self.servos[i]._max)) 206 | break 207 | 208 | elif userInput == 'oneServo': 209 | # Reset all servos to center value, and send command 210 | self.reset_all_servos_off() 211 | self.send_servo_msg() 212 | 213 | # First get servo number to command 214 | nSrv = -1 215 | while (1): 216 | userInput = input('Which servo to control? Enter a number 1 through 12: ') 217 | 218 | if userInput not in range(1,numServos+1): 219 | print("Invalid servo number entered, try again") 220 | else: 221 | nSrv = userInput - 1 222 | break 223 | 224 | # Loop and act on user command 225 | print('Enter command, q to go back to option select: ') 226 | while (1): 227 | 228 | userInput = self.getKey() 229 | 230 | if userInput == 'q': 231 | break 232 | elif userInput not in keyDict: 233 | print('Key not in valid key commands, try again') 234 | else: 235 | keyDict[userInput](self.servos[nSrv]) 236 | print('Servo %2i cmd: %4i'%(nSrv,self.servos[nSrv].value)) 237 | self.send_servo_msg() 238 | 239 | elif userInput == 'allServos': 240 | # Reset all servos to center value, and send command 241 | self.reset_all_servos_center() 242 | self.send_servo_msg() 243 | 244 | print('Enter command, q to go back to option select: ') 245 | while (1): 246 | 247 | userInput = self.getKey() 248 | 249 | if userInput == 'q': 250 | break 251 | elif userInput not in keyDict: 252 | print('Key not in valid key commands, try again') 253 | elif userInput in ('b','n','m'): 254 | print('Saving values not supported in all servo control mode') 255 | else: 256 | for s in self.servos.values(): 257 | keyDict[userInput](s) 258 | print('All Servos Commanded') 259 | self.send_servo_msg() 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | # print self._last_time_cmd_rcv, self.is_controller_connected 268 | # if not self.is_controller_connected: 269 | # self.set_actuators_idle() 270 | 271 | # Set the control rate in Hz 272 | rate = rospy.Rate(10) 273 | rate.sleep() 274 | 275 | if __name__ == "__main__": 276 | smsc = SpotMicroServoControl() 277 | smsc.run() 278 | -------------------------------------------------------------------------------- /spot_micro_joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(spot_micro_joy) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | joy 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES spot_micro_joy 108 | # CATKIN_DEPENDS joy rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/spot_micro_joy.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/spot_micro_joy_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_joy.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /spot_micro_joy/launch/everything.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /spot_micro_joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_joy 4 | 0.0.0 5 | The spot_micro_joy package 6 | 7 | 8 | 9 | 10 | ubuntu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | joy 53 | rospy 54 | std_msgs 55 | joy 56 | rospy 57 | std_msgs 58 | joy 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /spot_micro_joy/scripts/spotMicroJoystickMove.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy 4 | from std_msgs.msg import Float32, Bool 5 | from sensor_msgs.msg import Joy 6 | from geometry_msgs.msg import Vector3 7 | from geometry_msgs.msg import Twist 8 | from math import pi 9 | 10 | 11 | class SpotMicroJoystickControl(): 12 | BUTTON_IDLE = 0 13 | BUTTON_WALK = 1 14 | BUTTON_STAND = 2 15 | BUTTON_ANGLE = 3 16 | 17 | ANGLE_AXES_ROLL = 0 18 | ANGLE_AXES_HEIGHT = 1 19 | ANGLE_AXES_YAW = 2 20 | ANGLE_AXES_PITCH = 3 21 | 22 | WALK_AXES_FORWARD = 1 23 | WALK_AXES_STRAFE = 0 24 | WALK_AXES_YAW = 2 25 | 26 | MODE_IDLE = 0 27 | MODE_STAND = 1 28 | MODE_ANGLE = 2 29 | MODE_WALK = 3 30 | 31 | MAX_ROLL_DEG = 45 32 | MAX_YAW_DEG = 45 33 | MAX_PATCH_DEG = 45 34 | 35 | MAX_FORWARD_SPEED = 0.05 36 | MAX_STRAFE_SPEED = 0.05 37 | MAX_YAW_SPEED_DEG = 15 38 | 39 | def __init__(self): 40 | 41 | self._angle_cmd_msg = Vector3() 42 | self._angle_cmd_msg.x = 0 43 | self._angle_cmd_msg.y = 0 44 | self._angle_cmd_msg.z = 0 45 | 46 | self._vel_cmd_msg = Twist() 47 | self._vel_cmd_msg.linear.x = 0 48 | self._vel_cmd_msg.linear.y = 0 49 | self._vel_cmd_msg.linear.z = 0 50 | self._vel_cmd_msg.angular.x = 0 51 | self._vel_cmd_msg.angular.y = 0 52 | self._vel_cmd_msg.angular.z = 0 53 | 54 | self._walk_event_cmd_msg = Bool() 55 | self._walk_event_cmd_msg.data = True # Mostly acts as an event driven action on receipt of a true message 56 | 57 | self._stand_event_cmd_msg = Bool() 58 | self._stand_event_cmd_msg.data = True 59 | 60 | self._idle_event_cmd_msg = Bool() 61 | self._idle_event_cmd_msg.data = True 62 | 63 | rospy.loginfo("Setting Up the Spot Micro Joystick Control Node...") 64 | 65 | # Set up and title the ros node for this code 66 | rospy.init_node('spot_micro_joystick_control') 67 | 68 | # Create publishers for commanding velocity, angle, and robot states 69 | self._ros_pub_angle_cmd = rospy.Publisher('/angle_cmd', Vector3, queue_size=1) 70 | self._ros_pub_vel_cmd = rospy.Publisher('/cmd_vel', Twist, queue_size=1) 71 | self._ros_pub_walk_cmd = rospy.Publisher('/walk_cmd', Bool, queue_size=1) 72 | self._ros_pub_stand_cmd = rospy.Publisher('/stand_cmd', Bool, queue_size=1) 73 | self._ros_pub_idle_cmd = rospy.Publisher('/idle_cmd', Bool, queue_size=1) 74 | 75 | rospy.loginfo("Joystick control node publishers corrrectly initialized") 76 | 77 | def reset_all_motion_commands_to_zero(self): 78 | '''Reset body motion cmd states to zero and publish zero value body motion commands''' 79 | 80 | self._vel_cmd_msg.linear.x = 0 81 | self._vel_cmd_msg.linear.y = 0 82 | self._vel_cmd_msg.linear.z = 0 83 | self._vel_cmd_msg.angular.x = 0 84 | self._vel_cmd_msg.angular.y = 0 85 | self._vel_cmd_msg.angular.z = 0 86 | 87 | self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) 88 | 89 | def reset_all_angle_commands_to_zero(self): 90 | '''Reset angle cmd states to zero and publish them''' 91 | 92 | self._angle_cmd_msg.x = 0 93 | self._angle_cmd_msg.y = 0 94 | self._angle_cmd_msg.z = 0 95 | 96 | self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) 97 | 98 | def on_joy(self, msg): 99 | self.on_joy_buttons(msg.buttons) 100 | self.on_joy_axes(msg.axes) 101 | 102 | def on_joy_buttons(self, buttons): 103 | if buttons[self.BUTTON_IDLE] == 1: 104 | self._ros_pub_idle_cmd.publish(self._idle_event_cmd_msg) 105 | rospy.loginfo('Idle command issued from joystick.') 106 | self.mode = self.MODE_IDLE 107 | elif buttons[self.BUTTON_STAND] == 1: 108 | self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg) 109 | rospy.loginfo('Stand command issued from joystick.') 110 | self.mode = self.MODE_STAND 111 | elif buttons[self.BUTTON_ANGLE] == 1: 112 | self.reset_all_angle_commands_to_zero() 113 | rospy.loginfo('Entering joystick angle command mode.') 114 | self.mode = self.MODE_ANGLE 115 | elif buttons[self.BUTTON_WALK] == 1: 116 | self.reset_all_angle_commands_to_zero() 117 | self._ros_pub_walk_cmd.publish(self._walk_event_cmd_msg) 118 | rospy.loginfo('Entering joystick walk command mode.') 119 | self.mode = self.MODE_WALK 120 | 121 | def on_joy_axes(self, axes): 122 | if self.mode == self.MODE_ANGLE: 123 | self.on_joy_angle_mode(axes) 124 | elif self.mode == self.MODE_WALK: 125 | self.on_joy_walk_mode(axes) 126 | 127 | def on_joy_walk_mode(self, axes): 128 | self._vel_cmd_msg.linear.x = axes[self.WALK_AXES_FORWARD] * self.MAX_FORWARD_SPEED 129 | self._vel_cmd_msg.linear.y = axes[self.WALK_AXES_STRAFE] * self.MAX_STRAFE_SPEED 130 | self._vel_cmd_msg.angular.z = pi / 180 * axes[self.WALK_AXES_YAW] * self.MAX_YAW_SPEED_DEG 131 | print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s ' \ 132 | % (self._vel_cmd_msg.linear.x, self._vel_cmd_msg.linear.y, self._vel_cmd_msg.angular.z * 180 / pi)) 133 | self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) 134 | 135 | def on_joy_angle_mode(self, axes): 136 | self._angle_cmd_msg.x = pi / 180 * axes[self.ANGLE_AXES_ROLL] * self.MAX_ROLL_DEG * -1 137 | self._angle_cmd_msg.y = pi / 180 * axes[self.ANGLE_AXES_PITCH] * self.MAX_PATCH_DEG * -1 138 | self._angle_cmd_msg.z = pi / 180 * axes[self.ANGLE_AXES_YAW] * self.MAX_YAW_DEG 139 | print('Cmd Values: phi: %1.3f deg, theta: %1.3f deg, psi: %1.3f deg ' \ 140 | % ( 141 | self._angle_cmd_msg.x * 180 / pi, self._angle_cmd_msg.y * 180 / pi, 142 | self._angle_cmd_msg.z * 180 / pi)) 143 | self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) 144 | 145 | def run(self): 146 | print("green = idle") 147 | print("yellow = stand") 148 | print("blue = angle") 149 | print("red = walk") 150 | 151 | # Publish all body motion commands to 0 152 | self.reset_all_motion_commands_to_zero() 153 | rospy.Subscriber("/joy", Joy, self.on_joy) 154 | rospy.spin() 155 | 156 | 157 | if __name__ == "__main__": 158 | smjc = SpotMicroJoystickControl() 159 | smjc.run() 160 | -------------------------------------------------------------------------------- /spot_micro_keyboard_command/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_micro_keyboard_command) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | geometry_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES spot_micro_keyboard_command 108 | # CATKIN_DEPENDS rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/spot_micro_keyboard_command.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/spot_micro_keyboard_command_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_keyboard_command.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /spot_micro_keyboard_command/launch/keyboard_command.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | --> 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /spot_micro_keyboard_command/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_keyboard_command 4 | 0.0.0 5 | The spot_micro_keyboard_command package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | geometry_msgs 55 | rospy 56 | std_msgs 57 | geometry_msgs 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /spot_micro_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(spot_micro_launch) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | hector_geotiff 12 | hector_geotiff_plugins 13 | hector_map_server 14 | hector_mapping 15 | hector_trajectory_server 16 | rviz 17 | tf 18 | tf2 19 | topic_tools 20 | ) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | 26 | ## Uncomment this if the package has a setup.py. This macro ensures 27 | ## modules and global scripts declared therein get installed 28 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 29 | # catkin_python_setup() 30 | 31 | ################################################ 32 | ## Declare ROS messages, services and actions ## 33 | ################################################ 34 | 35 | ## To declare and build messages, services or actions from within this 36 | ## package, follow these steps: 37 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 38 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 39 | ## * In the file package.xml: 40 | ## * add a build_depend tag for "message_generation" 41 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 42 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 43 | ## but can be declared for certainty nonetheless: 44 | ## * add a exec_depend tag for "message_runtime" 45 | ## * In this file (CMakeLists.txt): 46 | ## * add "message_generation" and every package in MSG_DEP_SET to 47 | ## find_package(catkin REQUIRED COMPONENTS ...) 48 | ## * add "message_runtime" and every package in MSG_DEP_SET to 49 | ## catkin_package(CATKIN_DEPENDS ...) 50 | ## * uncomment the add_*_files sections below as needed 51 | ## and list every .msg/.srv/.action file to be processed 52 | ## * uncomment the generate_messages entry below 53 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 54 | 55 | ## Generate messages in the 'msg' folder 56 | # add_message_files( 57 | # FILES 58 | # Message1.msg 59 | # Message2.msg 60 | # ) 61 | 62 | ## Generate services in the 'srv' folder 63 | # add_service_files( 64 | # FILES 65 | # Service1.srv 66 | # Service2.srv 67 | # ) 68 | 69 | ## Generate actions in the 'action' folder 70 | # add_action_files( 71 | # FILES 72 | # Action1.action 73 | # Action2.action 74 | # ) 75 | 76 | ## Generate added messages and services with any dependencies listed here 77 | # generate_messages( 78 | # DEPENDENCIES 79 | # std_msgs # Or other packages containing msgs 80 | # ) 81 | 82 | ################################################ 83 | ## Declare ROS dynamic reconfigure parameters ## 84 | ################################################ 85 | 86 | ## To declare and build dynamic reconfigure parameters within this 87 | ## package, follow these steps: 88 | ## * In the file package.xml: 89 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "dynamic_reconfigure" to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 94 | ## and list every .cfg file to be processed 95 | 96 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 97 | # generate_dynamic_reconfigure_options( 98 | # cfg/DynReconf1.cfg 99 | # cfg/DynReconf2.cfg 100 | # ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if your package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | # INCLUDE_DIRS include 113 | # LIBRARIES spot_micro_launch 114 | # CATKIN_DEPENDS hector_geotiff hector_geotiff_plugins hector_map_server hector_mapping hector_trajectory_server rviz tf tf2 topic_tools 115 | # DEPENDS system_lib 116 | ) 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | ## Specify additional locations of header files 123 | ## Your package locations should be listed before other locations 124 | include_directories( 125 | # include 126 | ${catkin_INCLUDE_DIRS} 127 | ) 128 | 129 | ## Declare a C++ library 130 | # add_library(${PROJECT_NAME} 131 | # src/${PROJECT_NAME}/spot_micro_launch.cpp 132 | # ) 133 | 134 | ## Add cmake target dependencies of the library 135 | ## as an example, code may need to be generated before libraries 136 | ## either from message generation or dynamic reconfigure 137 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Declare a C++ executable 140 | ## With catkin_make all packages are built within a single CMake context 141 | ## The recommended prefix ensures that target names across packages don't collide 142 | # add_executable(${PROJECT_NAME}_node src/spot_micro_launch_node.cpp) 143 | 144 | ## Rename C++ executable without prefix 145 | ## The above recommended prefix causes long target names, the following renames the 146 | ## target back to the shorter version for ease of user use 147 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 148 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 149 | 150 | ## Add cmake target dependencies of the executable 151 | ## same as for the library above 152 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 153 | 154 | ## Specify libraries to link a library or executable target against 155 | # target_link_libraries(${PROJECT_NAME}_node 156 | # ${catkin_LIBRARIES} 157 | # ) 158 | 159 | ############# 160 | ## Install ## 161 | ############# 162 | 163 | # all install targets should use catkin DESTINATION variables 164 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 165 | 166 | ## Mark executable scripts (Python etc.) for installation 167 | ## in contrast to setup.py, you can choose the destination 168 | # catkin_install_python(PROGRAMS 169 | # scripts/my_python_script 170 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark executables for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 175 | # install(TARGETS ${PROJECT_NAME}_node 176 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark libraries for installation 180 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 181 | # install(TARGETS ${PROJECT_NAME} 182 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 185 | # ) 186 | 187 | ## Mark cpp header files for installation 188 | # install(DIRECTORY include/${PROJECT_NAME}/ 189 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 190 | # FILES_MATCHING PATTERN "*.h" 191 | # PATTERN ".svn" EXCLUDE 192 | # ) 193 | 194 | ## Mark other files for installation (e.g. launch and bag files, etc.) 195 | # install(FILES 196 | # # myfile1 197 | # # myfile2 198 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 199 | # ) 200 | 201 | ############# 202 | ## Testing ## 203 | ############# 204 | 205 | ## Add gtest based cpp test target and link libraries 206 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_launch.cpp) 207 | # if(TARGET ${PROJECT_NAME}-test) 208 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 209 | # endif() 210 | 211 | ## Add folders to be run by python nosetests 212 | # catkin_add_nosetests(test) 213 | -------------------------------------------------------------------------------- /spot_micro_launch/launch/keyboard_control_and_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /spot_micro_launch/launch/motion_control_and_hector_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 68 | 69 | 70 | 71 | 72 | 73 | 77 | 78 | -------------------------------------------------------------------------------- /spot_micro_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_launch 4 | 0.0.0 5 | The spot_micro_launch package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | hector_geotiff 53 | hector_geotiff_plugins 54 | hector_map_server 55 | hector_mapping 56 | hector_trajectory_server 57 | rviz 58 | tf 59 | tf2 60 | topic_tools 61 | rplidar_ros 62 | hector_geotiff 63 | hector_geotiff_plugins 64 | hector_map_server 65 | hector_mapping 66 | hector_trajectory_server 67 | rviz 68 | tf 69 | tf2 70 | topic_tools 71 | rplidar_ros 72 | hector_geotiff 73 | hector_geotiff_plugins 74 | hector_map_server 75 | hector_mapping 76 | hector_trajectory_server 77 | rviz 78 | tf 79 | tf2 80 | topic_tools 81 | rplidar_ros 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_micro_motion_cmd) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | geometry_msgs 14 | i2cpwm_board 15 | tf2 16 | tf2_ros 17 | tf2_geometry_msgs 18 | ) 19 | 20 | add_subdirectory(libs/spot_micro_kinematics_cpp) 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 42 | ## but can be declared for certainty nonetheless: 43 | ## * add a exec_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | ## Generate messages in the 'msg' folder 55 | # add_message_files( 56 | # FILES 57 | # Message1.msg 58 | # Message2.msg 59 | # ) 60 | 61 | ## Generate services in the 'srv' folder 62 | # add_service_files( 63 | # FILES 64 | # Service1.srv 65 | # Service2.srv 66 | # ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | # generate_messages( 77 | # DEPENDENCIES 78 | # std_msgs 79 | # ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | # generate_dynamic_reconfigure_options( 97 | # cfg/DynReconf1.cfg 98 | # cfg/DynReconf2.cfg 99 | # ) 100 | 101 | ################################### 102 | ## catkin specific configuration ## 103 | ################################### 104 | ## The catkin_package macro generates cmake config files for your package 105 | ## Declare things to be passed to dependent projects 106 | ## INCLUDE_DIRS: uncomment this if your package contains header files 107 | ## LIBRARIES: libraries you create in this project that dependent projects also need 108 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 109 | ## DEPENDS: system dependencies of this project that dependent projects also need 110 | catkin_package( 111 | INCLUDE_DIRS include/spot_micro_motion_cmd 112 | # LIBRARIES spot_micro_motion_cmd 113 | CATKIN_DEPENDS roscpp std_msgs geometry_msgs tf2 tf2_ros 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | include/spot_micro_motion_cmd 125 | src/smfsm 126 | src/rate_limited_first_order_filter 127 | ${catkin_INCLUDE_DIRS} 128 | ) 129 | 130 | ## Declare a C++ library 131 | add_library(smfsm 132 | src/smfsm/spot_micro_state.cpp 133 | src/smfsm/spot_micro_idle.cpp 134 | src/smfsm/spot_micro_stand.cpp 135 | src/smfsm/spot_micro_transition_stand.cpp 136 | src/smfsm/spot_micro_transition_idle.cpp 137 | src/smfsm/spot_micro_walk.cpp 138 | ) 139 | 140 | target_link_libraries(smfsm 141 | spot_micro_kinematics 142 | ) 143 | 144 | ## Add cmake target dependencies of the library 145 | ## as an example, code may need to be generated before libraries 146 | ## either from message generation or dynamic reconfigure 147 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Declare a C++ executable 150 | ## With catkin_make all packages are built within a single CMake context 151 | ## The recommended prefix ensures that target names across packages don't collide 152 | add_executable(${PROJECT_NAME}_node src/spot_micro_motion_cmd.cpp src/spot_micro_motion_cmd_node.cpp src/utils.cpp) 153 | 154 | ## Rename C++ executable without prefix 155 | ## The above recommended prefix causes long target names, the following renames the 156 | ## target back to the shorter version for ease of user use 157 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 158 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 159 | 160 | ## Add cmake target dependencies of the executable 161 | ## same as for the library above 162 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 163 | 164 | ## Specify libraries to link a library or executable target against 165 | target_link_libraries(${PROJECT_NAME}_node 166 | ${catkin_LIBRARIES} 167 | smfsm 168 | spot_micro_kinematics 169 | ) 170 | 171 | ############# 172 | ## Install ## 173 | ############# 174 | 175 | # all install targets should use catkin DESTINATION variables 176 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 177 | 178 | ## Mark executable scripts (Python etc.) for installation 179 | ## in contrast to setup.py, you can choose the destination 180 | # catkin_install_python(PROGRAMS 181 | # scripts/my_python_script 182 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark executables for installation 186 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 187 | # install(TARGETS ${PROJECT_NAME}_node 188 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark libraries for installation 192 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 193 | # install(TARGETS ${PROJECT_NAME} 194 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 195 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 196 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 197 | # ) 198 | 199 | ## Mark cpp header files for installation 200 | # install(DIRECTORY include/${PROJECT_NAME}/ 201 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 202 | # FILES_MATCHING PATTERN "*.h" 203 | # PATTERN ".svn" EXCLUDE 204 | # ) 205 | 206 | ## Mark other files for installation (e.g. launch and bag files, etc.) 207 | # install(FILES 208 | # # myfile1 209 | # # myfile2 210 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 211 | # ) 212 | 213 | ############# 214 | ## Testing ## 215 | ############# 216 | 217 | ## Add gtest based cpp test target and link libraries 218 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_motion_cmd.cpp) 219 | # if(TARGET ${PROJECT_NAME}-test) 220 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 221 | # endif() 222 | 223 | ## Add folders to be run by python nosetests 224 | # catkin_add_nosetests(test) 225 | 226 | 227 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml: -------------------------------------------------------------------------------- 1 | # Configuration parameters 2 | 3 | # Robot wireframe size parameters 4 | # Lengths representening a wireframe model of the robot. All lengths joint to joint 5 | hip_link_length: 0.055 # Straight line distance of the hip link (horizontal leg link) 6 | upper_leg_link_length: 0.1075 # Straight line distance of the upper leg link, joint to joint 7 | lower_leg_link_length: 0.130 # Straight line distance of the lower leg link, joint to joint 8 | body_width: 0.078 # Horizontal width between hip joints 9 | body_length: 0.186 # Length between shoulder joints 10 | 11 | # Stance parameters 12 | default_stand_height: 0.155 # Height of robot body center when standing 13 | stand_front_x_offset: 0.015 # Fwb/back offset of front two legs from default stance 14 | stand_back_x_offset: -0.000 # Fwd/back offset of back two legs from default stance 15 | #stand_front_x_offset: -0.010 # Offset better tuned for trot gait 16 | #stand_back_x_offset: -0.010 # Offset better tuned for trot gait 17 | lie_down_height: 0.083 # Height of body center when sitting 18 | lie_down_foot_x_offset: 0.065 # Fwd/back offset of all(?) feet from default stance when sitting 19 | 20 | # Servo parameters 21 | num_servos: 12 22 | servo_max_angle_deg: 82.5 23 | RF_3: {num: 1, center: 306,range: 372,direction: 1, center_angle_deg: 88.2} 24 | RF_2: {num: 2, center: 306,range: 389,direction: 1, center_angle_deg: -27.6} 25 | RF_1: {num: 3, center: 306,range: 396,direction: -1, center_angle_deg: -5.4} 26 | RB_3: {num: 4, center: 306,range: 396,direction: 1, center_angle_deg: 85.8} 27 | RB_2: {num: 5, center: 306,range: 396,direction: 1, center_angle_deg: -35.4} 28 | RB_1: {num: 6, center: 306,range: 411,direction: 1, center_angle_deg: -4.4} 29 | LB_3: {num: 7, center: 306,range: 390,direction: 1, center_angle_deg: -73.9} 30 | LB_2: {num: 8, center: 306,range: 392,direction: 1, center_angle_deg: 38.7} 31 | LB_1: {num: 9, center: 306,range: 374,direction: -1, center_angle_deg: -0.4} 32 | LF_3: {num: 10, center: 306,range: 387,direction: 1, center_angle_deg: -82.8} 33 | LF_2: {num: 11, center: 306,range: 397,direction: 1, center_angle_deg: 38.6} 34 | LF_1: {num: 12, center: 306,range: 389,direction: 1, center_angle_deg: -7.6} 35 | 36 | # Control Parameters 37 | transit_tau: 0.3 # Time constant in seconds for first order filters during transition state 38 | transit_rl: 0.06 # Body motion rate limit in m/s for transition state 39 | transit_angle_rl: 0.35 # Body motion angular rate limit in rad/s for transition state 40 | max_fwd_velocity: 0.4 41 | max_side_velocity: 0.4 42 | max_yaw_rate: 0.35 43 | 44 | # Gait parameters 45 | z_clearance: 0.050 # Max height of foot during swing phase. Increase to have feet go 46 | # higher during swing. Feet travel in a triangular motion during swing 47 | alpha: 0.5 # Controls whether feet are centered within fwd/back motion 48 | beta: 0.5 # Controls whether feet are centered within side to side motion 49 | foot_height_time_constant: 0.02 50 | 51 | ################## 52 | # GAIT SELECTION # 53 | ################## 54 | # Uncomment one set of gait parameters for the desired 55 | # gait to use, and comment out the other set. 56 | 57 | ############################################ 58 | ############## 8 Phase Gait ################ 59 | ############################################ 60 | # Only one leg is ever in swing phase at one time 61 | # 4 swing phases + 4 body motion phases to balance body over 62 | # three legs that will remain in contact with ground 63 | num_phases: 8 64 | rb_contact_phases: [1, 0, 1, 1, 1, 1, 1, 1] 65 | rf_contact_phases: [1, 1, 1, 0, 1, 1, 1, 1] 66 | lf_contact_phases: [1, 1, 1, 1, 1, 1, 1, 0] 67 | lb_contact_phases: [1, 1, 1, 1, 1, 0, 1, 1] 68 | overlap_time: 0.0 69 | swing_time: 0.36 70 | body_shift_phases: [1, 2, 3, 4, 5, 6, 7, 8] 71 | fwd_body_balance_shift: 0.035 # Amount the body center will shift forward when walking 72 | back_body_balance_shift: 0.005 # Amount the body center will shift backward when walking 73 | side_body_balance_shift: 0.015 # Amount the body center will shift side to side when walking 74 | 75 | ################################################# 76 | ############## 4 Phase Trot Gait ################ 77 | ################################################# 78 | # 4 Phase gait where the two diagonally oposite legs move in 79 | # tandem together during swing phase 80 | # 2 swing phases + 2 phases where all four feet are on the ground 81 | # num_phases: 4 82 | # rb_contact_phases: [1, 0, 1, 1] 83 | # rf_contact_phases: [1, 1, 1, 0] 84 | # lf_contact_phases: [1, 0, 1, 1] 85 | # lb_contact_phases: [1, 1, 1, 0] 86 | # overlap_time: 0.18 87 | # swing_time: 0.18 88 | # body_shift_phases: [0,0,0,0] 89 | # fwd_body_balance_shift: 0 90 | # back_body_balance_shift: 0 91 | # side_body_balance_shift: 0 92 | 93 | # TF Parameter 94 | lidar_x_pos: 0.045 # X offset of lidar coordinate frame center from robot body center 95 | lidar_y_pos: 0.0 # Y offset of lidar coordinate frame center from robot body center 96 | lidar_z_pos: 0.085 # Z offset of lidar coordinate frame center from robot body center 97 | lidar_yaw_angle: 180 # Angle in degrees of yaw angle of the mounted lidar. Pitch and roll are assumed to be 0 98 | 99 | # Node parameters 100 | dt: 0.02 # Loop time of main loop, seconds (0.02 s is 50 hz) 101 | publish_odom: true # Determines whether odometry is published or not 102 | 103 | # Debug mode 104 | debug_mode: false # Prints out additional information 105 | plot_mode: false # Publishes data for the python plotting node to work 106 | 107 | # Standalone mode 108 | standalone_mode: false # Enables node to run without i2cpwnboard 109 | 110 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/data/usage.txt: -------------------------------------------------------------------------------- 1 | This folder is to store the data related to the program 2 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h: -------------------------------------------------------------------------------- 1 | #pragma once //designed to include the current source file only once in a single compilation. 2 | #ifndef SPOT_MICRO_MOTION_CMD //usd for conditional compiling. 3 | #define SPOT_MICRO_MOTION_CMD 4 | 5 | #include 6 | #include 7 | #include 8 | #include "std_msgs/Bool.h" 9 | #include "std_msgs/String.h" 10 | #include "geometry_msgs/Vector3.h" 11 | #include "geometry_msgs/Twist.h" 12 | #include "std_msgs/Float32MultiArray.h" 13 | #include "i2cpwm_board/Servo.h" 14 | #include "i2cpwm_board/ServoArray.h" 15 | 16 | #include "command.h" 17 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 18 | #include "spot_micro_state.h" 19 | 20 | // Define a configuration struct 21 | // To hold configuration parameters from parameter server/config file 22 | struct SpotMicroNodeConfig { 23 | smk::SpotMicroConfig smc; 24 | float default_stand_height; 25 | float stand_front_x_offset; 26 | float stand_back_x_offset; 27 | float lie_down_height; 28 | float lie_down_feet_x_offset; 29 | int num_servos; 30 | float servo_max_angle_deg; 31 | std::map> servo_config; 32 | float dt; 33 | float transit_tau; 34 | float transit_rl; 35 | float transit_angle_rl; 36 | bool debug_mode; 37 | bool run_standalone; 38 | bool plot_mode; 39 | float max_fwd_velocity; 40 | float max_side_velocity; 41 | float max_yaw_rate; 42 | float z_clearance; 43 | float alpha; 44 | float beta; 45 | int num_phases; 46 | std::vector rb_contact_phases; 47 | std::vector rf_contact_phases; 48 | std::vector lf_contact_phases; 49 | std::vector lb_contact_phases; 50 | float overlap_time; 51 | float swing_time; 52 | int overlap_ticks; 53 | int swing_ticks; 54 | int stance_ticks; 55 | std::vector phase_ticks; 56 | int phase_length; 57 | float foot_height_time_constant; 58 | std::vector body_shift_phases; 59 | float fwd_body_balance_shift; 60 | float side_body_balance_shift; 61 | float back_body_balance_shift; 62 | bool publish_odom; 63 | float lidar_x_pos; 64 | float lidar_y_pos; 65 | float lidar_z_pos; 66 | float lidar_yaw_angle; 67 | }; 68 | 69 | 70 | /* defining the class */ 71 | class SpotMicroMotionCmd 72 | { 73 | public: 74 | // Constructor 75 | SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh); 76 | 77 | // Destructor 78 | ~SpotMicroMotionCmd(); 79 | 80 | // Main loop runner, called periodically at the loop rate 81 | void runOnce(); 82 | 83 | // Publish a servo configuration message 84 | bool publishServoConfiguration(); 85 | 86 | // Set servo proprotional message data 87 | void setServoCommandMessageData(); 88 | 89 | // Publishes a servo proportional command message 90 | void publishServoProportionalCommand(); 91 | 92 | // Publishes a servo absolute command message with all servos set to a command 93 | // value of 0. This effectively disables the servos (stops them from holding 94 | // position, should just freewheel) 95 | void publishZeroServoAbsoluteCommand(); 96 | 97 | // Returns the loaded parameters 98 | SpotMicroNodeConfig getNodeConfig(); 99 | 100 | // Returns leg positions representing a neutral stance 101 | smk::LegsFootPos getNeutralStance(); 102 | 103 | // Returns leg positions representing a lieing down stance 104 | smk::LegsFootPos getLieDownStance(); 105 | 106 | // Manually override and command idle mode, used for shutdown 107 | void commandIdle(); 108 | 109 | // Returns current state name 110 | std::string getCurrentStateName(); 111 | 112 | private: 113 | // Declare SpotMicroState a friend so it can access and modify private 114 | // members of this class 115 | friend class SpotMicroState; 116 | 117 | // Pointer to state object 118 | std::unique_ptr state_; 119 | 120 | // Command object for encapsulating external commands 121 | Command cmd_; // Command object, encapsulate commands 122 | 123 | // Spot Micro Kinematics object. Holds kinematic state of robot, and holds 124 | // kinematics operations for setting position/orientation of the robot 125 | smk::SpotMicroKinematics sm_; 126 | 127 | // Spot Micro Node Config object 128 | SpotMicroNodeConfig smnc_; 129 | 130 | // Holds the body state to be commanded: feet position, body position and 131 | // angles 132 | smk::BodyState body_state_cmd_; 133 | 134 | // Odometry of the robot position and orientation based on integrated rate 135 | // commands. Only x and y position, and yaw angle, will be integrated from 136 | // rate commands 137 | smk::BodyState robot_odometry_; 138 | 139 | // Map to hold servo command values in radians 140 | std::map servo_cmds_rad_ = { {"RF_3", 0.0f}, {"RF_2", 0.0f}, {"RF_1", 0.0f}, 141 | {"RB_3", 0.0f}, {"RB_2", 0.0f}, {"RB_1", 0.0f}, 142 | {"LB_3", 0.0f}, {"LB_2", 0.0f}, {"LB_1", 0.0f}, 143 | {"LF_3", 0.0f}, {"LF_2", 0.0f}, {"LF_1", 0.0f} }; 144 | 145 | // Reads parameters from parameter server to initialize spot micro node config 146 | // struct 147 | void readInConfigParameters(); 148 | 149 | // Servo array message for servo proportional command 150 | i2cpwm_board::ServoArray servo_array_; 151 | 152 | // Servo array message for servo absolute command 153 | i2cpwm_board::ServoArray servo_array_absolute_; 154 | 155 | 156 | // ROS publisher and subscriber handles 157 | ros::NodeHandle nh_; // Defining the ros NodeHandle variable for registrating the same with the master 158 | ros::NodeHandle pnh_; // Private version of node handle 159 | ros::Subscriber stand_sub_; // ros subscriber handle for stand_cmd topic 160 | ros::Subscriber idle_sub_; // ros subscriber handle for idle_cmd topic 161 | ros::Subscriber walk_sub_; 162 | ros::Subscriber vel_cmd_sub_; 163 | ros::Subscriber body_angle_cmd_sub_; 164 | ros::Publisher servos_absolute_pub_; 165 | ros::Publisher servos_proportional_pub_; 166 | ros::Publisher body_state_pub_; 167 | ros::Publisher lcd_vel_cmd_pub_; 168 | ros::Publisher lcd_angle_cmd_pub_; 169 | ros::Publisher lcd_state_pub_; 170 | ros::ServiceClient servos_config_client_; 171 | tf2_ros::TransformBroadcaster transform_br_; 172 | tf2_ros::StaticTransformBroadcaster static_transform_br_; 173 | 174 | // Message for encapsulating robot body state 175 | std_msgs::Float32MultiArray body_state_msg_; 176 | 177 | // Messages to hold robot state information for displaying on LCD monitor 178 | // and for any other monitoring purposes. 179 | std_msgs::String lcd_state_string_msg_; 180 | geometry_msgs::Twist lcd_vel_cmd_msg_; 181 | geometry_msgs::Vector3 lcd_angle_cmd_msg_; 182 | 183 | // Callback method for stand command 184 | void standCommandCallback(const std_msgs::Bool::ConstPtr& msg); 185 | 186 | // Callback method for idle command 187 | void idleCommandCallback(const std_msgs::Bool::ConstPtr& msg); 188 | 189 | // Callback method for walk command 190 | void walkCommandCallback(const std_msgs::Bool::ConstPtr& msg); 191 | 192 | // Callback method for angle command 193 | void angleCommandCallback(const geometry_msgs::Vector3ConstPtr& msg); 194 | 195 | // Callback method for velocity command 196 | // Currently, the only supported commands from this message are 197 | // x and y axis linear velocity, and z axis angular rate 198 | void velCommandCallback(const geometry_msgs::TwistConstPtr& msg); 199 | 200 | // Resets all events if they were true 201 | void resetEventCommands(); 202 | 203 | // State Machine Related Methods 204 | // Handle input commands, delegate to state machine 205 | void handleInputCommands(); 206 | 207 | // Changes state of the state machine 208 | void changeState(std::unique_ptr sms); 209 | 210 | // Publishes body state as a float array for plotting by another node 211 | void publishBodyState(); 212 | 213 | // Publish LCD monitor messages 214 | void publishLcdMonitorData(); 215 | 216 | // Broadcast static tf2 coordinate frame transformation to /tf_static 217 | // Should only be called once at initalization, as it's only for static 218 | // transformations of the robot model that do not change over time 219 | void publishStaticTransforms(); 220 | 221 | // Broadcast dynamic tf2 coordinate frame transformations to /tf 222 | // Will broadcast dynamic robot and leg joint transformations 223 | void publishDynamicTransforms(); 224 | 225 | // Integrate robot odometry. The robot doesn't actually have any 226 | // sensed odometry, but an open loop estimate derived from velocity 227 | // commands should still be useful 228 | void integrateOdometry(); 229 | 230 | // Calculates the robot odometry coordinate frame 231 | Eigen::Affine3d getOdometryTransform(); 232 | 233 | }; 234 | #endif 235 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once //designed to include the current source file only once in a single compilation. 2 | 3 | #include 4 | #include "tf2/LinearMath/Quaternion.h" 5 | #include "tf2_eigen/tf2_eigen.h" 6 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 7 | 8 | // Utility functions for spot mico motion command node 9 | 10 | 11 | // Convert a Eigen Matrix4f to an Affine3d 12 | Eigen::Affine3d matrix4fToAffine3d(const Eigen::Matrix4f& in); 13 | 14 | 15 | // Create a ROS tf2 TransformStamped from a Eigen Affine3d, 16 | // parent frame id and child frame id. Stamped with current time, 17 | // so should be broadcast ASAP 18 | geometry_msgs::TransformStamped eigAndFramesToTrans( 19 | const Eigen::Affine3d& transform, 20 | std::string parent_frame_id, std::string child_frame_id); 21 | 22 | 23 | // Create a transform from a translation, rotation, and parent and 24 | // child frame IDs. Will stamp the transform with ros::Time::now(), 25 | // so the returned transform should be broadcast asap 26 | geometry_msgs::TransformStamped createTransform( 27 | std::string parent_frame_id, std::string child_frame_id, 28 | double x, double y, double z, 29 | double roll, double pitch, double yaw); -------------------------------------------------------------------------------- /spot_micro_motion_cmd/launch/motion_cmd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | --> 14 | 15 | 16 | 17 | 18 | 19 | 20 | │walk: Start walk mode and keyboard motion control 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_motion_cmd 4 | 0.0.0 5 | The spot_micro_motion_cmd package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | i2cpwm_board 53 | roscpp 54 | std_msgs 55 | geometry_msgs 56 | tf2 57 | tf2_ros 58 | tf2_geometry_msgs 59 | i2cpwm_board 60 | roscpp 61 | std_msgs 62 | geometry_msgs 63 | tf2 64 | tf2_ros 65 | tf2_geometry_msgs 66 | i2cpwm_board 67 | roscpp 68 | std_msgs 69 | geometry_msgs 70 | tf2 71 | tf2_ros 72 | tf2_geometry_msgs 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // Encapsulates a first order filter with rate limiting 6 | // Can be used to extract a first order time history response. 7 | // Major assumption of use is that the object is called at a fixed sample time 8 | // 9 | // Atributes: 10 | // dt_: sample time in seconds 11 | // tau_: time constant in seconds 12 | // state_: internal state of the filter 13 | // cmd_: Command value 14 | // alpha_: derived constant from sample time and time constant 15 | // 16 | // This filter assumes the following equations: 17 | // - y[i] is the current output, current state 18 | // - y[i-1] is the previous output, previous state 19 | // - u[i] is the current command 20 | // 21 | // y[0], u[0] = x0 22 | // 23 | // y[i] = (1 - alpha) * y[i-1] + alpha * u[i] 24 | // 25 | // alpha = dt / (tau + dt) 26 | // 27 | // If y[i] - y[0] exeeds the rate limit, then y[i] is limited to an 28 | // increment cooresponding to the rate limit 29 | class RateLmtdFirstOrderFilter { 30 | private: 31 | float dt_{0.001f}; 32 | float tau_; 33 | float state_; 34 | float cmd_; 35 | float alpha_; 36 | float rate_limit_; 37 | 38 | public: 39 | // Constructor 40 | RateLmtdFirstOrderFilter(float dt, float tau, float x0, float rate_limit) 41 | : dt_(dt), 42 | tau_(tau), 43 | state_(x0), 44 | cmd_(x0), 45 | alpha_(dt/(tau+dt)), 46 | rate_limit_(rate_limit) {} 47 | 48 | RateLmtdFirstOrderFilter() = default; 49 | 50 | void setCommand(float cmd) { 51 | cmd_ = cmd; 52 | } 53 | 54 | float runTimestepAndGetOutput() { 55 | runTimestep(); 56 | return state_; 57 | } 58 | 59 | void runTimestep() { 60 | // Convenience variables 61 | float a = alpha_; 62 | float y_prev = state_; 63 | float u = cmd_; 64 | 65 | // Update equation 66 | float y = (1-a)*y_prev + a*u; 67 | 68 | // Enforce rate limit 69 | float rate = (y-y_prev)/dt_; 70 | if (std::fabs(rate) > rate_limit_) { 71 | if (rate > 0.0f) { 72 | y = y_prev + rate_limit_*dt_; 73 | } else { 74 | y = y_prev - rate_limit_*dt_; 75 | } 76 | } 77 | state_ = y; 78 | } 79 | 80 | 81 | float getOutput() const { 82 | return state_; 83 | } 84 | 85 | 86 | void resetState(float x0) { 87 | state_ = x0; 88 | } 89 | 90 | }; 91 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/command.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "std_msgs/Float32.h" 4 | #include "std_msgs/Bool.h" 5 | 6 | 7 | class Command { 8 | public: 9 | 10 | float x_vel_cmd_mps_; 11 | float y_vel_cmd_mps_; 12 | float yaw_rate_cmd_rps_; 13 | float phi_cmd_; 14 | float theta_cmd_; 15 | float psi_cmd_; 16 | 17 | bool idle_cmd_; 18 | bool walk_cmd_; 19 | bool stand_cmd_; 20 | 21 | // Constructor 22 | Command() 23 | : x_vel_cmd_mps_(0.0) 24 | , y_vel_cmd_mps_(0.0) 25 | , yaw_rate_cmd_rps_(0.0) 26 | , phi_cmd_(0.0f) 27 | , theta_cmd_(0.0f) 28 | , psi_cmd_(0.0f) 29 | , idle_cmd_(false) 30 | , walk_cmd_(false) 31 | , stand_cmd_(false) 32 | { } 33 | 34 | bool getStandCmd() const { 35 | // stand cmd status getter. 36 | return stand_cmd_; 37 | } 38 | 39 | bool getIdleCmd() const { 40 | return idle_cmd_; 41 | } 42 | 43 | bool getWalkCmd() const { 44 | return walk_cmd_; 45 | } 46 | 47 | float getXSpeedCmd() const { 48 | return x_vel_cmd_mps_; 49 | } 50 | 51 | float getYSpeedCmd() const { 52 | return y_vel_cmd_mps_; 53 | } 54 | 55 | float getYawRateCmd() const { 56 | return yaw_rate_cmd_rps_; 57 | } 58 | 59 | float getPhiCmd() const { 60 | return phi_cmd_; 61 | } 62 | 63 | float getThetaCmd() const { 64 | return theta_cmd_; 65 | } 66 | 67 | float getPsiCmd() const { 68 | return psi_cmd_; 69 | } 70 | 71 | void resetAllCommands() { 72 | x_vel_cmd_mps_ = 0; 73 | y_vel_cmd_mps_ = 0; 74 | yaw_rate_cmd_rps_ = 0; 75 | phi_cmd_ = 0; 76 | theta_cmd_ = 0; 77 | psi_cmd_ = 0; 78 | } 79 | 80 | void resetEventCmds() { 81 | // Reset all event commands to false 82 | idle_cmd_ = false; 83 | walk_cmd_ = false; 84 | stand_cmd_ = false; 85 | } 86 | }; 87 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp: -------------------------------------------------------------------------------- 1 | #include "spot_micro_idle.h" 2 | #include "spot_micro_motion_cmd.h" 3 | #include "spot_micro_transition_stand.h" 4 | 5 | SpotMicroIdleState::SpotMicroIdleState() { 6 | // Construcotr, doesn't need to do anything, for now... 7 | //std::cout << "SpotMicroIdleState Ctor" << std::endl; 8 | } 9 | 10 | SpotMicroIdleState::~SpotMicroIdleState() { 11 | //std::cout << "SpotMicroIdleState Dtor" << std::endl; 12 | } 13 | 14 | void SpotMicroIdleState::handleInputCommands(const smk::BodyState& body_state, 15 | const SpotMicroNodeConfig& smnc, 16 | const Command& cmd, 17 | SpotMicroMotionCmd* smmc, 18 | smk::BodyState* body_state_cmd_) { 19 | if (smnc.debug_mode) { 20 | std::cout << "In Spot Micro Idle State" << std::endl; 21 | } 22 | 23 | // Check if stand command issued, if so, transition to stand state 24 | if (cmd.getStandCmd() == true) { 25 | changeState(smmc, std::make_unique()); 26 | 27 | } else { 28 | // Otherwise, just command idle servo commands 29 | smmc->publishZeroServoAbsoluteCommand(); 30 | } 31 | 32 | } 33 | 34 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "spot_micro_state.h" 6 | #include "command.h" 7 | 8 | 9 | class SpotMicroIdleState : public SpotMicroState { 10 | public: 11 | SpotMicroIdleState(); // Constructor 12 | ~SpotMicroIdleState(); // Destructor 13 | virtual void handleInputCommands(const smk::BodyState& body_state, 14 | const SpotMicroNodeConfig& smnc, 15 | const Command& cmd, 16 | SpotMicroMotionCmd* smmc, 17 | smk::BodyState* body_state_cmd); 18 | 19 | virtual std::string getCurrentStateName() { 20 | return "Idle"; 21 | } 22 | }; 23 | 24 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp: -------------------------------------------------------------------------------- 1 | #include "spot_micro_stand.h" 2 | 3 | #include "spot_micro_transition_idle.h" 4 | #include "spot_micro_walk.h" 5 | #include "spot_micro_motion_cmd.h" 6 | #include "rate_limited_first_order_filter.h" 7 | 8 | SpotMicroStandState::SpotMicroStandState() { 9 | // Construcotr, doesn't need to do anything, for now... 10 | //std::cout << "SpotMicroStandState Ctor" << std::endl; 11 | } 12 | 13 | SpotMicroStandState::~SpotMicroStandState() { 14 | //std::cout << "SpotMicroStandState Dtor" << std::endl; 15 | } 16 | 17 | void SpotMicroStandState::handleInputCommands(const smk::BodyState& body_state, 18 | const SpotMicroNodeConfig& smnc, 19 | const Command& cmd, 20 | SpotMicroMotionCmd* smmc, 21 | smk::BodyState* body_state_cmd) { 22 | 23 | if (smnc.debug_mode) { 24 | std::cout << "In Spot Micro Stand State" << std::endl; 25 | } 26 | 27 | 28 | 29 | if (cmd.getIdleCmd() == true) { 30 | // Call parent class's change state method 31 | changeState(smmc, std::make_unique()); 32 | 33 | } else if (cmd.getWalkCmd() == true) { 34 | changeState(smmc, std::make_unique()); 35 | 36 | } else { 37 | // Get command values 38 | cmd_state_.euler_angs.phi = cmd.getPhiCmd(); 39 | cmd_state_.euler_angs.theta = cmd.getThetaCmd(); 40 | cmd_state_.euler_angs.psi = cmd.getPsiCmd(); 41 | 42 | // Set command to filters 43 | angle_cmd_filters_.x.setCommand(cmd_state_.euler_angs.phi); 44 | angle_cmd_filters_.y.setCommand(cmd_state_.euler_angs.theta); 45 | angle_cmd_filters_.z.setCommand(cmd_state_.euler_angs.psi); 46 | 47 | // Run Filters and get command values 48 | body_state_cmd->euler_angs.phi = 49 | angle_cmd_filters_.x.runTimestepAndGetOutput(); 50 | body_state_cmd->euler_angs.theta = 51 | angle_cmd_filters_.y.runTimestepAndGetOutput(); 52 | body_state_cmd->euler_angs.psi = 53 | angle_cmd_filters_.z.runTimestepAndGetOutput(); 54 | 55 | body_state_cmd->xyz_pos = cmd_state_.xyz_pos; 56 | 57 | body_state_cmd->leg_feet_pos = cmd_state_.leg_feet_pos; 58 | 59 | // Set and publish command 60 | smmc->setServoCommandMessageData(); 61 | smmc->publishServoProportionalCommand(); 62 | } 63 | } 64 | 65 | 66 | 67 | void SpotMicroStandState::init(const smk::BodyState& body_state, 68 | const SpotMicroNodeConfig& smnc, 69 | const Command& cmd, 70 | SpotMicroMotionCmd* smmc) { 71 | // Set default stance 72 | cmd_state_.leg_feet_pos = smmc->getNeutralStance(); 73 | 74 | // End body state position and angles 75 | cmd_state_.euler_angs.phi = 0.0f; 76 | cmd_state_.euler_angs.theta = 0.0f; 77 | cmd_state_.euler_angs.psi = 0.0f; 78 | 79 | cmd_state_.xyz_pos.x = 0.0f; 80 | cmd_state_.xyz_pos.y = smnc.default_stand_height; 81 | cmd_state_.xyz_pos.z = 0.0f; 82 | 83 | float dt = smnc.dt; 84 | float tau = smnc.transit_tau; 85 | float rate_limit = smnc.transit_angle_rl; 86 | 87 | typedef RateLmtdFirstOrderFilter rlof; 88 | 89 | angle_cmd_filters_.x = 90 | rlof(dt, tau, cmd_state_.euler_angs.phi, rate_limit); 91 | angle_cmd_filters_.y = 92 | rlof(dt, tau, cmd_state_.euler_angs.theta, rate_limit); 93 | angle_cmd_filters_.z = 94 | rlof(dt, tau, cmd_state_.euler_angs.psi, rate_limit); 95 | 96 | } 97 | 98 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "spot_micro_state.h" 4 | #include "command.h" 5 | #include 6 | 7 | 8 | class SpotMicroStandState : public SpotMicroState { 9 | public: 10 | SpotMicroStandState(); // Constructor 11 | ~SpotMicroStandState(); // Destructor 12 | virtual void handleInputCommands(const smk::BodyState& body_state, 13 | const SpotMicroNodeConfig& smnc, 14 | const Command& cmd, 15 | SpotMicroMotionCmd* smmc, 16 | smk::BodyState* body_state_cmd); 17 | 18 | virtual void init(const smk::BodyState& body_state, 19 | const SpotMicroNodeConfig& smnc, 20 | const Command& cmd, 21 | SpotMicroMotionCmd* smmc); 22 | 23 | // Returns current state name as a string 24 | virtual std::string getCurrentStateName() { 25 | return "Stand"; 26 | } 27 | 28 | private: 29 | smk::BodyState cmd_state_; 30 | 31 | // Three filters for angle commands 32 | XyzFilters angle_cmd_filters_; 33 | }; 34 | 35 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp: -------------------------------------------------------------------------------- 1 | #include "spot_micro_state.h" 2 | 3 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 4 | #include "spot_micro_motion_cmd.h" 5 | 6 | using namespace smk; 7 | 8 | // Constructor 9 | SpotMicroState::SpotMicroState() { 10 | // Nothing needs to be done 11 | //std::cout << "SpotMicroState Ctor" << std::endl; 12 | } 13 | 14 | 15 | SpotMicroState::~SpotMicroState() { 16 | //std::cout << "SpotMicroState Dtor" << std::endl; 17 | } 18 | 19 | 20 | void SpotMicroState::changeState(SpotMicroMotionCmd* smmc, std::unique_ptr sms) { 21 | smmc->changeState(std::move(sms)); 22 | } 23 | 24 | 25 | void SpotMicroState::initBodyStateFilters( 26 | float dt, float tau, float rl, float rl_ang, 27 | const smk::BodyState& body_state, 28 | BodyStateFilters* body_state_filters) { 29 | 30 | // typedef for convenience 31 | typedef RateLmtdFirstOrderFilter rlfof; 32 | 33 | // Create and initialize filter objects 34 | body_state_filters->leg_right_front = 35 | {.x = rlfof(dt, tau, body_state.leg_feet_pos.right_front.x, rl), 36 | .y = rlfof(dt, tau, body_state.leg_feet_pos.right_front.y, rl), 37 | .z = rlfof(dt, tau, body_state.leg_feet_pos.right_front.z, rl)}; 38 | 39 | body_state_filters->leg_right_back = 40 | {.x = rlfof(dt, tau, body_state.leg_feet_pos.right_back.x, rl), 41 | .y = rlfof(dt, tau, body_state.leg_feet_pos.right_back.y, rl), 42 | .z = rlfof(dt, tau, body_state.leg_feet_pos.right_back.z, rl)}; 43 | 44 | body_state_filters->leg_left_back = 45 | {.x = rlfof(dt, tau, body_state.leg_feet_pos.left_back.x, rl), 46 | .y = rlfof(dt, tau, body_state.leg_feet_pos.left_back.y, rl), 47 | .z = rlfof(dt, tau, body_state.leg_feet_pos.left_back.z, rl)}; 48 | 49 | body_state_filters->leg_left_front = 50 | {.x = rlfof(dt, tau, body_state.leg_feet_pos.left_front.x, rl), 51 | .y = rlfof(dt, tau, body_state.leg_feet_pos.left_front.y, rl), 52 | .z = rlfof(dt, tau, body_state.leg_feet_pos.left_front.z, rl)}; 53 | 54 | body_state_filters->body_pos = 55 | {.x = rlfof(dt, tau, body_state.xyz_pos.x, rl), 56 | .y = rlfof(dt, tau, body_state.xyz_pos.y, rl), 57 | .z = rlfof(dt, tau, body_state.xyz_pos.z, rl)}; 58 | 59 | body_state_filters->body_angs = 60 | {.x = rlfof(dt, tau, body_state.euler_angs.phi, rl_ang), 61 | .y = rlfof(dt, tau, body_state.euler_angs.theta, rl_ang), 62 | .z = rlfof(dt, tau, body_state.euler_angs.psi, rl_ang)}; 63 | } 64 | 65 | 66 | void SpotMicroState::setBodyStateFilterCommands( 67 | const smk::BodyState& body_state, 68 | BodyStateFilters* body_state_filters) { 69 | 70 | // Set commands for all filters 71 | // Right front leg 72 | body_state_filters->leg_right_front.x.setCommand( 73 | body_state.leg_feet_pos.right_front.x); 74 | 75 | body_state_filters->leg_right_front.y.setCommand( 76 | body_state.leg_feet_pos.right_front.y); 77 | 78 | body_state_filters->leg_right_front.z.setCommand( 79 | body_state.leg_feet_pos.right_front.z); 80 | 81 | // Right Back leg 82 | body_state_filters->leg_right_back.x.setCommand( 83 | body_state.leg_feet_pos.right_back.x); 84 | 85 | body_state_filters->leg_right_back.y.setCommand( 86 | body_state.leg_feet_pos.right_back.y); 87 | 88 | body_state_filters->leg_right_back.z.setCommand( 89 | body_state.leg_feet_pos.right_back.z); 90 | 91 | // Left Back leg 92 | body_state_filters->leg_left_back.x.setCommand( 93 | body_state.leg_feet_pos.left_back.x); 94 | 95 | body_state_filters->leg_left_back.y.setCommand( 96 | body_state.leg_feet_pos.left_back.y); 97 | 98 | body_state_filters->leg_left_back.z.setCommand( 99 | body_state.leg_feet_pos.left_back.z); 100 | 101 | // Left front leg 102 | body_state_filters->leg_left_front.x.setCommand( 103 | body_state.leg_feet_pos.left_front.x); 104 | 105 | body_state_filters->leg_left_front.y.setCommand( 106 | body_state.leg_feet_pos.left_front.y); 107 | 108 | body_state_filters->leg_left_front.z.setCommand( 109 | body_state.leg_feet_pos.left_front.z); 110 | 111 | // Body pos 112 | body_state_filters->body_pos.x.setCommand(body_state.xyz_pos.x); 113 | body_state_filters->body_pos.y.setCommand(body_state.xyz_pos.y); 114 | body_state_filters->body_pos.z.setCommand(body_state.xyz_pos.z); 115 | 116 | body_state_filters->body_angs.x.setCommand(body_state.euler_angs.phi); 117 | body_state_filters->body_angs.y.setCommand(body_state.euler_angs.theta); 118 | body_state_filters->body_angs.z.setCommand(body_state.euler_angs.psi); 119 | } 120 | 121 | 122 | void SpotMicroState::runFilters(BodyStateFilters* body_state_filters) { 123 | 124 | // xyz body position 125 | body_state_filters->body_pos.x.runTimestep(); 126 | body_state_filters->body_pos.y.runTimestep(); 127 | body_state_filters->body_pos.z.runTimestep(); 128 | 129 | // Phi, theta psi body angles 130 | body_state_filters->body_angs.x.runTimestep(); 131 | body_state_filters->body_angs.y.runTimestep(); 132 | body_state_filters->body_angs.z.runTimestep(); 133 | 134 | // right back leg 135 | body_state_filters->leg_right_back.x.runTimestep(); 136 | body_state_filters->leg_right_back.y.runTimestep(); 137 | body_state_filters->leg_right_back.z.runTimestep(); 138 | 139 | // right front leg 140 | body_state_filters->leg_right_front.x.runTimestep(); 141 | body_state_filters->leg_right_front.y.runTimestep(); 142 | body_state_filters->leg_right_front.z.runTimestep(); 143 | 144 | // right front leg 145 | body_state_filters->leg_left_front.x.runTimestep(); 146 | body_state_filters->leg_left_front.y.runTimestep(); 147 | body_state_filters->leg_left_front.z.runTimestep(); 148 | 149 | // right front leg 150 | body_state_filters->leg_left_back.x.runTimestep(); 151 | body_state_filters->leg_left_back.y.runTimestep(); 152 | body_state_filters->leg_left_back.z.runTimestep(); 153 | } 154 | 155 | void SpotMicroState::assignFilterValuesToBodyState( 156 | const BodyStateFilters& body_state_filters, 157 | smk::BodyState* body_state) { 158 | 159 | body_state->xyz_pos.x = body_state_filters.body_pos.x.getOutput(); 160 | body_state->xyz_pos.y = body_state_filters.body_pos.y.getOutput(); 161 | body_state->xyz_pos.z = body_state_filters.body_pos.z.getOutput(); 162 | 163 | body_state->euler_angs.phi = body_state_filters.body_angs.x.getOutput(); 164 | body_state->euler_angs.theta = body_state_filters.body_angs.y.getOutput(); 165 | body_state->euler_angs.psi = body_state_filters.body_angs.z.getOutput(); 166 | 167 | // right back leg 168 | body_state->leg_feet_pos.right_back.x = 169 | body_state_filters.leg_right_back.x.getOutput(); 170 | body_state->leg_feet_pos.right_back.y = 171 | body_state_filters.leg_right_back.y.getOutput(); 172 | body_state->leg_feet_pos.right_back.z = 173 | body_state_filters.leg_right_back.z.getOutput(); 174 | 175 | // right front leg 176 | body_state->leg_feet_pos.right_front.x = 177 | body_state_filters.leg_right_front.x.getOutput(); 178 | body_state->leg_feet_pos.right_front.y = 179 | body_state_filters.leg_right_front.y.getOutput(); 180 | body_state->leg_feet_pos.right_front.z = 181 | body_state_filters.leg_right_front.z.getOutput(); 182 | 183 | // right front leg 184 | body_state->leg_feet_pos.left_front.x = 185 | body_state_filters.leg_left_front.x.getOutput(); 186 | body_state->leg_feet_pos.left_front.y = 187 | body_state_filters.leg_left_front.y.getOutput(); 188 | body_state->leg_feet_pos.left_front.z = 189 | body_state_filters.leg_left_front.z.getOutput(); 190 | 191 | // right front leg 192 | body_state->leg_feet_pos.left_back.x = 193 | body_state_filters.leg_left_back.x.getOutput(); 194 | body_state->leg_feet_pos.left_back.y = 195 | body_state_filters.leg_left_back.y.getOutput(); 196 | body_state->leg_feet_pos.left_back.z = 197 | body_state_filters.leg_left_back.z.getOutput(); 198 | } 199 | 200 | bool SpotMicroState::checkBodyStateEquality( 201 | const smk::BodyState& body_state1, 202 | const smk::BodyState& body_state2, 203 | float tol) { 204 | // Initialize return value true, run checks and set false if any fail 205 | bool ret_val = true; 206 | 207 | // right back leg 208 | if (std::fabs(body_state1.leg_feet_pos.right_back.x - 209 | body_state2.leg_feet_pos.right_back.x) > tol) { 210 | ret_val = false; 211 | } else if (std::fabs(body_state1.leg_feet_pos.right_back.y - 212 | body_state2.leg_feet_pos.right_back.y) > tol) { 213 | ret_val = false; 214 | } else if (std::fabs(body_state1.leg_feet_pos.right_back.z - 215 | body_state2.leg_feet_pos.right_back.z) > tol) { 216 | ret_val = false; 217 | 218 | // right front leg 219 | } else if (std::fabs(body_state1.leg_feet_pos.right_front.x - 220 | body_state2.leg_feet_pos.right_front.x) > tol) { 221 | ret_val = false; 222 | } else if (std::fabs(body_state1.leg_feet_pos.right_front.y - 223 | body_state2.leg_feet_pos.right_front.y) > tol) { 224 | ret_val = false; 225 | } else if (std::fabs(body_state1.leg_feet_pos.right_front.z - 226 | body_state2.leg_feet_pos.right_front.z) > tol) { 227 | ret_val = false; 228 | 229 | // left front leg 230 | } else if (std::fabs(body_state1.leg_feet_pos.left_front.x - 231 | body_state2.leg_feet_pos.left_front.x) > tol) { 232 | ret_val = false; 233 | } else if (std::fabs(body_state1.leg_feet_pos.left_front.y - 234 | body_state2.leg_feet_pos.left_front.y) > tol) { 235 | ret_val = false; 236 | } else if (std::fabs(body_state1.leg_feet_pos.left_front.z - 237 | body_state2.leg_feet_pos.left_front.z) > tol) { 238 | ret_val = false; 239 | 240 | // left back leg 241 | } else if (std::fabs(body_state1.leg_feet_pos.left_back.x - 242 | body_state2.leg_feet_pos.left_back.x) > tol) { 243 | ret_val = false; 244 | } else if (std::fabs(body_state1.leg_feet_pos.left_back.y - 245 | body_state2.leg_feet_pos.left_back.y) > tol) { 246 | ret_val = false; 247 | } else if (std::fabs(body_state1.leg_feet_pos.left_back.z - 248 | body_state2.leg_feet_pos.left_back.z) > tol) { 249 | ret_val = false; 250 | 251 | // body xyz pos 252 | } else if (std::fabs(body_state1.xyz_pos.x - 253 | body_state2.xyz_pos.x) > tol) { 254 | ret_val = false; 255 | } else if (std::fabs(body_state1.xyz_pos.y - 256 | body_state2.xyz_pos.y) > tol) { 257 | ret_val = false; 258 | } else if (std::fabs(body_state1.xyz_pos.z - 259 | body_state2.xyz_pos.z) > tol) { 260 | ret_val = false; 261 | 262 | // body angles 263 | } else if (std::fabs(body_state1.euler_angs.phi - 264 | body_state2.euler_angs.phi) > tol) { 265 | ret_val = false; 266 | } else if (std::fabs(body_state1.euler_angs.theta - 267 | body_state2.euler_angs.theta) > tol) { 268 | ret_val = false; 269 | } else if (std::fabs(body_state1.euler_angs.psi - 270 | body_state2.euler_angs.psi) > tol) { 271 | ret_val = false; 272 | } 273 | 274 | return ret_val; 275 | } 276 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 3 | 4 | #include "rate_limited_first_order_filter.h" 5 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 6 | #include "command.h" 7 | 8 | // Forward declaration of classes and structs. Can't include 9 | // spot_micro_motion_cmd.h here otherwise get circular dependence compile errors 10 | class SpotMicroMotionCmd; 11 | struct SpotMicroNodeConfig; 12 | 13 | 14 | // Struct holding a filter for three things relative to axes 15 | struct XyzFilters { 16 | RateLmtdFirstOrderFilter x; 17 | RateLmtdFirstOrderFilter y; 18 | RateLmtdFirstOrderFilter z; 19 | }; 20 | 21 | // Convenience structure for hold filters for all possible transitory states 22 | struct BodyStateFilters { 23 | XyzFilters leg_right_back; 24 | XyzFilters leg_right_front; 25 | XyzFilters leg_left_front; 26 | XyzFilters leg_left_back; 27 | XyzFilters body_pos; 28 | XyzFilters body_angs; 29 | }; 30 | 31 | class SpotMicroState { 32 | public: 33 | 34 | // Constructor 35 | SpotMicroState(); 36 | 37 | // Destructor 38 | virtual ~SpotMicroState(); 39 | 40 | // virtual method to handle input commands, may change SpotMicroMotionCmd object 41 | // passed by reference. 42 | virtual void handleInputCommands(const smk::BodyState& body_state, 43 | const SpotMicroNodeConfig& smnc, 44 | const Command& cmd, 45 | SpotMicroMotionCmd* smmc, 46 | smk::BodyState* body_state_cmd) {} 47 | 48 | virtual void init(const smk::BodyState& body_state, 49 | const SpotMicroNodeConfig& smnc, 50 | const Command& cmd, 51 | SpotMicroMotionCmd* smmc) {} 52 | 53 | virtual std::string getCurrentStateName() { 54 | return "None"; 55 | } 56 | 57 | protected: 58 | 59 | // Calls SpotMicroMotionCmd's method to change the currently active state 60 | void changeState(SpotMicroMotionCmd* smmc, std::unique_ptr sms); 61 | 62 | // Initializes set of filters controlling body state values to values 63 | // contained in body_state 64 | virtual void initBodyStateFilters(float dt, float tau, float rl, float rl_ang, 65 | const smk::BodyState& body_state, 66 | BodyStateFilters* body_state_filters); 67 | 68 | // Sets body state filter commands to the values contained in body_state 69 | virtual void setBodyStateFilterCommands(const smk::BodyState& body_state, 70 | BodyStateFilters* body_state_filters); 71 | 72 | // Calls run timestep method for all body state filters 73 | virtual void runFilters(BodyStateFilters* body_state_filters); 74 | 75 | // Assigns current filter value to body state 76 | virtual void assignFilterValuesToBodyState( 77 | const BodyStateFilters& body_state_filters, 78 | smk::BodyState* body_state); 79 | 80 | // Checks equality of body state structs to an absolute tolerance. Returns 81 | // true if all absolute value differences of body state values are within 82 | // tolernace 83 | virtual bool checkBodyStateEquality(const smk::BodyState& body_state1, 84 | const smk::BodyState& body_state2, 85 | float tol); 86 | 87 | }; 88 | 89 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp: -------------------------------------------------------------------------------- 1 | #include "spot_micro_transition_idle.h" 2 | 3 | #include "spot_micro_idle.h" 4 | #include "spot_micro_motion_cmd.h" 5 | #include "spot_micro_state.h" 6 | 7 | SpotMicroTransitionIdleState::SpotMicroTransitionIdleState() { 8 | // Construcotr, doesn't need to do anything, for now... 9 | //std::cout << "SpotMicroTransitionIdleState Ctor" << std::endl; 10 | } 11 | 12 | SpotMicroTransitionIdleState::~SpotMicroTransitionIdleState() { 13 | //std::cout << "SpotMicroTransitionIdleState Dtor" << std::endl; 14 | } 15 | 16 | 17 | void SpotMicroTransitionIdleState::init(const smk::BodyState& body_state, 18 | const SpotMicroNodeConfig& smnc, 19 | const Command& cmd, 20 | SpotMicroMotionCmd* smmc) { 21 | // Set initial state and end state 22 | // Get starting body state 23 | start_body_state_ = body_state; 24 | 25 | // Create end state 26 | // Create end state feet positions, a default foot stance 27 | end_body_state_.leg_feet_pos = smmc->getLieDownStance(); 28 | 29 | // End body state position and angles 30 | end_body_state_.euler_angs.phi = 0.0f; 31 | end_body_state_.euler_angs.theta = 0.0f; 32 | end_body_state_.euler_angs.psi = 0.0f; 33 | 34 | end_body_state_.xyz_pos.x = 0.0f; 35 | end_body_state_.xyz_pos.y = smnc.lie_down_height; 36 | end_body_state_.xyz_pos.z = 0.0f; 37 | 38 | // Initialize filters 39 | float dt = smnc.dt; 40 | float tau = smnc.transit_tau; 41 | float rl = smnc.transit_rl; 42 | float rl_ang = smnc.transit_angle_rl; 43 | 44 | initBodyStateFilters(dt, tau, rl, rl_ang, 45 | body_state, &body_state_filters_); 46 | 47 | // Set destination commands for all filters 48 | setBodyStateFilterCommands(end_body_state_, &body_state_filters_); 49 | } 50 | 51 | 52 | void SpotMicroTransitionIdleState::handleInputCommands( 53 | const smk::BodyState& body_state, 54 | const SpotMicroNodeConfig& smnc, 55 | const Command& cmd, 56 | SpotMicroMotionCmd* smmc, 57 | smk:: BodyState* body_state_cmd) { 58 | if (smnc.debug_mode) { 59 | std::cout << "In Spot Micro Transition Idle State" << std::endl; 60 | } 61 | 62 | // Check if desired end state reached, if so, change to stand state 63 | if (checkBodyStateEquality(body_state, end_body_state_, 0.001f)) { 64 | changeState(smmc, std::make_unique()); 65 | 66 | } else { 67 | // Otherwise, rise filters and assign output values to body state command 68 | runFilters(&body_state_filters_); 69 | 70 | // Assing filter values to cmd 71 | assignFilterValuesToBodyState(body_state_filters_, 72 | body_state_cmd); 73 | 74 | // Send command 75 | smmc->setServoCommandMessageData(); 76 | smmc->publishServoProportionalCommand(); 77 | 78 | } 79 | 80 | } 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "spot_micro_state.h" 4 | #include "command.h" 5 | #include "rate_limited_first_order_filter.h" 6 | 7 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 8 | 9 | class SpotMicroTransitionIdleState : public SpotMicroState { 10 | public: 11 | SpotMicroTransitionIdleState(); // Constructor 12 | ~SpotMicroTransitionIdleState(); // Destructor 13 | 14 | virtual void handleInputCommands(const smk::BodyState& body_state, 15 | const SpotMicroNodeConfig& smnc, 16 | const Command& cmd, 17 | SpotMicroMotionCmd* smmc, 18 | smk::BodyState* body_state_cmd); 19 | 20 | virtual void init(const smk::BodyState& body_state, 21 | const SpotMicroNodeConfig& smnc, 22 | const Command& cmd, 23 | SpotMicroMotionCmd* smmc); 24 | 25 | // Returns current state name as a string 26 | virtual std::string getCurrentStateName() { 27 | return "Transit Idle"; 28 | } 29 | private: 30 | smk::BodyState start_body_state_; 31 | smk::BodyState end_body_state_; 32 | RateLmtdFirstOrderFilter rlfof; 33 | BodyStateFilters body_state_filters_; 34 | }; 35 | 36 | 37 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp: -------------------------------------------------------------------------------- 1 | #include "spot_micro_transition_stand.h" 2 | 3 | #include "spot_micro_stand.h" 4 | #include "spot_micro_motion_cmd.h" 5 | #include "spot_micro_state.h" 6 | 7 | SpotMicroTransitionStandState::SpotMicroTransitionStandState() { 8 | // Construcotr, doesn't need to do anything, for now... 9 | //std::cout << "SpotMicroTransitionStandState Ctor" << std::endl; 10 | } 11 | 12 | SpotMicroTransitionStandState::~SpotMicroTransitionStandState() { 13 | //std::cout << "SpotMicroTransitionStandState Dtor" << std::endl; 14 | } 15 | 16 | 17 | void SpotMicroTransitionStandState::init(const smk::BodyState& body_state, 18 | const SpotMicroNodeConfig& smnc, 19 | const Command& cmd, 20 | SpotMicroMotionCmd* smmc) { 21 | // Set initial state and end state 22 | // Get starting body state 23 | start_body_state_ = body_state; 24 | 25 | // Create end state 26 | // Create end state feet positions, a default foot stance 27 | end_body_state_.leg_feet_pos = smmc->getNeutralStance(); 28 | 29 | // End body state position and angles 30 | end_body_state_.euler_angs.phi = 0.0f; 31 | end_body_state_.euler_angs.theta = 0.0f; 32 | end_body_state_.euler_angs.psi = 0.0f; 33 | 34 | end_body_state_.xyz_pos.x = 0.0f; 35 | end_body_state_.xyz_pos.y = smnc.default_stand_height; 36 | end_body_state_.xyz_pos.z = 0.0f; 37 | 38 | // Initialize filters 39 | float dt = smnc.dt; 40 | float tau = smnc.transit_tau; 41 | float rl = smnc.transit_rl; 42 | float rl_ang = smnc.transit_angle_rl; 43 | 44 | initBodyStateFilters(dt, tau, rl, rl_ang, 45 | body_state, &body_state_filters_); 46 | 47 | // Set destination commands for all filters 48 | setBodyStateFilterCommands(end_body_state_, &body_state_filters_); 49 | } 50 | 51 | 52 | void SpotMicroTransitionStandState::handleInputCommands( 53 | const smk::BodyState& body_state, 54 | const SpotMicroNodeConfig& smnc, 55 | const Command& cmd, 56 | SpotMicroMotionCmd* smmc, 57 | smk:: BodyState* body_state_cmd) { 58 | if (smnc.debug_mode) { 59 | std::cout << "In Spot Micro Transition Stand State" << std::endl; 60 | } 61 | 62 | // Check if desired end state reached, if so, change to stand state 63 | if (checkBodyStateEquality(body_state, end_body_state_, 0.001f)) { 64 | changeState(smmc, std::make_unique()); 65 | 66 | } else { 67 | // Otherwise, rise filters and assign output values to body state command 68 | runFilters(&body_state_filters_); 69 | 70 | // Assing filter values to cmd 71 | assignFilterValuesToBodyState(body_state_filters_, 72 | body_state_cmd); 73 | 74 | // Send command 75 | smmc->setServoCommandMessageData(); 76 | smmc->publishServoProportionalCommand(); 77 | 78 | } 79 | 80 | } 81 | 82 | 83 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "spot_micro_state.h" 6 | #include "command.h" 7 | #include "rate_limited_first_order_filter.h" 8 | 9 | #include "spot_micro_kinematics/spot_micro_kinematics.h" 10 | 11 | class SpotMicroTransitionStandState : public SpotMicroState { 12 | public: 13 | SpotMicroTransitionStandState(); // Constructor 14 | ~SpotMicroTransitionStandState(); // Destructor 15 | 16 | virtual void handleInputCommands(const smk::BodyState& body_state, 17 | const SpotMicroNodeConfig& smnc, 18 | const Command& cmd, 19 | SpotMicroMotionCmd* smmc, 20 | smk::BodyState* body_state_cmd); 21 | 22 | virtual void init(const smk::BodyState& body_state, 23 | const SpotMicroNodeConfig& smnc, 24 | const Command& cmd, 25 | SpotMicroMotionCmd* smmc); 26 | 27 | // Returns current state name as a string 28 | virtual std::string getCurrentStateName() { 29 | return "Transit Stand"; 30 | } 31 | private: 32 | smk::BodyState start_body_state_; 33 | smk::BodyState end_body_state_; 34 | RateLmtdFirstOrderFilter rlfof; 35 | BodyStateFilters body_state_filters_; 36 | }; 37 | 38 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "spot_micro_motion_cmd.h" 4 | #include "spot_micro_state.h" 5 | #include "command.h" 6 | 7 | struct ContactFeet { 8 | bool right_back_in_swing; 9 | bool right_front_in_swing; 10 | bool left_front_in_swing; 11 | bool left_back_in_swing;\ 12 | }; 13 | 14 | class SpotMicroWalkState : public SpotMicroState { 15 | public: 16 | SpotMicroWalkState(); // Constructor 17 | ~SpotMicroWalkState(); // Destructor 18 | 19 | virtual void handleInputCommands(const smk::BodyState& body_state, 20 | const SpotMicroNodeConfig& smnc, 21 | const Command& cmd, 22 | SpotMicroMotionCmd* smmc, 23 | smk::BodyState* body_state_cmd); 24 | 25 | virtual void init(const smk::BodyState& body_state, 26 | const SpotMicroNodeConfig& smnc, 27 | const Command& cmd, 28 | SpotMicroMotionCmd* smmc); 29 | 30 | // Returns current state name as a string 31 | virtual std::string getCurrentStateName() { 32 | return "Walk"; 33 | } 34 | 35 | private: 36 | 37 | SpotMicroNodeConfig smnc_; 38 | smk::BodyState cmd_state_; 39 | 40 | int ticks_; 41 | int phase_index_; 42 | int subphase_ticks_; 43 | ContactFeet contact_feet_states_; 44 | 45 | // Updates the integer phase index cooresponding to the current phase the 46 | // robot gait should be in. Updates the subphase ticks, the ticks since the 47 | // start of the current phase. And updates the contact feet states 48 | // representing which feet are in swing and stance phases 49 | void updatePhaseData(); 50 | 51 | 52 | // Steps the gait controller one timestep, sets the feet command state, and 53 | // possibly other command states (such as xyz position, euler angles) if 54 | // necessary 55 | smk::LegsFootPos stepGait(const smk::BodyState& body_state, 56 | const Command& cmd, 57 | const SpotMicroNodeConfig& smnc, 58 | const smk::LegsFootPos& default_stance_feet_pos); 59 | 60 | // Returns new foot position incremented by stance controller 61 | smk::Point stanceController(const smk::Point& foot_pos, 62 | const Command& cmd, 63 | const SpotMicroNodeConfig& smnc); 64 | 65 | // Returns new foot position incremented by swing leg controller 66 | smk::Point swingLegController(const smk::Point& foot_pos, 67 | const Command& cmd, 68 | const SpotMicroNodeConfig& smnc, 69 | float swing_proportion, 70 | const smk::Point& default_stance_foot_pos); 71 | 72 | // Steps the body shift controller that shifts the body xyz position 73 | // to maintain balance during the gait cycle 74 | smk::Point stepBodyShift(const smk::BodyState& body_state, 75 | const Command& cmd, 76 | const SpotMicroNodeConfig& smnc); 77 | }; 78 | 79 | 80 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp: -------------------------------------------------------------------------------- 1 | // Node file to create object and initialising the ROS node 2 | #include "spot_micro_motion_cmd.h" 3 | #include 4 | 5 | 6 | int main(int argc, char** argv) { 7 | /* initialising the ROS node creating node handle 8 | for regestring it to the master and then private node handle to 9 | handle the parameters */ 10 | ros::init(argc, argv, "spot_micro_motion_cmd"); 11 | ros::NodeHandle nh; 12 | ros::NodeHandle pnh("~"); 13 | 14 | SpotMicroMotionCmd node(nh,pnh); // Creating the object 15 | 16 | ros::Rate rate(1.0/node.getNodeConfig().dt); // Defing the looping rate 17 | 18 | 19 | // Only proceed if servo configuration publishing succeeds 20 | if (node.publishServoConfiguration()) { 21 | 22 | bool debug_mode = node.getNodeConfig().debug_mode; 23 | ros::Time begin; 24 | /* Looking for any interupt else it will continue looping */ 25 | // Main loop runs indefinitely unless there is an interupt call 26 | while (ros::ok()) 27 | { 28 | if (debug_mode) { 29 | begin = ros::Time::now(); 30 | } 31 | 32 | node.runOnce(); 33 | ros::spinOnce(); 34 | rate.sleep(); 35 | 36 | if (debug_mode) { 37 | std::cout << (ros::Time::now() - begin) << std::endl; 38 | } 39 | } 40 | 41 | } 42 | return 0; 43 | } 44 | -------------------------------------------------------------------------------- /spot_micro_motion_cmd/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils.h" 2 | 3 | #include 4 | #include 5 | #include "tf2/LinearMath/Quaternion.h" 6 | #include "tf2_eigen/tf2_eigen.h" 7 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 8 | 9 | using namespace Eigen; 10 | using namespace geometry_msgs; 11 | 12 | 13 | Affine3d matrix4fToAffine3d(const Matrix4f& in) { 14 | // Convert a Eigen Matrix4F to an Affine3d by first casting 15 | // float to double (to a Matrix4d), then calling the constructor for Affine3d 16 | // with the Matrix4d 17 | return Affine3d(in.cast()); 18 | } 19 | 20 | 21 | // Create a ROS tf2 TransformStamped from a Eigen Affine3d, 22 | // parent frame id and child frame id. Stamped with current time, 23 | // so should be broadcast ASAP 24 | TransformStamped eigAndFramesToTrans( 25 | const Affine3d& transform, 26 | std::string parent_frame_id, std::string child_frame_id) { 27 | 28 | TransformStamped transform_stamped = tf2::eigenToTransform(transform); 29 | 30 | transform_stamped.header.stamp = ros::Time::now(); 31 | transform_stamped.header.frame_id = parent_frame_id; 32 | transform_stamped.child_frame_id = child_frame_id; 33 | 34 | return transform_stamped; 35 | } 36 | 37 | 38 | // Create a transform from a translation, rotation, and parent and 39 | // child frame IDs. Will stamp the transform with ros::Time::now(), 40 | // so the returned transform should be broadcast asap 41 | TransformStamped createTransform( 42 | std::string parent_frame_id, std::string child_frame_id, 43 | double x, double y, double z, 44 | double roll, double pitch, double yaw) { 45 | 46 | TransformStamped tr_stamped; 47 | 48 | tr_stamped.header.stamp = ros::Time::now(); 49 | tr_stamped.header.frame_id = parent_frame_id; 50 | tr_stamped.child_frame_id = child_frame_id; 51 | 52 | tr_stamped.transform.translation.x = x; 53 | tr_stamped.transform.translation.y = y; 54 | tr_stamped.transform.translation.z = z; 55 | tf2::Quaternion q; 56 | q.setRPY(roll, pitch, yaw); 57 | tr_stamped.transform.rotation.x = q.x(); 58 | tr_stamped.transform.rotation.y = q.y(); 59 | tr_stamped.transform.rotation.z = q.z(); 60 | tr_stamped.transform.rotation.w = q.w(); 61 | 62 | return tr_stamped; 63 | } 64 | 65 | -------------------------------------------------------------------------------- /spot_micro_plot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_micro_plot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES spot_micro_keyboard_command 107 | # CATKIN_DEPENDS rospy std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/spot_micro_keyboard_command.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/spot_micro_keyboard_command_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # catkin_install_python(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_keyboard_command.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | -------------------------------------------------------------------------------- /spot_micro_plot/launch/start_plotting.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /spot_micro_plot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_plot 4 | 0.0.0 5 | The spot_micro_plot package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | std_msgs 54 | rospy 55 | std_msgs 56 | rospy 57 | std_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /spot_micro_plot/scripts/spotMicroPlot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import numpy as np 4 | import time 5 | 6 | import rospy 7 | from spot_micro_kinematics_python.spot_micro_stick_figure import SpotMicroStickFigure 8 | from spot_micro_kinematics_python.utilities import transformations 9 | from math import pi 10 | from std_msgs.msg import Float32, Bool 11 | from std_msgs.msg import Float32MultiArray 12 | import matplotlib.pyplot as plt 13 | import mpl_toolkits.mplot3d.axes3d as p3 14 | import matplotlib.animation as animation 15 | 16 | 17 | ######################################################################### 18 | ########################## Global Variables ############################# 19 | ######################################################################### 20 | num_servos = 12 21 | 22 | r2d = 180/pi 23 | d2r = pi/180 24 | 25 | 26 | # Attaching 3D axis to the figure 27 | fig = plt.figure() 28 | ax = p3.Axes3D(fig) 29 | ax.set_proj_type('ortho') 30 | ax.set_facecolor('black') 31 | 32 | ax.set_xlabel('X') 33 | ax.set_ylabel('Z') 34 | ax.set_zlabel('Y') 35 | 36 | ax.set_xlim3d([-0.2, 0.2]) 37 | ax.set_zlim3d([0, 0.3]) 38 | ax.set_ylim3d([0.2,-0.2]) 39 | 40 | x = 0 41 | 42 | # def update_x_speed_cmd(msg): 43 | # '''Updates x speed command from received message''' 44 | # x = msg.data 45 | # print('here') 46 | 47 | def update_lines(num, x, lines): 48 | msg = rospy.wait_for_message("/body_state", Float32MultiArray, timeout=None) 49 | 50 | foot_data = np.array([ [msg.data[0], msg.data[1], msg.data[2]], 51 | [msg.data[3], msg.data[4], msg.data[5]], 52 | [msg.data[6], msg.data[7], msg.data[8]], 53 | [msg.data[9], msg.data[10], msg.data[11]] ]) 54 | 55 | xpos = msg.data[12] 56 | ypos = msg.data[13] 57 | zpos = msg.data[14] 58 | 59 | phi = msg.data[15] 60 | theta = msg.data[16] 61 | psi = msg.data[17] 62 | 63 | # print(foot_data) 64 | sm.set_absolute_foot_coordinates(foot_data) 65 | temp_rot = transformations.rotxyz(phi, psi, theta) 66 | temp_pose = np.identity(4) 67 | temp_pose[0:3, 0:3] = temp_rot 68 | temp_pose[0,3] = xpos 69 | temp_pose[1,3] = ypos 70 | temp_pose[2,3] = zpos 71 | 72 | sm.set_absolute_body_pose(temp_pose) 73 | 74 | # print(len(msg.data)) 75 | # print(msg.data[0]) 76 | # l.set_data([0, num/100.0], [0, num/100.0]) 77 | # print(num/100.0) 78 | # l.set_3d_properties([0, num/100.0]) 79 | 80 | # Get leg coordinates and append to data list 81 | coord_data = sm.get_leg_coordinates() 82 | 83 | line_to_leg__and_link_dict = {4:(0,0), 84 | 5:(0,1), 85 | 6:(0,2), 86 | 7:(1,0), 87 | 8:(1,1), 88 | 9:(1,2), 89 | 10:(2,0), 90 | 11:(2,1), 91 | 12:(2,2), 92 | 13:(3,0), 93 | 14:(3,1), 94 | 15:(3,2)} 95 | 96 | for line, i in zip(lines, range(len(lines))): 97 | 98 | line.set_linewidth(4) 99 | if i < 4: 100 | # First four lines are the square body 101 | if i == 3: 102 | ind = -1 103 | else: 104 | ind = i 105 | x_vals = [coord_data[ind][0][0], coord_data[ind+1][0][0]] 106 | y_vals = [coord_data[ind][0][1], coord_data[ind+1][0][1]] 107 | z_vals = [coord_data[ind][0][2], coord_data[ind+1][0][2]] 108 | # NOTE: there is no .set_data() for 3 dim data... 109 | line.set_data(x_vals,z_vals) 110 | line.set_3d_properties(y_vals) 111 | 112 | # Next 12 lines are legs 113 | # Leg 1, link 1, link 2, link 3 114 | # Leg 2, link 1, link 2, link 3... 115 | else: 116 | leg_num = line_to_leg__and_link_dict[i][0] 117 | link_num = line_to_leg__and_link_dict[i][1] 118 | x_vals = [coord_data[leg_num][link_num][0], coord_data[leg_num][link_num+1][0]] 119 | y_vals = [coord_data[leg_num][link_num][1], coord_data[leg_num][link_num+1][1]] 120 | z_vals = [coord_data[leg_num][link_num][2], coord_data[leg_num][link_num+1][2]] 121 | 122 | line.set_data(x_vals,z_vals) 123 | line.set_3d_properties(y_vals) 124 | return lines 125 | 126 | # Set up and title the ros node for this code 127 | rospy.init_node('spot_micro_plot') 128 | 129 | # Instantiate spot micro stick figure obeject 130 | sm = SpotMicroStickFigure(x=0,y=0.093,z=0) 131 | 132 | coords = sm.get_leg_coordinates() 133 | 134 | # Initialize empty list top hold line objects 135 | lines = [] 136 | 137 | # Construct the body of 4 lines from the first point of each leg (the four corners of the body) 138 | for i in range(4): 139 | # For last leg, connect back to first leg point 140 | if i == 3: 141 | ind = -1 142 | else: 143 | ind = i 144 | 145 | # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure 146 | # appear oriented better 147 | x_vals = [coords[ind][0][0], coords[ind+1][0][0]] 148 | y_vals = [coords[ind][0][1], coords[ind+1][0][1]] 149 | z_vals = [coords[ind][0][2], coords[ind+1][0][2]] 150 | lines.append(ax.plot(x_vals,z_vals,y_vals,color='k')[0]) 151 | 152 | 153 | # Plot color order for leg links: (hip, upper leg, lower leg) 154 | plt_colors = ['r','c','b'] 155 | for leg in coords: 156 | for i in range(3): 157 | 158 | # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure 159 | # appear oriented better 160 | x_vals = [leg[i][0], leg[i+1][0]] 161 | y_vals = [leg[i][1], leg[i+1][1]] 162 | z_vals = [leg[i][2], leg[i+1][2]] 163 | lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0]) 164 | 165 | 166 | 167 | 168 | lines_ani = animation.FuncAnimation(fig, update_lines, frames=1000, fargs=(x,lines), interval=100) 169 | 170 | plt.show() 171 | 172 | 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /spot_micro_rviz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(spot_micro_rviz) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES spotmicroai 107 | # CATKIN_DEPENDS roscpp rospy 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/spotmicroai.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/spotmicroai_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables and/or libraries for installation 167 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 168 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark cpp header files for installation 174 | # install(DIRECTORY include/${PROJECT_NAME}/ 175 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 176 | # FILES_MATCHING PATTERN "*.h" 177 | # PATTERN ".svn" EXCLUDE 178 | # ) 179 | 180 | ## Mark other files for installation (e.g. launch and bag files, etc.) 181 | # install(FILES 182 | # # myfile1 183 | # # myfile2 184 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 185 | # ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_spotmicroai.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /spot_micro_rviz/launch/show_and_move_model_via_gui.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /spot_micro_rviz/launch/show_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /spot_micro_rviz/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /spot_micro_rviz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | spot_micro_rviz 4 | 0.0.0 5 | Spot Micro RVIZ Visualization Package 6 | 7 | 8 | 9 | 10 | mike 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | roscpp 41 | rospy 42 | roscpp 43 | rospy 44 | roscpp 45 | rospy 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /spot_micro_rviz/rviz/mapping_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /Path1 11 | - /Pose1 12 | Splitter Ratio: 0.5 13 | Tree Height: 540 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.03 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 0.7 55 | Class: rviz/Map 56 | Color Scheme: map 57 | Draw Behind: false 58 | Enabled: true 59 | Name: Map 60 | Topic: /map 61 | Value: true 62 | - Alpha: 1 63 | Buffer Length: 1 64 | Class: rviz/Path 65 | Color: 25; 255; 0 66 | Enabled: true 67 | Name: Path 68 | Topic: /trajectory 69 | Value: true 70 | - Alpha: 1 71 | Axes Length: 1 72 | Axes Radius: 0.1 73 | Class: rviz/Pose 74 | Color: 255; 25; 0 75 | Enabled: true 76 | Head Length: 0.3 77 | Head Radius: 0.1 78 | Name: Pose 79 | Shaft Length: 1 80 | Shaft Radius: 0.05 81 | Shape: Axes 82 | Topic: /slam_out_pose 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 48; 48; 48 87 | Fixed Frame: map 88 | Frame Rate: 30 89 | Name: root 90 | Tools: 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/MoveCamera 94 | - Class: rviz/Select 95 | - Class: rviz/FocusCamera 96 | - Class: rviz/Measure 97 | - Class: rviz/SetInitialPose 98 | Topic: /initialpose 99 | - Class: rviz/SetGoal 100 | Topic: /move_base_simple/goal 101 | - Class: rviz/PublishPoint 102 | Single click: true 103 | Topic: /clicked_point 104 | Value: true 105 | Views: 106 | Current: 107 | Class: rviz/XYOrbit 108 | Distance: 44.388 109 | Focal Point: 110 | X: 9.50434 111 | Y: -0.685607 112 | Z: 0 113 | Name: Current View 114 | Near Clip Distance: 0.01 115 | Pitch: 1.56976 116 | Target Frame: 117 | Value: XYOrbit (rviz) 118 | Yaw: -0.7846 119 | Saved: ~ 120 | Window Geometry: 121 | Displays: 122 | collapsed: false 123 | Height: 846 124 | Hide Left Dock: false 125 | Hide Right Dock: false 126 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 127 | Selection: 128 | collapsed: false 129 | Time: 130 | collapsed: false 131 | Tool Properties: 132 | collapsed: false 133 | Views: 134 | collapsed: false 135 | Width: 1200 136 | X: 53 137 | Y: 60 -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/gen_urdf.sh: -------------------------------------------------------------------------------- 1 | export LC_NUMERIC="en_US.UTF-8" 2 | xacro spot_micro.urdf.xacro > spot_micro.urdf 3 | -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/backpart.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/backpart.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/foot.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/frontpart.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/frontpart.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/larm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/larm.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/larm_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/larm_cover.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/lfoot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/lfoot.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/lshoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/lshoulder.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/mainbody.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/mainbody.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/rarm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/rarm.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/rarm_cover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/rarm_cover.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/rfoot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/rfoot.stl -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/rplidar_main.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/rplidar_main.STL -------------------------------------------------------------------------------- /spot_micro_rviz/urdf/stl/rshoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mike4192/spotMicro/2a34f5d303dff91b62180031b31ef512a672f3c3/spot_micro_rviz/urdf/stl/rshoulder.stl --------------------------------------------------------------------------------