diff --git a/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt
new file mode 120000
index 0000000000000000000000000000000000000000..581e61db89fce59006b1ceb2d208d9f3e5fbcb5e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt
@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/10_left_link3_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/10_left_link3_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..a5a827e6f523d76522eec7d0c63653223f80fd70
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/10_left_link3_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/11_left_link3_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/11_left_link3_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..c6b504c9ff6f7938809f6763e3943b8dff11eff0
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/11_left_link3_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/12_robot_back_overview.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/12_robot_back_overview.png
new file mode 100755
index 0000000000000000000000000000000000000000..6ca0d007c83870b6d912162877292b66df8fd6cd
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/12_robot_back_overview.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/13_robot_back_angle_directions.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/13_robot_back_angle_directions.png
new file mode 100755
index 0000000000000000000000000000000000000000..4654649ccbd79b234570f144a7084a04e2418f91
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/13_robot_back_angle_directions.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/14_right_link1_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/14_right_link1_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..0094a13641b1da2603e7fd571558f71e99076416
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/14_right_link1_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/15_right_link1_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/15_right_link1_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..8f41d4e96012b4c37c89287238d16919fe81d1d9
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/15_right_link1_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/16_left_link1_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/16_left_link1_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..566b549dc4ce1b3f473a7adaddda0ec75996e802
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/16_left_link1_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/17_left_link1_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/17_left_link1_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..d575b466f2baad37d262a8e4a261628407c366f3
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/17_left_link1_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/1_robot_right_links.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/1_robot_right_links.png
new file mode 100755
index 0000000000000000000000000000000000000000..8f2bf335388b41012f754918df025f0c89c8aad9
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/1_robot_right_links.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/2_right_straight_links.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/2_right_straight_links.png
new file mode 100755
index 0000000000000000000000000000000000000000..b9c745da6b31d04228c6f20af007662d074579ab
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/2_right_straight_links.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/3_right_link_angles_example.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/3_right_link_angles_example.png
new file mode 100755
index 0000000000000000000000000000000000000000..57c323c86660ce0839482f3b8fc1207591c6d7e4
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/3_right_link_angles_example.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..6a700d56a327b9731fc1bac7fd3cc79799b10200
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..b47ab12abe9f35f801a2cc9eae3ad3171911efc7
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/5_right_link3_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/5_right_link3_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..ce1e72e3ff52841dd889721fa3c22153c543ed52
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/5_right_link3_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/6_right_link3_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/6_right_link3_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..780bba41e09ba84a70b8db7fb9537b68bdbb4e5e
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/6_right_link3_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/7_robot_left_overview.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/7_robot_left_overview.png
new file mode 100755
index 0000000000000000000000000000000000000000..8f01dd6ff31213d9061362068d90097d8b9cfb61
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/7_robot_left_overview.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/8_left_link2_config_step_1.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/8_left_link2_config_step_1.png
new file mode 100755
index 0000000000000000000000000000000000000000..a5ddc7126f6f88c86bf2f9f33588e8ba938694b3
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/8_left_link2_config_step_1.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/9_left_link2_config_step_2.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/9_left_link2_config_step_2.png
new file mode 100755
index 0000000000000000000000000000000000000000..df3e760014c07d9662e7ff817d0764e27af4bccd
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/9_left_link2_config_step_2.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/custom_shoulder_assembly.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/custom_shoulder_assembly.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..77b8e91982a50282effb24fe6faa81d587071305
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/custom_shoulder_assembly.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/kinematic_coord_system.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/kinematic_coord_system.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..ef34d68591983c70242357b6556518ffa22df139
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/kinematic_coord_system.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/lidar_mount.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/lidar_mount.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..d22e0173e8398e588b4a2f1e1ba2e5ae23505c32
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/lidar_mount.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_bottom.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_bottom.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..e3ccbeaa0bfc155cbb6d482a5ac439b4edfac21a
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_bottom.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..2a3f23ac0a791f91411d632a5fa95079fe8a31bc
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top_no_servos.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top_no_servos.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..6e2a65460690e085a3aae06b2d9f369bdabe091e
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top_no_servos.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/rpi_platform.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/rpi_platform.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..b9df2396e3aaf356ed369dd2b1c7d0a72ecad768
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/rpi_platform.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/rviz_animation.gif b/spotMicroCode/catkin_ws/src/spotMicro/assets/rviz_animation.gif
new file mode 100644
index 0000000000000000000000000000000000000000..de2af20f5834453c36c49443de0feddab82c2d48
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/rviz_animation.gif differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_calibration_spreadsheet.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_calibration_spreadsheet.png
new file mode 100644
index 0000000000000000000000000000000000000000..e2c2aadb12300b6971e5561e0b69c7e51d458cea
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_calibration_spreadsheet.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_move_prompt.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_move_prompt.png
new file mode 100755
index 0000000000000000000000000000000000000000..8f88e439becccbfec23b38615e483df6da44b186
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_move_prompt.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_slam.gif b/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_slam.gif
new file mode 100644
index 0000000000000000000000000000000000000000..bb234030637c2f67616a29b7c480c8c7aa19f810
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_slam.gif differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_walking.gif b/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_walking.gif
new file mode 100644
index 0000000000000000000000000000000000000000..9d33e591bd7d8b75abedb5e0efd3463345b10d5d
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_walking.gif differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/state_machine.png b/spotMicroCode/catkin_ws/src/spotMicro/assets/state_machine.png
new file mode 100644
index 0000000000000000000000000000000000000000..aedbd0f50a70d7b5939b3578235531be5040aa0b
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/state_machine.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/tf2_coord_system.jpg b/spotMicroCode/catkin_ws/src/spotMicro/assets/tf2_coord_system.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..f1d9ff51f64de9a2a69ecbd76b5f802bb381ebc4
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/tf2_coord_system.jpg differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/walking_and_slam.gif b/spotMicroCode/catkin_ws/src/spotMicro/assets/walking_and_slam.gif
new file mode 100644
index 0000000000000000000000000000000000000000..999d2939bd587cd916a7c7584fe305724c9d70e5
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/assets/walking_and_slam.gif differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/additional_hardware_description.md b/spotMicroCode/catkin_ws/src/spotMicro/docs/additional_hardware_description.md
new file mode 100644
index 0000000000000000000000000000000000000000..e67ec7f12ad31079fb6d08a905d9084f5507cbc6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/docs/additional_hardware_description.md
@@ -0,0 +1,51 @@
+# Additional Hardware Description
+This document provides some addition description of the harware for the spot micro robot.
+
+* [Custom 3d Printed Parts](#custom-3d-printed-parts)
+* [Coordinate Frames](#coordinate-frames)
+* [Sample Hardware Install Photos](#sample-hardware-installation-photos)
+
+
+## Custom 3d Printed Parts
+
+Several custom 3d printed pieces were created to expand on KDY's original design. These include a custom shoulder assembly to provide additional reinforcement, and several mounting platforms for convenience.
+
+The modified shoulder assembly, shown below, includes an additional plastic piece to provide additional reinforcement to the shoulder axis. It requires printing an antire set of shoulder joint parts, which can be found [at this thingverse page](https://www.thingiverse.com/thing:4591999). Two sets must be printed mirrored for the opposite side's legs. The modified shoulder will require an additional 8x M3x10 screws, 8x M3 nuts, and 4x F625zz bearings in total for assembly.
+
+
+
+
+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.
+
+
+
+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.
+
+
+
+
+
+## Coordinate Frames
+There are many coordinate frames on the spot micro frame (one at each joint!), but some of the more important frames are described here.
+
+
+#### Kinematics Coordinate Frame
+With regard to kinematics of the robot frame, the coordinate frame is oriented as follows: X positive forward, Y positive up, Z positive left. This frame was is only relevant if working on the kinematic calculations for the robot. It is the same coordinate frame as used in the paper sourced for the inverse kinematic calculations for this project ("Inverse Kinematic Analysis Of A Quadruped Robot").
+
+
+
+#### TF2 Coordinate Frame
+The TF2 coordinate frames is the base robot body coordinate frame used for all transforms published to TF2 within the ROS framework. This is the robot coordinate frame of interest with regard to mapping and navigation. This frame is oriented as follows: X positive forward, Y positive left, Z positive up.
+
+
+
+
+## Sample Hardware Install Photos
+The following photos a sample installation of components on the spot micro frame.
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/joystick_control.md b/spotMicroCode/catkin_ws/src/spotMicro/docs/joystick_control.md
new file mode 100644
index 0000000000000000000000000000000000000000..7697a6247e28a9934e0e90e10ee20f4635ed036d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/docs/joystick_control.md
@@ -0,0 +1,44 @@
+# Joystick control
+With the spot_micro_joy node it is possible to control the robot by a joystick. The default configuration is set up for
+an PS4 controller which kan be connected by bluetooth directly. If you want to use another controler you need at
+least 4 axes and 4 buttons.
+
+To test if you joystick is working you can use the jstest command line this:
+```
+ubuntu@spotmicro:~$ jstest /dev/input/js0
+Driver version is 2.1.0.
+Joystick (Wireless Controller) has 8 axes (X, Y, Rx, Ry, Z, Rz, Hat0X, Hat0Y)
+and 13 buttons (BtnX, BtnY, BtnTL, BtnTR, BtnTR2, BtnSelect, BtnStart, BtnMode, BtnThumbL, BtnThumbR, ?, ?, ?).
+Testing ... (interrupt to exit)
+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
+```
+
+To check if your controller is woking with ROS, mapped and calibrated correctly you might try to start the joy ROS Node by hand.
+If you need to calibrate your joystick, i recommend jstest-gtk. To persist you configuration run jscal-store.
+
+```
+ubuntu@spotmicro:~$ rosrun joy joy_node
+[ INFO] [1615052346.145750548]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
+```
+In another teminal take a look into the messages your joy_node emits
+```
+ubuntu@spotmicro:~$ rostopic echo joy
+header:
+ seq: 1
+ stamp:
+ secs: 1615052430
+ nsecs: 954517162
+ frame_id: ''
+axes: [0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+buttons: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+---
+```
+All axes should send values between -1 and 1. Buttons are 1 or 0.
+
+Now you can check (and modify if needed) the mappings in ```spotMicroJoystickMove.py```
+
+Once done, bring your robot in a save position and start ```roslaunch spot_micro_joy everything.launch``` which will
+start everything needed to operate the robot by joystick.
+
+It is a good idea to start with a low value for transit_angle_rl. Once everything is working fine you might increase it
+to get a more agile and responsive doggy :)
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration.md b/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration.md
new file mode 100644
index 0000000000000000000000000000000000000000..20e32921cb4db8c448def9aa093800e196bf029e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration.md
@@ -0,0 +1,141 @@
+# Servo Calibration Guide
+
+This document provides a comprehensive guide to install and calibrate servos on a spot micro frame, and create a cooresponding servo configuration the ROS motion command node requires. A spreadhseet named `servo_calibration_spreadsheet.ods` is included in this repo in the `docs` directory as an aid for calculating the servo configuration values.
+
+
+The servo configuration dictionary is contained within the configuration file `spot_micro_motion_cmd.yaml` and holds servo configuration values as shown below:
+```yaml
+num_servos: 12
+servo_max_angle_deg: 82.5
+RF_3: {num: 1, center: 306, range: 385, direction: 1, center_angle_deg: 84.0}
+RF_2: {num: 2, center: 306, range: 385, direction: 1, center_angle_deg: -27.9}
+RF_1: {num: 3, center: 306, range: 396, direction: -1, center_angle_deg: -5.4}
+RB_3: {num: 4, center: 306, range: 394, direction: 1, center_angle_deg: 90.4}
+.
+.
+.
+```
+
+Servo's are defined by a abbreviation and number referring to their location and position within a leg. "RF" cooresponds to right front, "RB" to right back, similarly "LF" and "LB" to the left front and left back legs. A number 1 cooresponds to link 1 (the hip joint), 2 to link 2 (the upper leg, or shoulder joint), and 3 to link 3 (the lower leg or knee joint).
+
+### Description of Servo Configuration Values
+* **num_servos**: Fixed at 12 for this spot micro platform
+
+* **servo_max_angle_deg**: Maximum permissible command angle in a single direction for every servo. Robotic servos usually have a total movement range of 180 deg (i.e. +/- 90 deg in each direction from center). The maximum value for this limit is usually 90 deg, however it is useful to slightly constrain it to account for errors in servo center position, potential performance loss near the edges of a servo's travel, or to avoid mechanical limits or restrictions. A value of 82.5 deg works for my robot.
+
+* **num:** Port on the PCA9685 board (numbered 1-16) which that servo is connected to
+
+* **center:** The "raw" pwm value the PCA9685 node recieves representing the servo center position. A servo's position is generally commanded by a 1 to 2 ms pulse in a 50 hz cycle (20 ms time period). The PCA9685 board and cooresponding ros node controls the length of this pulse via a 12 bit pwm value. Assuming a 20ms total cycle length, 0 means no pulse, 2^12=4096 means a constant high signal, and 2048 for example would mean a pulse length of 10ms. A servo's center position is usually positioned with a 1.5 ms pulse which cooresponds to a pwm value of about 307. This value can be fine tuned by moving a sample servo with a horn on it via the servo keyboard move node and finding the center point from where the servo moves equally in opposite directions (say +/- 90 deg equally from the center. The value of 307 is a good starting point, and the value should be the same for all the servos in a set of the same kind.
+
+* **range:** The "raw" pwm value that cooresponds to the max end to end movement range for a servo. This will be related to the servo_max_angle_deg defined in the config file. As an example, the servo range value would be the raw pwm value for a servo position at +80 deg minus the raw pwm value for a servo position at -80 deg . The spreadsheet in this repo calculates this range value.
+
+* **direction:** A value of 1 or -1. Reverses the direction or servo rotation if needed, dependent on the coordinate axes of a specific leg joint. Computed in the servo calibration spreadsheet.
+
+* **center_angle_deg:** The value in degrees, in a leg joint's specific coordinate system, that cooresponds to a servo's position when at it's "center" pwm value. Value calculated in the servo calibration spreadsheet.
+
+
+### Servo Installation
+Servos must be connected in the following order to the PCA 9685 control board:
+1. Right front knee
+2. Right front shoulder
+3. Right front hip
+4. Right back knee
+5. Right back shoulder
+6. Right back hip
+7. Left back knee
+8. Left back shoulder
+9. Left back hip
+10. Left front knee
+11. Left front shoulder
+12. left front hip
+
+It is reccomended to install servos in the spot micro frame when they are powered and commanded to their center position. The joint which a servo is installed in should be approximately positioned at it's "nuetral" stance, the position about which most leg motion will occur. This ensures maximum servo travel will be available around the typical joint command angles. The two figures below roughly depict the joint orientations for which servo's should be installed when at their center position.
+
+
+
+
+
+### Commanding Individual Servos for Calibration
+Individual servo's can be commanded via the servo_move_keyboard ROS node for the calibration procedure. After one or more servos are connected to the PCA9685, the steps for commanding a single servo are as follows:
+
+1. On the raspberry pi, start the i2cpwm_board node:
+`rosrun i2cpwm_board i2cpwm_board`
+2. On another terminal on the raspberry pi, or on a linux machine with ROS installed and capabile of communicating with the raspberry pi, run the servo_move_keyboard ros node:
+`rosrun servo_move_keyboard servoMoveKeyboard.py`
+3. After the descriptive prompt appears, type `oneServo` to enter the one servo command node.
+4. Select the servo to command by entering the integer number cooresponding to the PCA9685 port to command, for example, `2`. After a servo is selected, all other servos are commanded to idle (or freewheel) such that they can be moved by hand. The following prompt will appear:
+
+
+
+5. Use the key `y` to command the default center value of the selected servo (pwm = 306). Use the keys `g` and `j` to decrease or increase the servo pwm command value by 1, respectively. This moves the servo in fine increments. The current pwm value is printed in the terminal. Use the `f` and `k` keys to move the servo in coarser increments.
+ * Key's `z` and `x` can be used to quickly command the servo to it's min and max values, respectively (by default 83 and 520, but these can be updated per servo by the keys specified in the instrucitonal prompt). Be careful commanding min/max, and ensure your servos can physically move to those positions and won't get stuck.
+6. After commanding a servo to desired calibration positions and noting down values in the calibration spreadsheet, exit the one servo control mode by pressing `q`
+7. Go back to step 3 to repeat the process for another servo.
+
+### Guide to Creating Servo Calibration Values
+Adequate kinematic performance can be achieved through calibration of links by eye, as depicted in the diagrams below. However, I also used a smartphone inclinometer app as an aid to measure angles of 45 deg for the link 1 calibration. It is reccomended to prop up the robot on a box or similar test stand so the legs can hang freely for calibration.
+
+**Leg links should be visualized as straight lines segments from reference point to reference point (e.g. axis of rotation to axis of rotation).** When positioning joints for calibration, use the wireframe representation of each link for orientation reference rather than the 3d printed part itself.
+
+The servo calibration can be built up with aid of the servo_calibration_spreadsheet, shown in the figure below. Generally the procedure is the position each link to two reference positions (angles) and note down the corresponding pwm values for those positions. I used gross angles of 0 and 90 deg for ease of placement by eye. While the slope value is not used, it is useful to keep an eye on it as all servos should have similar calculated slopes. Large discrepancies in this value could be indicative of errors.
+If you get unexpected values for servo 3 and 12 you might check if you printed a modified model with inverted hip-servos. To check this take a look from inside the body. If you see 4 ball-bearings you have the original design, if you see 2 ball-bearings and 2 server-horns the servos 3 and 12 will be inverted.
+
+
+
+#### Right Side Legs Links 2 and 3 Calibration
+
+**Consistency in the following steps is more important than accuracy.**
+
+Start with calibrating links 2 and 3 for all legs, starting with the right side of the robot. The figure below depicts the coordinate systems of the links of the right side legs positioned at 0 degrees, and defines the positive and negative angle directions.
+
+
+
+Note that link 3's angles are relative to link 2, as exemplified by the figure below:
+
+
+Starting with link 2 (ignoring the orientation of link 3), command link 2 to positions of 0 and -90 degrees, and note the cooresponding PWM values in the spreadsheet. These two positions are shown in the following two figures.
+
+
+
+
+Next, move onto link 3. For ease of visualizing reference orientation, manually position link 2 in a vertical orientation. Command link 3 to 0 and +90 degree positions and record the cooresponding pwm values. The two positions of link 3 are shown in the following two figures.
+
+
+
+
+Repeat this process for the other right side leg.
+
+#### Left Side Legs Links 2 and 3 Calibration
+Next repeat this process for the legs on the left side of the robot. **Note that** the coordinate systems for the left legs have different directions for positive and negative angles. These are depicted in the figure below for completeness.
+
+
+
+Repeat the calibration process for link 2 on the left side legs. Position link 2 to 0 and +90 degree positions, and record the PWM values in the spreadsheet. The two positions are shown in the following two figures.
+
+
+
+
+Repeat the calibration process for link 3 on the left side legs. Manually position link 2 vertically, then position link 3 at 0 and -90 degrees and record the cooresponding pwm values in the spreadsheet. The two positions are shown below.
+
+
+
+
+Repeat the process for both left side legs.
+
+#### Left and Right Legs Link 1 Calibration
+
+Finally, calibrate link 1 for the left and right legs in a similar pattern. The coordinate systems for link 1 of the left and right legs is shown in the figures below.
+
+
+
+
+Start with the right legs, and command link 1 to 0 and -45 degrees, and record the cooresponding PWM values in the spreadsheet. A reference value of 45 degrees is used instead of 90 due to mechanical limits. Repeat the process for the left legs. These positions are shown in the four figures below.
+
+
+
+
+
+
+
+
+This complets the leg servo calibration process. Take the **bold** values from the servo calibration spreadsheet and copy them to the servo configuration dictionaries in the `spot_micro_motion_cmd.yaml` config file. This file is located in your catkin worksapce in the following subfolders ```src/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml```.
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration_spreadsheet.ods b/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration_spreadsheet.ods
new file mode 100644
index 0000000000000000000000000000000000000000..2ef7eeade88d4463990647ce51d2c70df08b0e77
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration_spreadsheet.ods differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/slam_information.md b/spotMicroCode/catkin_ws/src/spotMicro/docs/slam_information.md
new file mode 100644
index 0000000000000000000000000000000000000000..71707117ae9280961174cb0d4457f73ea033774b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/docs/slam_information.md
@@ -0,0 +1,77 @@
+# SLAM Information
+This document provides additional information for running SLAM on a spot micro quadruped through this project.
+
+* [Required Setup](#required-setup)
+* [Generating a Map](#generating-a-map-frames)
+* [Saving a Map](#saving-a-map)
+* [Recording a Data Log](#recording-a-data-log)
+* [Reprocessing Scan Data Through Log Playback](#reprocessing-scan-data-through-log-playback)
+
+
+Example of robot walking and mapping an environment.
+
+## Required Setup
+Two prerequisite steps are required for SLAM:
+1. Installing additional ROS packages
+2. Elevating permissions of the USB port the lidar is connected to for it's ROS package
+
+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:
+```
+sudo apt-get install ros-kinetic-rplidar-ros
+sudo apt-get install ros-kinetic-hector-slam
+```
+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.
+
+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:
+`ls -l /dev |grep ttyUSB`
+Elevate that ports authorit with the following command. Example if the port was `USB0`:
+`sudo chmod 666 /dev/ttyUSB0`
+
+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.
+
+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.
+
+This project assumes a RPLidar A1. If another Lidar is used, then a different lidar driver ROS package will be needed.
+
+
+## Generating a Map
+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.
+
+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:
+* `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).
+* `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.
+
+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.
+
+## Saving a Map
+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:
+`rosrun map_server map_saver -f my_map_filename`
+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.
+
+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:
+`rostopic pub syscommand std_msgs/String "savegeotiff"`
+
+
+## Recording a Data Log
+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.
+
+The record a data log, make a new directory for storing logfiles:
+```
+mkdir ~/bagfiles
+cd ~/bagfiles
+```
+And record all rostopics with the following command:
+`rosbag record -a`
+
+Note that log files will be on the order of 1 GB in size per around 10 minutes of operation.
+
+## Reprocessing Scan Data Through Log Playback
+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.
+
+As an example, the steps to playback a recorded dataset through hector slam are:
+1. Playback a log using a command such as `rosbag play <your bagfile>`. Additional command line arguments, such as `--topics <topic1_to_play> <topic2_to_play> ...` can be added to specify which topics to playback only.
+2. Start the hector slam node and rviz to visualize the mapping process. This can be done through the following two launch commands:
+`roslaunch spot_micro_rviz slam.launch`
+`roslaunch spot_micro_launch motion_control_and_hector_slam.launch run_post_proc:=true`
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..13d19266ef02f8cfd39f8886aeb6b3148491534e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(lcd_monitor)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ geometry_msgs
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES lcd_monitor
+# CATKIN_DEPENDS rospy
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/lcd_monitor.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/lcd_monitor_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_lcd_monitor.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/launch/lcd_monitor.launch b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/launch/lcd_monitor.launch
new file mode 100644
index 0000000000000000000000000000000000000000..148234d0d7b54833bd3a0cc8ac84d05371d2b271
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/launch/lcd_monitor.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Launch plotting node standalone -->
+
+<launch>
+ <!-- Defining the node and executable and publishing the output on terminal-->
+ <node name="lcd_monitor_node" pkg="lcd_monitor" type="sm_lcd_node.py" output="screen">
+ </node>
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d1a5bd1c294487629c13bee6b7cbe11c378317f2
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>lcd_monitor</name>
+ <version>0.0.0</version>
+ <description>The lcd_monitor package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/lcd_monitor</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/scripts/sm_lcd_node.py b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/scripts/sm_lcd_node.py
new file mode 100755
index 0000000000000000000000000000000000000000..6430ac5ceda82e1cd3b9b7f96032f95998fa7f89
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/scripts/sm_lcd_node.py
@@ -0,0 +1,7 @@
+#! /usr/bin/env python
+
+from lcd_monitor.sm_lcd_driver import main
+
+# Node program, calls main function in spot_micro_simple_command python package
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/setup.py b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..fb52c30da038025f8b4ac949c513622648647544
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/setup.py
@@ -0,0 +1,11 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+ packages=['lcd_monitor'],
+ package_dir={'': 'src'})
+
+setup(**setup_args)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py
new file mode 100644
index 0000000000000000000000000000000000000000..56fd3bbb19199d0acb61e1bdbaf6e4231c0b25f6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py
@@ -0,0 +1,181 @@
+#!/usr/bin/python
+
+# -*- coding: utf-8 -*-
+# Original code found at:
+# https://gist.github.com/DenisFromHR/cc863375a6e19dce359d
+
+"""
+Compiled, mashed and generally mutilated 2014-2015 by Denis Pleic
+Made available under GNU GENERAL PUBLIC LICENSE
+
+# Modified Python I2C library for Raspberry Pi
+# as found on http://www.recantha.co.uk/blog/?p=4849
+# Joined existing 'i2c_lib.py' and 'lcddriver.py' into a single library
+# added bits and pieces from various sources
+# By DenisFromHR (Denis Pleic)
+# 2015-02-10, ver 0.1
+
+"""
+
+# i2c bus (0 -- original Pi, 1 -- Rev 2 Pi)
+I2CBUS = 1
+
+# LCD Address
+ADDRESS = 0x27
+
+import smbus
+from time import sleep
+
+class i2c_device:
+ def __init__(self, addr, port=I2CBUS):
+ self.addr = addr
+ self.bus = smbus.SMBus(port)
+
+# Write a single command
+ def write_cmd(self, cmd):
+ self.bus.write_byte(self.addr, cmd)
+ sleep(0.0001)
+
+# Write a command and argument
+ def write_cmd_arg(self, cmd, data):
+ self.bus.write_byte_data(self.addr, cmd, data)
+ sleep(0.0001)
+
+# Write a block of data
+ def write_block_data(self, cmd, data):
+ self.bus.write_block_data(self.addr, cmd, data)
+ sleep(0.0001)
+
+# Read a single byte
+ def read(self):
+ return self.bus.read_byte(self.addr)
+
+# Read
+ def read_data(self, cmd):
+ return self.bus.read_byte_data(self.addr, cmd)
+
+# Read a block of data
+ def read_block_data(self, cmd):
+ return self.bus.read_block_data(self.addr, cmd)
+
+
+# commands
+LCD_CLEARDISPLAY = 0x01
+LCD_RETURNHOME = 0x02
+LCD_ENTRYMODESET = 0x04
+LCD_DISPLAYCONTROL = 0x08
+LCD_CURSORSHIFT = 0x10
+LCD_FUNCTIONSET = 0x20
+LCD_SETCGRAMADDR = 0x40
+LCD_SETDDRAMADDR = 0x80
+
+# flags for display entry mode
+LCD_ENTRYRIGHT = 0x00
+LCD_ENTRYLEFT = 0x02
+LCD_ENTRYSHIFTINCREMENT = 0x01
+LCD_ENTRYSHIFTDECREMENT = 0x00
+
+# flags for display on/off control
+LCD_DISPLAYON = 0x04
+LCD_DISPLAYOFF = 0x00
+LCD_CURSORON = 0x02
+LCD_CURSOROFF = 0x00
+LCD_BLINKON = 0x01
+LCD_BLINKOFF = 0x00
+
+# flags for display/cursor shift
+LCD_DISPLAYMOVE = 0x08
+LCD_CURSORMOVE = 0x00
+LCD_MOVERIGHT = 0x04
+LCD_MOVELEFT = 0x00
+
+# flags for function set
+LCD_8BITMODE = 0x10
+LCD_4BITMODE = 0x00
+LCD_2LINE = 0x08
+LCD_1LINE = 0x00
+LCD_5x10DOTS = 0x04
+LCD_5x8DOTS = 0x00
+
+# flags for backlight control
+LCD_BACKLIGHT = 0x08
+LCD_NOBACKLIGHT = 0x00
+
+En = 0b00000100 # Enable bit
+Rw = 0b00000010 # Read/Write bit
+Rs = 0b00000001 # Register select bit
+
+class lcd:
+ #initializes objects and lcd
+ def __init__(self):
+ self.lcd_device = i2c_device(ADDRESS)
+
+ self.lcd_write(0x03)
+ self.lcd_write(0x03)
+ self.lcd_write(0x03)
+ self.lcd_write(0x02)
+
+ self.lcd_write(LCD_FUNCTIONSET | LCD_2LINE | LCD_5x8DOTS | LCD_4BITMODE)
+ self.lcd_write(LCD_DISPLAYCONTROL | LCD_DISPLAYON)
+ self.lcd_write(LCD_CLEARDISPLAY)
+ self.lcd_write(LCD_ENTRYMODESET | LCD_ENTRYLEFT)
+ sleep(0.2)
+
+
+ # clocks EN to latch command
+ def lcd_strobe(self, data):
+ self.lcd_device.write_cmd(data | En | LCD_BACKLIGHT)
+ sleep(.0005)
+ self.lcd_device.write_cmd(((data & ~En) | LCD_BACKLIGHT))
+ sleep(.0001)
+
+ def lcd_write_four_bits(self, data):
+ self.lcd_device.write_cmd(data | LCD_BACKLIGHT)
+ self.lcd_strobe(data)
+
+ # write a command to lcd
+ def lcd_write(self, cmd, mode=0):
+ self.lcd_write_four_bits(mode | (cmd & 0xF0))
+ self.lcd_write_four_bits(mode | ((cmd << 4) & 0xF0))
+
+ # write a character to lcd (or character rom) 0x09: backlight | RS=DR<
+ # works!
+ def lcd_write_char(self, charvalue, mode=1):
+ self.lcd_write_four_bits(mode | (charvalue & 0xF0))
+ self.lcd_write_four_bits(mode | ((charvalue << 4) & 0xF0))
+
+ # put string function with optional char positioning
+ def lcd_display_string(self, string, line=1, pos=0):
+ if line == 1:
+ pos_new = pos
+ elif line == 2:
+ pos_new = 0x40 + pos
+ elif line == 3:
+ pos_new = 0x14 + pos
+ elif line == 4:
+ pos_new = 0x54 + pos
+
+ self.lcd_write(0x80 + pos_new)
+
+ for char in string:
+ self.lcd_write(ord(char), Rs)
+
+ # clear lcd and set to home
+ def lcd_clear(self):
+ self.lcd_write(LCD_CLEARDISPLAY)
+ self.lcd_write(LCD_RETURNHOME)
+
+ # define backlight on/off (lcd.backlight(1); off= lcd.backlight(0)
+ def backlight(self, state): # for state, 1 = on, 0 = off
+ if state == 1:
+ self.lcd_device.write_cmd(LCD_BACKLIGHT)
+ elif state == 0:
+ self.lcd_device.write_cmd(LCD_NOBACKLIGHT)
+
+ # add custom characters (0 - 7)
+ def lcd_load_custom_chars(self, fontdata):
+ self.lcd_write(0x40)
+ for char in fontdata:
+ for line in char:
+ self.lcd_write_char(line)
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py
new file mode 100644
index 0000000000000000000000000000000000000000..88cf6c45c82f5e09c982e0313033488b6a8d8631
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py
@@ -0,0 +1,83 @@
+
+from lcd_monitor import I2C_LCD_driver
+import datetime
+import time
+import rospy
+from math import pi
+from geometry_msgs.msg import Vector3
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+
+class SpotMicroLcd():
+ ''' Class to encapsulate lcd driver for spot micro robot '''
+
+ def __init__(self):
+ '''Constructor'''
+ self._mylcd = I2C_LCD_driver.lcd()
+
+ self._state_str = 'None'
+ self._padded_state_str = 'None'
+
+ self._fwd_speed_cmd = 0.0
+ self._side_speed_cmd = 0.0
+ self._yaw_rate_cmd = 0.0
+
+ self._phi_cmd = 0.0
+ self._theta_cmd = 0.0
+ self._psi_cmd = 0.0
+
+ rospy.init_node('lcd_monitor_node')
+
+ rospy.Subscriber('lcd_vel_cmd',Twist,self.update_speed_cmd)
+ rospy.Subscriber('lcd_angle_cmd',Vector3,self.update_angle_cmd)
+ rospy.Subscriber('lcd_state',String,self.update_state_string)
+
+ def update_speed_cmd(self, msg):
+ ''' Updates speed command attributes'''
+ self._fwd_speed_cmd = msg.linear.x*100.0
+ self._side_speed_cmd = msg.linear.y*100.0
+ self._yaw_rate_cmd = msg.angular.z*180.0/pi
+
+ def update_angle_cmd(self, msg):
+ ''' Updates angle command attributes'''
+ self._phi_cmd = msg.x * 180.0/pi
+ self._theta_cmd = msg.y * 180.0/pi
+ self._psi_cmd = msg.z * 180.0/pi
+
+ def update_state_string(self, msg):
+ ''' Updates angle command attributes'''
+ self._state_str = msg.data
+
+ if self._state_str == "Transit Stand":
+ self._padded_state_str = "To Stand"
+ elif self._state_str == "Transit Idle":
+ self._padded_state_str = "To Idle"
+ else:
+ self._padded_state_str = self._state_str
+
+ self._padded_state_str = self._padded_state_str.ljust(16,' ')
+
+ def run(self):
+ ''' Runs the lcd driver and prints data'''
+
+ # Define the loop rate in Hz
+ rate = rospy.Rate(3)
+
+ while not rospy.is_shutdown():
+
+ self._mylcd.lcd_display_string('State: %s'%(self._padded_state_str),1)
+
+
+ if self._state_str == "Stand":
+ self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._phi_cmd, self._theta_cmd, self._psi_cmd),2)
+ elif self._state_str == "Walk":
+ 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)
+ else:
+ self._mylcd.lcd_display_string(' ',2)
+ # Sleep till next loop
+ rate.sleep()
+
+
+def main():
+ sm_lcd_obj = SpotMicroLcd()
+ sm_lcd_obj.run()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/.gitlab-ci.yml b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..af7f5c738af27ce10cc787a7a6090ce240841374
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/.gitlab-ci.yml
@@ -0,0 +1,11 @@
+pages:
+ stage: deploy
+ script:
+ - mkdir .public
+ - cp -r doc/html/* .public/
+ - mv .public public
+ artifacts:
+ paths:
+ - public
+ only:
+ - master
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..19b76024f8c0cedd22935acb18caf8c7a8d96865
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(i2cpwm_board)
+
+find_package(catkin REQUIRED COMPONENTS roscpp std_msgs message_generation)
+
+
+add_message_files(DIRECTORY msg FILES Servo.msg ServoArray.msg ServoConfig.msg ServoConfigArray.msg Position.msg PositionArray.msg)
+
+add_service_files(DIRECTORY srv FILES IntValue.srv ServosConfig.srv DriveMode.srv StopServos.srv)
+
+generate_messages(DEPENDENCIES std_msgs)
+
+
+catkin_package(CATKIN_DEPENDS roscpp std_msgs message_runtime)
+
+
+include_directories(include ${catkin_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+
+add_executable(i2cpwm_board src/i2cpwm_controller.cpp)
+target_link_libraries(i2cpwm_board ${catkin_LIBRARIES} i2c)
+add_dependencies(i2cpwm_board i2cpwm_board_generate_messages_cpp)
+
+install(TARGETS i2cpwm_board
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" )
+install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch )
+
+execute_process(COMMAND doxygen doc/Doxyfile)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/README.md b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..5ae652880f7e5afdb2af0b475c99e30267ea625d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/README.md
@@ -0,0 +1,10 @@
+ros-i2cpwmboard is the project for the i2cpwm_board controller node.
+
+The user documentation is available at: http://bradanlane.gitlab.io/ros-i2cpwmboard/
+
+Integrated demo video: https://vimeo.com/193201509
+LoCoRo Robot demo video: https://vimeo.com/189997166
+
+Status: parameter initialization added.
+
+The LoCoRo (low cost robot) project configuration and launch files have been moved to a separate poroject: https://gitlab.com/bradanlane/locoro
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/Doxyfile b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/Doxyfile
new file mode 100644
index 0000000000000000000000000000000000000000..fb8bd6bbe7094167c8b4b4d7a671d9e395279f8b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/Doxyfile
@@ -0,0 +1,2481 @@
+# Doxyfile 1.8.11
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = i2cpwm_board
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = 0.5.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface"
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = doc
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF = "The $name class" \
+ "The $name widget" \
+ "The $name file" \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = YES
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = YES
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = NO
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = YES
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = YES
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = YES
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = YES
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = NO
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = NO
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 64
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = YES
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = src
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
+# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
+
+FILE_PATTERNS = *.c \
+ *.cc \
+ *.cxx \
+ *.cpp \
+ *.c++ \
+ *.java \
+ *.ii \
+ *.ixx \
+ *.ipp \
+ *.i++ \
+ *.inl \
+ *.idl \
+ *.ddl \
+ *.odl \
+ *.h \
+ *.hh \
+ *.hxx \
+ *.hpp \
+ *.h++ \
+ *.cs \
+ *.d \
+ *.php \
+ *.php4 \
+ *.php5 \
+ *.phtml \
+ *.inc \
+ *.m \
+ *.markdown \
+ *.md \
+ *.mm \
+ *.dox \
+ *.py \
+ *.pyw \
+ *.f90 \
+ *.f \
+ *.for \
+ *.tcl \
+ *.vhd \
+ *.vhdl \
+ *.ucf \
+ *.qsf \
+ *.as \
+ *.js
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = NO
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS =
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH = msg \
+ srv \
+ /opt/ros/kinetic/share/geometry_msgs/msg
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = YES
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = NO
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER = doc/header.html
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER = doc/footer.html
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET = doc/doxygen.css
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET = doc/styles.css
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = YES
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = YES
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = NO
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = letter
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = YES
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = YES
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = NO
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/doxygen.css b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/doxygen.css
new file mode 100644
index 0000000000000000000000000000000000000000..1425ec530d3181ed8c9651987cfa9ae72df9b339
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/doxygen.css
@@ -0,0 +1,1475 @@
+/* The standard CSS for doxygen 1.8.11 */
+
+body, table, div, p, dl {
+ font: 400 14px/22px Roboto,sans-serif;
+}
+
+/* @group Heading Levels */
+
+h1.groupheader {
+ font-size: 150%;
+}
+
+.title {
+ font: 400 14px/28px Roboto,sans-serif;
+ font-size: 150%;
+ font-weight: bold;
+ margin: 10px 2px;
+}
+
+h2.groupheader {
+ border-bottom: 1px solid #879ECB;
+ color: #354C7B;
+ font-size: 150%;
+ font-weight: normal;
+ margin-top: 1.75em;
+ padding-top: 8px;
+ padding-bottom: 4px;
+ width: 100%;
+}
+
+h3.groupheader {
+ font-size: 100%;
+}
+
+h1, h2, h3, h4, h5, h6 {
+ -webkit-transition: text-shadow 0.5s linear;
+ -moz-transition: text-shadow 0.5s linear;
+ -ms-transition: text-shadow 0.5s linear;
+ -o-transition: text-shadow 0.5s linear;
+ transition: text-shadow 0.5s linear;
+ margin-right: 15px;
+}
+
+h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow {
+ text-shadow: 0 0 15px cyan;
+}
+
+dt {
+ font-weight: bold;
+}
+
+div.multicol {
+ -moz-column-gap: 1em;
+ -webkit-column-gap: 1em;
+ -moz-column-count: 3;
+ -webkit-column-count: 3;
+}
+
+p.startli, p.startdd {
+ margin-top: 2px;
+}
+
+p.starttd {
+ margin-top: 0px;
+}
+
+p.endli {
+ margin-bottom: 0px;
+}
+
+p.enddd {
+ margin-bottom: 4px;
+}
+
+p.endtd {
+ margin-bottom: 2px;
+}
+
+/* @end */
+
+caption {
+ font-weight: bold;
+}
+
+span.legend {
+ font-size: 70%;
+ text-align: center;
+}
+
+h3.version {
+ font-size: 90%;
+ text-align: center;
+}
+
+div.qindex, div.navtab{
+ background-color: #EBEFF6;
+ border: 1px solid #A3B4D7;
+ text-align: center;
+}
+
+div.qindex, div.navpath {
+ width: 100%;
+ line-height: 140%;
+}
+
+div.navtab {
+ margin-right: 15px;
+}
+
+/* @group Link Styling */
+
+a {
+ color: #3D578C;
+ font-weight: normal;
+ text-decoration: none;
+}
+
+.contents a:visited {
+ color: #4665A2;
+}
+
+a:hover {
+ text-decoration: underline;
+}
+
+a.qindex {
+ font-weight: bold;
+}
+
+a.qindexHL {
+ font-weight: bold;
+ background-color: #9CAFD4;
+ color: #ffffff;
+ border: 1px double #869DCA;
+}
+
+.contents a.qindexHL:visited {
+ color: #ffffff;
+}
+
+a.el {
+ font-weight: bold;
+}
+
+a.elRef {
+}
+
+a.code, a.code:visited, a.line, a.line:visited {
+ color: #4665A2;
+}
+
+a.codeRef, a.codeRef:visited, a.lineRef, a.lineRef:visited {
+ color: #4665A2;
+}
+
+/* @end */
+
+dl.el {
+ margin-left: -1cm;
+}
+
+pre.fragment {
+ border: 1px solid #C4CFE5;
+ background-color: #FBFCFD;
+ padding: 4px 6px;
+ margin: 4px 8px 4px 2px;
+ overflow: auto;
+ word-wrap: break-word;
+ font-size: 9pt;
+ line-height: 125%;
+ font-family: monospace, fixed;
+ font-size: 105%;
+}
+
+div.fragment {
+ padding: 4px 6px;
+ margin: 4px 8px 4px 2px;
+ background-color: #FBFCFD;
+ border: 1px solid #C4CFE5;
+}
+
+div.line {
+ font-family: monospace, fixed;
+ font-size: 13px;
+ min-height: 13px;
+ line-height: 1.0;
+ text-wrap: unrestricted;
+ white-space: -moz-pre-wrap; /* Moz */
+ white-space: -pre-wrap; /* Opera 4-6 */
+ white-space: -o-pre-wrap; /* Opera 7 */
+ white-space: pre-wrap; /* CSS3 */
+ word-wrap: break-word; /* IE 5.5+ */
+ text-indent: -53px;
+ padding-left: 53px;
+ padding-bottom: 0px;
+ margin: 0px;
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+div.line:after {
+ content:"\000A";
+ white-space: pre;
+}
+
+div.line.glow {
+ background-color: cyan;
+ box-shadow: 0 0 10px cyan;
+}
+
+
+span.lineno {
+ padding-right: 4px;
+ text-align: right;
+ border-right: 2px solid #0F0;
+ background-color: #E8E8E8;
+ white-space: pre;
+}
+span.lineno a {
+ background-color: #D8D8D8;
+}
+
+span.lineno a:hover {
+ background-color: #C8C8C8;
+}
+
+div.ah, span.ah {
+ background-color: black;
+ font-weight: bold;
+ color: #ffffff;
+ margin-bottom: 3px;
+ margin-top: 3px;
+ padding: 0.2em;
+ border: solid thin #333;
+ border-radius: 0.5em;
+ -webkit-border-radius: .5em;
+ -moz-border-radius: .5em;
+ box-shadow: 2px 2px 3px #999;
+ -webkit-box-shadow: 2px 2px 3px #999;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444));
+ background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000 110%);
+}
+
+div.classindex ul {
+ list-style: none;
+ padding-left: 0;
+}
+
+div.classindex span.ai {
+ display: inline-block;
+}
+
+div.groupHeader {
+ margin-left: 16px;
+ margin-top: 12px;
+ font-weight: bold;
+}
+
+div.groupText {
+ margin-left: 16px;
+ font-style: italic;
+}
+
+body {
+ background-color: white;
+ color: black;
+ margin: 0;
+}
+
+div.contents {
+ margin-top: 10px;
+ margin-left: 12px;
+ margin-right: 8px;
+}
+
+td.indexkey {
+ background-color: #EBEFF6;
+ font-weight: bold;
+ border: 1px solid #C4CFE5;
+ margin: 2px 0px 2px 0;
+ padding: 2px 10px;
+ white-space: nowrap;
+ vertical-align: top;
+}
+
+td.indexvalue {
+ background-color: #EBEFF6;
+ border: 1px solid #C4CFE5;
+ padding: 2px 10px;
+ margin: 2px 0px;
+}
+
+tr.memlist {
+ background-color: #EEF1F7;
+}
+
+p.formulaDsp {
+ text-align: center;
+}
+
+img.formulaDsp {
+
+}
+
+img.formulaInl {
+ vertical-align: middle;
+}
+
+div.center {
+ text-align: center;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding: 0px;
+}
+
+div.center img {
+ border: 0px;
+}
+
+address.footer {
+ text-align: right;
+ padding-right: 12px;
+}
+
+img.footer {
+ border: 0px;
+ vertical-align: middle;
+}
+
+/* @group Code Colorization */
+
+span.keyword {
+ color: #008000
+}
+
+span.keywordtype {
+ color: #604020
+}
+
+span.keywordflow {
+ color: #e08000
+}
+
+span.comment {
+ color: #800000
+}
+
+span.preprocessor {
+ color: #806020
+}
+
+span.stringliteral {
+ color: #002080
+}
+
+span.charliteral {
+ color: #008080
+}
+
+span.vhdldigit {
+ color: #ff00ff
+}
+
+span.vhdlchar {
+ color: #000000
+}
+
+span.vhdlkeyword {
+ color: #700070
+}
+
+span.vhdllogic {
+ color: #ff0000
+}
+
+blockquote {
+ background-color: #F7F8FB;
+ border-left: 2px solid #9CAFD4;
+ margin: 0 24px 0 4px;
+ padding: 0 12px 0 16px;
+}
+
+/* @end */
+
+/*
+.search {
+ color: #003399;
+ font-weight: bold;
+}
+
+form.search {
+ margin-bottom: 0px;
+ margin-top: 0px;
+}
+
+input.search {
+ font-size: 75%;
+ color: #000080;
+ font-weight: normal;
+ background-color: #e8eef2;
+}
+*/
+
+td.tiny {
+ font-size: 75%;
+}
+
+.dirtab {
+ padding: 4px;
+ border-collapse: collapse;
+ border: 1px solid #A3B4D7;
+}
+
+th.dirtab {
+ background: #EBEFF6;
+ font-weight: bold;
+}
+
+hr {
+ height: 0px;
+ border: none;
+ border-top: 1px solid #4A6AAA;
+}
+
+hr.footer {
+ height: 1px;
+}
+
+/* @group Member Descriptions */
+
+table.memberdecls {
+ border-spacing: 0px;
+ padding: 0px;
+}
+
+.memberdecls td, .fieldtable tr {
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+.memberdecls td.glow, .fieldtable tr.glow {
+ background-color: cyan;
+ box-shadow: 0 0 15px cyan;
+}
+
+.mdescLeft, .mdescRight,
+.memItemLeft, .memItemRight,
+.memTemplItemLeft, .memTemplItemRight, .memTemplParams {
+ background-color: #F9FAFC;
+ border: none;
+ margin: 4px;
+ padding: 1px 0 0 8px;
+}
+
+.mdescLeft, .mdescRight {
+ padding: 0px 8px 4px 8px;
+ color: #555;
+}
+
+.memSeparator {
+ border-bottom: 1px solid #DEE4F0;
+ line-height: 1px;
+ margin: 0px;
+ padding: 0px;
+}
+
+.memItemLeft, .memTemplItemLeft {
+ white-space: nowrap;
+}
+
+.memItemRight {
+ width: 100%;
+}
+
+.memTemplParams {
+ color: #4665A2;
+ white-space: nowrap;
+ font-size: 80%;
+}
+
+/* @end */
+
+/* @group Member Details */
+
+/* Styles for detailed member documentation */
+
+.memtemplate {
+ font-size: 80%;
+ color: #4665A2;
+ font-weight: normal;
+ margin-left: 9px;
+}
+
+.memnav {
+ background-color: #EBEFF6;
+ border: 1px solid #A3B4D7;
+ text-align: center;
+ margin: 2px;
+ margin-right: 15px;
+ padding: 2px;
+}
+
+.mempage {
+ width: 100%;
+}
+
+.memitem {
+ padding: 0;
+ margin-bottom: 10px;
+ margin-right: 5px;
+ -webkit-transition: box-shadow 0.5s linear;
+ -moz-transition: box-shadow 0.5s linear;
+ -ms-transition: box-shadow 0.5s linear;
+ -o-transition: box-shadow 0.5s linear;
+ transition: box-shadow 0.5s linear;
+ display: table !important;
+ width: 100%;
+}
+
+.memitem.glow {
+ box-shadow: 0 0 15px cyan;
+}
+
+.memname {
+ font-weight: bold;
+ margin-left: 6px;
+}
+
+.memname td {
+ vertical-align: bottom;
+}
+
+.memproto, dl.reflist dt {
+ border-top: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 0px 6px 0px;
+ color: #253555;
+ font-weight: bold;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ background-image:url('nav_f.png');
+ background-repeat:repeat-x;
+ background-color: #E2E8F2;
+ /* opera specific markup */
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ /* firefox specific markup */
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ -moz-border-radius-topright: 4px;
+ -moz-border-radius-topleft: 4px;
+ /* webkit specific markup */
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ -webkit-border-top-right-radius: 4px;
+ -webkit-border-top-left-radius: 4px;
+
+}
+
+.memdoc, dl.reflist dd {
+ border-bottom: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 10px 2px 10px;
+ background-color: #FBFCFD;
+ border-top-width: 0;
+ background-image:url('nav_g.png');
+ background-repeat:repeat-x;
+ background-color: #FFFFFF;
+ /* opera specific markup */
+ border-bottom-left-radius: 4px;
+ border-bottom-right-radius: 4px;
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ /* firefox specific markup */
+ -moz-border-radius-bottomleft: 4px;
+ -moz-border-radius-bottomright: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ /* webkit specific markup */
+ -webkit-border-bottom-left-radius: 4px;
+ -webkit-border-bottom-right-radius: 4px;
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+}
+
+dl.reflist dt {
+ padding: 5px;
+}
+
+dl.reflist dd {
+ margin: 0px 0px 10px 0px;
+ padding: 5px;
+}
+
+.paramkey {
+ text-align: right;
+}
+
+.paramtype {
+ white-space: nowrap;
+}
+
+.paramname {
+ color: #602020;
+ white-space: nowrap;
+}
+.paramname em {
+ font-style: normal;
+}
+.paramname code {
+ line-height: 14px;
+}
+
+.params, .retval, .exception, .tparams {
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+.params .paramname, .retval .paramname {
+ font-weight: bold;
+ vertical-align: top;
+}
+
+.params .paramtype {
+ font-style: italic;
+ vertical-align: top;
+}
+
+.params .paramdir {
+ font-family: "courier new",courier,monospace;
+ vertical-align: top;
+}
+
+table.mlabels {
+ border-spacing: 0px;
+}
+
+td.mlabels-left {
+ width: 100%;
+ padding: 0px;
+}
+
+td.mlabels-right {
+ vertical-align: bottom;
+ padding: 0px;
+ white-space: nowrap;
+}
+
+span.mlabels {
+ margin-left: 8px;
+}
+
+span.mlabel {
+ background-color: #728DC1;
+ border-top:1px solid #5373B4;
+ border-left:1px solid #5373B4;
+ border-right:1px solid #C4CFE5;
+ border-bottom:1px solid #C4CFE5;
+ text-shadow: none;
+ color: white;
+ margin-right: 4px;
+ padding: 2px 3px;
+ border-radius: 3px;
+ font-size: 7pt;
+ white-space: nowrap;
+ vertical-align: middle;
+}
+
+
+
+/* @end */
+
+/* these are for tree view inside a (index) page */
+
+div.directory {
+ margin: 10px 0px;
+ border-top: 1px solid #9CAFD4;
+ border-bottom: 1px solid #9CAFD4;
+ width: 100%;
+}
+
+.directory table {
+ border-collapse:collapse;
+}
+
+.directory td {
+ margin: 0px;
+ padding: 0px;
+ vertical-align: top;
+}
+
+.directory td.entry {
+ white-space: nowrap;
+ padding-right: 6px;
+ padding-top: 3px;
+}
+
+.directory td.entry a {
+ outline:none;
+}
+
+.directory td.entry a img {
+ border: none;
+}
+
+.directory td.desc {
+ width: 100%;
+ padding-left: 6px;
+ padding-right: 6px;
+ padding-top: 3px;
+ border-left: 1px solid rgba(0,0,0,0.05);
+}
+
+.directory tr.even {
+ padding-left: 6px;
+ background-color: #F7F8FB;
+}
+
+.directory img {
+ vertical-align: -30%;
+}
+
+.directory .levels {
+ white-space: nowrap;
+ width: 100%;
+ text-align: right;
+ font-size: 9pt;
+}
+
+.directory .levels span {
+ cursor: pointer;
+ padding-left: 2px;
+ padding-right: 2px;
+ color: #3D578C;
+}
+
+.arrow {
+ color: #9CAFD4;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+ cursor: pointer;
+ font-size: 80%;
+ display: inline-block;
+ width: 16px;
+ height: 22px;
+}
+
+.icon {
+ font-family: Arial, Helvetica;
+ font-weight: bold;
+ font-size: 12px;
+ height: 14px;
+ width: 16px;
+ display: inline-block;
+ background-color: #728DC1;
+ color: white;
+ text-align: center;
+ border-radius: 4px;
+ margin-left: 2px;
+ margin-right: 2px;
+}
+
+.icona {
+ width: 24px;
+ height: 22px;
+ display: inline-block;
+}
+
+.iconfopen {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderopen.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.iconfclosed {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderclosed.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.icondoc {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('doc.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+table.directory {
+ font: 400 14px Roboto,sans-serif;
+}
+
+/* @end */
+
+div.dynheader {
+ margin-top: 8px;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+address {
+ font-style: normal;
+ color: #2A3D61;
+}
+
+table.doxtable caption {
+ caption-side: top;
+}
+
+table.doxtable {
+ border-collapse:collapse;
+ margin-top: 4px;
+ margin-bottom: 4px;
+}
+
+table.doxtable td, table.doxtable th {
+ border: 1px solid #2D4068;
+ padding: 3px 7px 2px;
+}
+
+table.doxtable th {
+ background-color: #374F7F;
+ color: #FFFFFF;
+ font-size: 110%;
+ padding-bottom: 4px;
+ padding-top: 5px;
+}
+
+table.fieldtable {
+ /*width: 100%;*/
+ margin-bottom: 10px;
+ border: 1px solid #A8B8D9;
+ border-spacing: 0px;
+ -moz-border-radius: 4px;
+ -webkit-border-radius: 4px;
+ border-radius: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+ box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+}
+
+.fieldtable td, .fieldtable th {
+ padding: 3px 7px 2px;
+}
+
+.fieldtable td.fieldtype, .fieldtable td.fieldname {
+ white-space: nowrap;
+ border-right: 1px solid #A8B8D9;
+ border-bottom: 1px solid #A8B8D9;
+ vertical-align: top;
+}
+
+.fieldtable td.fieldname {
+ padding-top: 3px;
+}
+
+.fieldtable td.fielddoc {
+ border-bottom: 1px solid #A8B8D9;
+ /*width: 100%;*/
+}
+
+.fieldtable td.fielddoc p:first-child {
+ margin-top: 0px;
+}
+
+.fieldtable td.fielddoc p:last-child {
+ margin-bottom: 2px;
+}
+
+.fieldtable tr:last-child td {
+ border-bottom: none;
+}
+
+.fieldtable th {
+ background-image:url('nav_f.png');
+ background-repeat:repeat-x;
+ background-color: #E2E8F2;
+ font-size: 90%;
+ color: #253555;
+ padding-bottom: 4px;
+ padding-top: 5px;
+ text-align:left;
+ -moz-border-radius-topleft: 4px;
+ -moz-border-radius-topright: 4px;
+ -webkit-border-top-left-radius: 4px;
+ -webkit-border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ border-top-right-radius: 4px;
+ border-bottom: 1px solid #A8B8D9;
+}
+
+
+.tabsearch {
+ top: 0px;
+ left: 10px;
+ height: 36px;
+ background-image: url('tab_b.png');
+ z-index: 101;
+ overflow: hidden;
+ font-size: 13px;
+}
+
+.navpath ul
+{
+ font-size: 11px;
+ background-image:url('tab_b.png');
+ background-repeat:repeat-x;
+ background-position: 0 -5px;
+ height:30px;
+ line-height:30px;
+ color:#8AA0CC;
+ border:solid 1px #C2CDE4;
+ overflow:hidden;
+ margin:0px;
+ padding:0px;
+}
+
+.navpath li
+{
+ list-style-type:none;
+ float:left;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:url('bc_s.png');
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+}
+
+.navpath li.navelem a
+{
+ height:32px;
+ display:block;
+ text-decoration: none;
+ outline: none;
+ color: #283A5D;
+ font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ text-decoration: none;
+}
+
+.navpath li.navelem a:hover
+{
+ color:#6884BD;
+}
+
+.navpath li.footer
+{
+ list-style-type:none;
+ float:right;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:none;
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+ font-size: 8pt;
+}
+
+
+div.summary
+{
+ float: right;
+ font-size: 8pt;
+ padding-right: 5px;
+ width: 50%;
+ text-align: right;
+}
+
+div.summary a
+{
+ white-space: nowrap;
+}
+
+table.classindex
+{
+ margin: 10px;
+ white-space: nowrap;
+ margin-left: 3%;
+ margin-right: 3%;
+ width: 94%;
+ border: 0;
+ border-spacing: 0;
+ padding: 0;
+}
+
+div.ingroups
+{
+ font-size: 8pt;
+ width: 50%;
+ text-align: left;
+}
+
+div.ingroups a
+{
+ white-space: nowrap;
+}
+
+div.header
+{
+ background-image:url('nav_h.png');
+ background-repeat:repeat-x;
+ background-color: #F9FAFC;
+ margin: 0px;
+ border-bottom: 1px solid #C4CFE5;
+}
+
+div.headertitle
+{
+ padding: 5px 5px 5px 10px;
+}
+
+dl
+{
+ padding: 0 0 0 10px;
+}
+
+/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug */
+dl.section
+{
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+dl.note
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #D0C000;
+}
+
+dl.warning, dl.attention
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #FF0000;
+}
+
+dl.pre, dl.post, dl.invariant
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #00D000;
+}
+
+dl.deprecated
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #505050;
+}
+
+dl.todo
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #00C0E0;
+}
+
+dl.test
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #3030E0;
+}
+
+dl.bug
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #C08050;
+}
+
+dl.section dd {
+ margin-bottom: 6px;
+}
+
+
+#projectlogo
+{
+ text-align: center;
+ vertical-align: bottom;
+ border-collapse: separate;
+}
+
+#projectlogo img
+{
+ border: 0px none;
+}
+
+#projectalign
+{
+ vertical-align: middle;
+}
+
+#projectname
+{
+ font: 300% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 2px 0px;
+}
+
+#projectbrief
+{
+ font: 120% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#projectnumber
+{
+ font: 50% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#titlearea
+{
+ padding: 0px;
+ margin: 0px;
+ width: 100%;
+ border-bottom: 1px solid #5373B4;
+}
+
+.image
+{
+ text-align: center;
+}
+
+.dotgraph
+{
+ text-align: center;
+}
+
+.mscgraph
+{
+ text-align: center;
+}
+
+.diagraph
+{
+ text-align: center;
+}
+
+.caption
+{
+ font-weight: bold;
+}
+
+div.zoom
+{
+ border: 1px solid #90A5CE;
+}
+
+dl.citelist {
+ margin-bottom:50px;
+}
+
+dl.citelist dt {
+ color:#334975;
+ float:left;
+ font-weight:bold;
+ margin-right:10px;
+ padding:5px;
+}
+
+dl.citelist dd {
+ margin:2px 0;
+ padding:5px 0;
+}
+
+div.toc {
+ padding: 14px 25px;
+ background-color: #F4F6FA;
+ border: 1px solid #D8DFEE;
+ border-radius: 7px 7px 7px 7px;
+ float: right;
+ height: auto;
+ margin: 0 8px 10px 10px;
+ width: 200px;
+}
+
+div.toc li {
+ background: url("bdwn.png") no-repeat scroll 0 5px transparent;
+ font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif;
+ margin-top: 5px;
+ padding-left: 10px;
+ padding-top: 2px;
+}
+
+div.toc h3 {
+ font: bold 12px/1.2 Arial,FreeSans,sans-serif;
+ color: #4665A2;
+ border-bottom: 0 none;
+ margin: 0;
+}
+
+div.toc ul {
+ list-style: none outside none;
+ border: medium none;
+ padding: 0px;
+}
+
+div.toc li.level1 {
+ margin-left: 0px;
+}
+
+div.toc li.level2 {
+ margin-left: 15px;
+}
+
+div.toc li.level3 {
+ margin-left: 30px;
+}
+
+div.toc li.level4 {
+ margin-left: 45px;
+}
+
+.inherit_header {
+ font-weight: bold;
+ color: gray;
+ cursor: pointer;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+.inherit_header td {
+ padding: 6px 0px 2px 5px;
+}
+
+.inherit {
+ display: none;
+}
+
+tr.heading h2 {
+ margin-top: 12px;
+ margin-bottom: 4px;
+}
+
+/* tooltip related style info */
+
+.ttc {
+ position: absolute;
+ display: none;
+}
+
+#powerTip {
+ cursor: default;
+ white-space: nowrap;
+ background-color: white;
+ border: 1px solid gray;
+ border-radius: 4px 4px 4px 4px;
+ box-shadow: 1px 1px 7px gray;
+ display: none;
+ font-size: smaller;
+ max-width: 80%;
+ opacity: 0.9;
+ padding: 1ex 1em 1em;
+ position: absolute;
+ z-index: 2147483647;
+}
+
+#powerTip div.ttdoc {
+ color: grey;
+ font-style: italic;
+}
+
+#powerTip div.ttname a {
+ font-weight: bold;
+}
+
+#powerTip div.ttname {
+ font-weight: bold;
+}
+
+#powerTip div.ttdeci {
+ color: #006318;
+}
+
+#powerTip div {
+ margin: 0px;
+ padding: 0px;
+ font: 12px/16px Roboto,sans-serif;
+}
+
+#powerTip:before, #powerTip:after {
+ content: "";
+ position: absolute;
+ margin: 0px;
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.w:after, #powerTip.w:before,
+#powerTip.e:after, #powerTip.e:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.nw:after, #powerTip.nw:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ border: solid transparent;
+ content: " ";
+ height: 0;
+ width: 0;
+ position: absolute;
+}
+
+#powerTip.n:after, #powerTip.s:after,
+#powerTip.w:after, #powerTip.e:after,
+#powerTip.nw:after, #powerTip.ne:after,
+#powerTip.sw:after, #powerTip.se:after {
+ border-color: rgba(255, 255, 255, 0);
+}
+
+#powerTip.n:before, #powerTip.s:before,
+#powerTip.w:before, #powerTip.e:before,
+#powerTip.nw:before, #powerTip.ne:before,
+#powerTip.sw:before, #powerTip.se:before {
+ border-color: rgba(128, 128, 128, 0);
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.nw:after, #powerTip.nw:before {
+ top: 100%;
+}
+
+#powerTip.n:after, #powerTip.ne:after, #powerTip.nw:after {
+ border-top-color: #ffffff;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+#powerTip.n:before {
+ border-top-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+#powerTip.n:after, #powerTip.n:before {
+ left: 50%;
+}
+
+#powerTip.nw:after, #powerTip.nw:before {
+ right: 14px;
+}
+
+#powerTip.ne:after, #powerTip.ne:before {
+ left: 14px;
+}
+
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ bottom: 100%;
+}
+
+#powerTip.s:after, #powerTip.se:after, #powerTip.sw:after {
+ border-bottom-color: #ffffff;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+
+#powerTip.s:before, #powerTip.se:before, #powerTip.sw:before {
+ border-bottom-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+
+#powerTip.s:after, #powerTip.s:before {
+ left: 50%;
+}
+
+#powerTip.sw:after, #powerTip.sw:before {
+ right: 14px;
+}
+
+#powerTip.se:after, #powerTip.se:before {
+ left: 14px;
+}
+
+#powerTip.e:after, #powerTip.e:before {
+ left: 100%;
+}
+#powerTip.e:after {
+ border-left-color: #ffffff;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.e:before {
+ border-left-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+#powerTip.w:after, #powerTip.w:before {
+ right: 100%;
+}
+#powerTip.w:after {
+ border-right-color: #ffffff;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.w:before {
+ border-right-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+@media print
+{
+ #top { display: none; }
+ #side-nav { display: none; }
+ #nav-path { display: none; }
+ body { overflow:visible; }
+ h1, h2, h3, h4, h5, h6 { page-break-after: avoid; }
+ .summary { display: none; }
+ .memitem { page-break-inside: avoid; }
+ #doc-content
+ {
+ margin-left:0 !important;
+ height:auto !important;
+ width:auto !important;
+ overflow:inherit;
+ display:inline;
+ }
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/footer.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/footer.html
new file mode 100644
index 0000000000000000000000000000000000000000..3d5b2080eb4b181a024b11dd82d298895e6dbb6a
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/footer.html
@@ -0,0 +1,21 @@
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<!--BEGIN GENERATE_TREEVIEW-->
+<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
+ <ul>
+ $navpath
+ <li class="footer">$generatedby
+ <a href="http://www.doxygen.org/index.html">
+ <img class="footer" src="$relpath^doxygen.png" alt="doxygen"/></a> $doxygenversion </li>
+ </ul>
+</div>
+<!--END GENERATE_TREEVIEW-->
+<!--BEGIN !GENERATE_TREEVIEW-->
+<hr class="footer"/><address class="footer"><small>
+$generatedby  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="$relpath^doxygen.png" alt="doxygen"/>
+</a> $doxygenversion
+</small></address>
+<!--END !GENERATE_TREEVIEW-->
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/header.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/header.html
new file mode 100644
index 0000000000000000000000000000000000000000..da974503f7f0978740e0d5bfbc1317df09278f16
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/header.html
@@ -0,0 +1,55 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen $doxygenversion"/>
+<!--BEGIN PROJECT_NAME--><title>$projectname: $title</title><!--END PROJECT_NAME-->
+<!--BEGIN !PROJECT_NAME--><title>$title</title><!--END !PROJECT_NAME-->
+<link href="$relpath^tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="$relpath^jquery.js"></script>
+<script type="text/javascript" src="$relpath^dynsections.js"></script>
+$treeview
+$search
+$mathjax
+<link href="$relpath^$stylesheet" rel="stylesheet" type="text/css" />
+$extrastylesheet
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+
+<!--BEGIN TITLEAREA-->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <!--BEGIN PROJECT_LOGO-->
+ <td id="projectlogo"><img alt="Logo" src="$relpath^$projectlogo"/></td>
+ <!--END PROJECT_LOGO-->
+ <!--BEGIN PROJECT_NAME-->
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">$projectname
+ <!--BEGIN PROJECT_NUMBER--> <span id="projectnumber">$projectnumber</span><!--END PROJECT_NUMBER-->
+ </div></a>
+ <!--BEGIN PROJECT_BRIEF--><div id="projectbrief">$projectbrief</div><!--END PROJECT_BRIEF-->
+ </td>
+ <!--END PROJECT_NAME-->
+ <!--BEGIN !PROJECT_NAME-->
+ <!--BEGIN PROJECT_BRIEF-->
+ <td style="padding-left: 0.5em;">
+ <div id="projectbrief">$projectbrief</div>
+ </td>
+ <!--END PROJECT_BRIEF-->
+ <!--END !PROJECT_NAME-->
+ <!--BEGIN DISABLE_INDEX-->
+ <!--BEGIN SEARCHENGINE-->
+ <td>$searchbox</td>
+ <!--END SEARCHENGINE-->
+ <!--END DISABLE_INDEX-->
+ </tr>
+ </tbody>
+</table>
+</div>
+<!--END TITLEAREA-->
+<!-- end header part -->
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowdown.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowdown.png
new file mode 100644
index 0000000000000000000000000000000000000000..0b63f6d38c4b9ec907b820192ebe9724ed6eca22
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowdown.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowright.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowright.png
new file mode 100644
index 0000000000000000000000000000000000000000..c6ee22f937a07d1dbfc27c669d11f8ed13e2f152
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowright.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bc_s.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bc_s.png
new file mode 100644
index 0000000000000000000000000000000000000000..224b29aa9847d5a4b3902efd602b7ddf7d33e6c2
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bc_s.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bdwn.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bdwn.png
new file mode 100644
index 0000000000000000000000000000000000000000..940a0b950443a0bb1b216ac03c45b8a16c955452
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bdwn.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/closed.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/closed.png
new file mode 100644
index 0000000000000000000000000000000000000000..98cc2c909da37a6df914fbf67780eebd99c597f5
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/closed.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html
new file mode 100644
index 0000000000000000000000000000000000000000..00d268b69687b4ba078a1161e172e05df4f92c92
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html
@@ -0,0 +1,59 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: src Directory Reference</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+<div id="nav-path" class="navpath">
+ <ul>
+<li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li> </ul>
+</div>
+</div><!-- top -->
+<div class="header">
+ <div class="headertitle">
+<div class="title">src Directory Reference</div> </div>
+</div><!--header-->
+<div class="contents">
+<table class="memberdecls">
+<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="files"></a>
+Files</h2></td></tr>
+<tr class="memitem:i2cpwm__controller_8cpp"><td class="memItemLeft" align="right" valign="top">file  </td><td class="memItemRight" valign="bottom"><a class="el" href="i2cpwm__controller_8cpp.html">i2cpwm_controller.cpp</a> <a href="i2cpwm__controller_8cpp_source.html">[code]</a></td></tr>
+<tr class="memdesc:i2cpwm__controller_8cpp"><td class="mdescLeft"> </td><td class="mdescRight">controller for I2C interfaced 16 channel PWM boards with PCA9685 chip <br /></td></tr>
+<tr class="separator:"><td class="memSeparator" colspan="2"> </td></tr>
+</table>
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doc.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doc.png
new file mode 100644
index 0000000000000000000000000000000000000000..17edabff95f7b8da13c9516a04efe05493c29501
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doc.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.css b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.css
new file mode 100644
index 0000000000000000000000000000000000000000..1425ec530d3181ed8c9651987cfa9ae72df9b339
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.css
@@ -0,0 +1,1475 @@
+/* The standard CSS for doxygen 1.8.11 */
+
+body, table, div, p, dl {
+ font: 400 14px/22px Roboto,sans-serif;
+}
+
+/* @group Heading Levels */
+
+h1.groupheader {
+ font-size: 150%;
+}
+
+.title {
+ font: 400 14px/28px Roboto,sans-serif;
+ font-size: 150%;
+ font-weight: bold;
+ margin: 10px 2px;
+}
+
+h2.groupheader {
+ border-bottom: 1px solid #879ECB;
+ color: #354C7B;
+ font-size: 150%;
+ font-weight: normal;
+ margin-top: 1.75em;
+ padding-top: 8px;
+ padding-bottom: 4px;
+ width: 100%;
+}
+
+h3.groupheader {
+ font-size: 100%;
+}
+
+h1, h2, h3, h4, h5, h6 {
+ -webkit-transition: text-shadow 0.5s linear;
+ -moz-transition: text-shadow 0.5s linear;
+ -ms-transition: text-shadow 0.5s linear;
+ -o-transition: text-shadow 0.5s linear;
+ transition: text-shadow 0.5s linear;
+ margin-right: 15px;
+}
+
+h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow {
+ text-shadow: 0 0 15px cyan;
+}
+
+dt {
+ font-weight: bold;
+}
+
+div.multicol {
+ -moz-column-gap: 1em;
+ -webkit-column-gap: 1em;
+ -moz-column-count: 3;
+ -webkit-column-count: 3;
+}
+
+p.startli, p.startdd {
+ margin-top: 2px;
+}
+
+p.starttd {
+ margin-top: 0px;
+}
+
+p.endli {
+ margin-bottom: 0px;
+}
+
+p.enddd {
+ margin-bottom: 4px;
+}
+
+p.endtd {
+ margin-bottom: 2px;
+}
+
+/* @end */
+
+caption {
+ font-weight: bold;
+}
+
+span.legend {
+ font-size: 70%;
+ text-align: center;
+}
+
+h3.version {
+ font-size: 90%;
+ text-align: center;
+}
+
+div.qindex, div.navtab{
+ background-color: #EBEFF6;
+ border: 1px solid #A3B4D7;
+ text-align: center;
+}
+
+div.qindex, div.navpath {
+ width: 100%;
+ line-height: 140%;
+}
+
+div.navtab {
+ margin-right: 15px;
+}
+
+/* @group Link Styling */
+
+a {
+ color: #3D578C;
+ font-weight: normal;
+ text-decoration: none;
+}
+
+.contents a:visited {
+ color: #4665A2;
+}
+
+a:hover {
+ text-decoration: underline;
+}
+
+a.qindex {
+ font-weight: bold;
+}
+
+a.qindexHL {
+ font-weight: bold;
+ background-color: #9CAFD4;
+ color: #ffffff;
+ border: 1px double #869DCA;
+}
+
+.contents a.qindexHL:visited {
+ color: #ffffff;
+}
+
+a.el {
+ font-weight: bold;
+}
+
+a.elRef {
+}
+
+a.code, a.code:visited, a.line, a.line:visited {
+ color: #4665A2;
+}
+
+a.codeRef, a.codeRef:visited, a.lineRef, a.lineRef:visited {
+ color: #4665A2;
+}
+
+/* @end */
+
+dl.el {
+ margin-left: -1cm;
+}
+
+pre.fragment {
+ border: 1px solid #C4CFE5;
+ background-color: #FBFCFD;
+ padding: 4px 6px;
+ margin: 4px 8px 4px 2px;
+ overflow: auto;
+ word-wrap: break-word;
+ font-size: 9pt;
+ line-height: 125%;
+ font-family: monospace, fixed;
+ font-size: 105%;
+}
+
+div.fragment {
+ padding: 4px 6px;
+ margin: 4px 8px 4px 2px;
+ background-color: #FBFCFD;
+ border: 1px solid #C4CFE5;
+}
+
+div.line {
+ font-family: monospace, fixed;
+ font-size: 13px;
+ min-height: 13px;
+ line-height: 1.0;
+ text-wrap: unrestricted;
+ white-space: -moz-pre-wrap; /* Moz */
+ white-space: -pre-wrap; /* Opera 4-6 */
+ white-space: -o-pre-wrap; /* Opera 7 */
+ white-space: pre-wrap; /* CSS3 */
+ word-wrap: break-word; /* IE 5.5+ */
+ text-indent: -53px;
+ padding-left: 53px;
+ padding-bottom: 0px;
+ margin: 0px;
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+div.line:after {
+ content:"\000A";
+ white-space: pre;
+}
+
+div.line.glow {
+ background-color: cyan;
+ box-shadow: 0 0 10px cyan;
+}
+
+
+span.lineno {
+ padding-right: 4px;
+ text-align: right;
+ border-right: 2px solid #0F0;
+ background-color: #E8E8E8;
+ white-space: pre;
+}
+span.lineno a {
+ background-color: #D8D8D8;
+}
+
+span.lineno a:hover {
+ background-color: #C8C8C8;
+}
+
+div.ah, span.ah {
+ background-color: black;
+ font-weight: bold;
+ color: #ffffff;
+ margin-bottom: 3px;
+ margin-top: 3px;
+ padding: 0.2em;
+ border: solid thin #333;
+ border-radius: 0.5em;
+ -webkit-border-radius: .5em;
+ -moz-border-radius: .5em;
+ box-shadow: 2px 2px 3px #999;
+ -webkit-box-shadow: 2px 2px 3px #999;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444));
+ background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000 110%);
+}
+
+div.classindex ul {
+ list-style: none;
+ padding-left: 0;
+}
+
+div.classindex span.ai {
+ display: inline-block;
+}
+
+div.groupHeader {
+ margin-left: 16px;
+ margin-top: 12px;
+ font-weight: bold;
+}
+
+div.groupText {
+ margin-left: 16px;
+ font-style: italic;
+}
+
+body {
+ background-color: white;
+ color: black;
+ margin: 0;
+}
+
+div.contents {
+ margin-top: 10px;
+ margin-left: 12px;
+ margin-right: 8px;
+}
+
+td.indexkey {
+ background-color: #EBEFF6;
+ font-weight: bold;
+ border: 1px solid #C4CFE5;
+ margin: 2px 0px 2px 0;
+ padding: 2px 10px;
+ white-space: nowrap;
+ vertical-align: top;
+}
+
+td.indexvalue {
+ background-color: #EBEFF6;
+ border: 1px solid #C4CFE5;
+ padding: 2px 10px;
+ margin: 2px 0px;
+}
+
+tr.memlist {
+ background-color: #EEF1F7;
+}
+
+p.formulaDsp {
+ text-align: center;
+}
+
+img.formulaDsp {
+
+}
+
+img.formulaInl {
+ vertical-align: middle;
+}
+
+div.center {
+ text-align: center;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding: 0px;
+}
+
+div.center img {
+ border: 0px;
+}
+
+address.footer {
+ text-align: right;
+ padding-right: 12px;
+}
+
+img.footer {
+ border: 0px;
+ vertical-align: middle;
+}
+
+/* @group Code Colorization */
+
+span.keyword {
+ color: #008000
+}
+
+span.keywordtype {
+ color: #604020
+}
+
+span.keywordflow {
+ color: #e08000
+}
+
+span.comment {
+ color: #800000
+}
+
+span.preprocessor {
+ color: #806020
+}
+
+span.stringliteral {
+ color: #002080
+}
+
+span.charliteral {
+ color: #008080
+}
+
+span.vhdldigit {
+ color: #ff00ff
+}
+
+span.vhdlchar {
+ color: #000000
+}
+
+span.vhdlkeyword {
+ color: #700070
+}
+
+span.vhdllogic {
+ color: #ff0000
+}
+
+blockquote {
+ background-color: #F7F8FB;
+ border-left: 2px solid #9CAFD4;
+ margin: 0 24px 0 4px;
+ padding: 0 12px 0 16px;
+}
+
+/* @end */
+
+/*
+.search {
+ color: #003399;
+ font-weight: bold;
+}
+
+form.search {
+ margin-bottom: 0px;
+ margin-top: 0px;
+}
+
+input.search {
+ font-size: 75%;
+ color: #000080;
+ font-weight: normal;
+ background-color: #e8eef2;
+}
+*/
+
+td.tiny {
+ font-size: 75%;
+}
+
+.dirtab {
+ padding: 4px;
+ border-collapse: collapse;
+ border: 1px solid #A3B4D7;
+}
+
+th.dirtab {
+ background: #EBEFF6;
+ font-weight: bold;
+}
+
+hr {
+ height: 0px;
+ border: none;
+ border-top: 1px solid #4A6AAA;
+}
+
+hr.footer {
+ height: 1px;
+}
+
+/* @group Member Descriptions */
+
+table.memberdecls {
+ border-spacing: 0px;
+ padding: 0px;
+}
+
+.memberdecls td, .fieldtable tr {
+ -webkit-transition-property: background-color, box-shadow;
+ -webkit-transition-duration: 0.5s;
+ -moz-transition-property: background-color, box-shadow;
+ -moz-transition-duration: 0.5s;
+ -ms-transition-property: background-color, box-shadow;
+ -ms-transition-duration: 0.5s;
+ -o-transition-property: background-color, box-shadow;
+ -o-transition-duration: 0.5s;
+ transition-property: background-color, box-shadow;
+ transition-duration: 0.5s;
+}
+
+.memberdecls td.glow, .fieldtable tr.glow {
+ background-color: cyan;
+ box-shadow: 0 0 15px cyan;
+}
+
+.mdescLeft, .mdescRight,
+.memItemLeft, .memItemRight,
+.memTemplItemLeft, .memTemplItemRight, .memTemplParams {
+ background-color: #F9FAFC;
+ border: none;
+ margin: 4px;
+ padding: 1px 0 0 8px;
+}
+
+.mdescLeft, .mdescRight {
+ padding: 0px 8px 4px 8px;
+ color: #555;
+}
+
+.memSeparator {
+ border-bottom: 1px solid #DEE4F0;
+ line-height: 1px;
+ margin: 0px;
+ padding: 0px;
+}
+
+.memItemLeft, .memTemplItemLeft {
+ white-space: nowrap;
+}
+
+.memItemRight {
+ width: 100%;
+}
+
+.memTemplParams {
+ color: #4665A2;
+ white-space: nowrap;
+ font-size: 80%;
+}
+
+/* @end */
+
+/* @group Member Details */
+
+/* Styles for detailed member documentation */
+
+.memtemplate {
+ font-size: 80%;
+ color: #4665A2;
+ font-weight: normal;
+ margin-left: 9px;
+}
+
+.memnav {
+ background-color: #EBEFF6;
+ border: 1px solid #A3B4D7;
+ text-align: center;
+ margin: 2px;
+ margin-right: 15px;
+ padding: 2px;
+}
+
+.mempage {
+ width: 100%;
+}
+
+.memitem {
+ padding: 0;
+ margin-bottom: 10px;
+ margin-right: 5px;
+ -webkit-transition: box-shadow 0.5s linear;
+ -moz-transition: box-shadow 0.5s linear;
+ -ms-transition: box-shadow 0.5s linear;
+ -o-transition: box-shadow 0.5s linear;
+ transition: box-shadow 0.5s linear;
+ display: table !important;
+ width: 100%;
+}
+
+.memitem.glow {
+ box-shadow: 0 0 15px cyan;
+}
+
+.memname {
+ font-weight: bold;
+ margin-left: 6px;
+}
+
+.memname td {
+ vertical-align: bottom;
+}
+
+.memproto, dl.reflist dt {
+ border-top: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 0px 6px 0px;
+ color: #253555;
+ font-weight: bold;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ background-image:url('nav_f.png');
+ background-repeat:repeat-x;
+ background-color: #E2E8F2;
+ /* opera specific markup */
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ /* firefox specific markup */
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ -moz-border-radius-topright: 4px;
+ -moz-border-radius-topleft: 4px;
+ /* webkit specific markup */
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ -webkit-border-top-right-radius: 4px;
+ -webkit-border-top-left-radius: 4px;
+
+}
+
+.memdoc, dl.reflist dd {
+ border-bottom: 1px solid #A8B8D9;
+ border-left: 1px solid #A8B8D9;
+ border-right: 1px solid #A8B8D9;
+ padding: 6px 10px 2px 10px;
+ background-color: #FBFCFD;
+ border-top-width: 0;
+ background-image:url('nav_g.png');
+ background-repeat:repeat-x;
+ background-color: #FFFFFF;
+ /* opera specific markup */
+ border-bottom-left-radius: 4px;
+ border-bottom-right-radius: 4px;
+ box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+ /* firefox specific markup */
+ -moz-border-radius-bottomleft: 4px;
+ -moz-border-radius-bottomright: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px;
+ /* webkit specific markup */
+ -webkit-border-bottom-left-radius: 4px;
+ -webkit-border-bottom-right-radius: 4px;
+ -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15);
+}
+
+dl.reflist dt {
+ padding: 5px;
+}
+
+dl.reflist dd {
+ margin: 0px 0px 10px 0px;
+ padding: 5px;
+}
+
+.paramkey {
+ text-align: right;
+}
+
+.paramtype {
+ white-space: nowrap;
+}
+
+.paramname {
+ color: #602020;
+ white-space: nowrap;
+}
+.paramname em {
+ font-style: normal;
+}
+.paramname code {
+ line-height: 14px;
+}
+
+.params, .retval, .exception, .tparams {
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+.params .paramname, .retval .paramname {
+ font-weight: bold;
+ vertical-align: top;
+}
+
+.params .paramtype {
+ font-style: italic;
+ vertical-align: top;
+}
+
+.params .paramdir {
+ font-family: "courier new",courier,monospace;
+ vertical-align: top;
+}
+
+table.mlabels {
+ border-spacing: 0px;
+}
+
+td.mlabels-left {
+ width: 100%;
+ padding: 0px;
+}
+
+td.mlabels-right {
+ vertical-align: bottom;
+ padding: 0px;
+ white-space: nowrap;
+}
+
+span.mlabels {
+ margin-left: 8px;
+}
+
+span.mlabel {
+ background-color: #728DC1;
+ border-top:1px solid #5373B4;
+ border-left:1px solid #5373B4;
+ border-right:1px solid #C4CFE5;
+ border-bottom:1px solid #C4CFE5;
+ text-shadow: none;
+ color: white;
+ margin-right: 4px;
+ padding: 2px 3px;
+ border-radius: 3px;
+ font-size: 7pt;
+ white-space: nowrap;
+ vertical-align: middle;
+}
+
+
+
+/* @end */
+
+/* these are for tree view inside a (index) page */
+
+div.directory {
+ margin: 10px 0px;
+ border-top: 1px solid #9CAFD4;
+ border-bottom: 1px solid #9CAFD4;
+ width: 100%;
+}
+
+.directory table {
+ border-collapse:collapse;
+}
+
+.directory td {
+ margin: 0px;
+ padding: 0px;
+ vertical-align: top;
+}
+
+.directory td.entry {
+ white-space: nowrap;
+ padding-right: 6px;
+ padding-top: 3px;
+}
+
+.directory td.entry a {
+ outline:none;
+}
+
+.directory td.entry a img {
+ border: none;
+}
+
+.directory td.desc {
+ width: 100%;
+ padding-left: 6px;
+ padding-right: 6px;
+ padding-top: 3px;
+ border-left: 1px solid rgba(0,0,0,0.05);
+}
+
+.directory tr.even {
+ padding-left: 6px;
+ background-color: #F7F8FB;
+}
+
+.directory img {
+ vertical-align: -30%;
+}
+
+.directory .levels {
+ white-space: nowrap;
+ width: 100%;
+ text-align: right;
+ font-size: 9pt;
+}
+
+.directory .levels span {
+ cursor: pointer;
+ padding-left: 2px;
+ padding-right: 2px;
+ color: #3D578C;
+}
+
+.arrow {
+ color: #9CAFD4;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+ cursor: pointer;
+ font-size: 80%;
+ display: inline-block;
+ width: 16px;
+ height: 22px;
+}
+
+.icon {
+ font-family: Arial, Helvetica;
+ font-weight: bold;
+ font-size: 12px;
+ height: 14px;
+ width: 16px;
+ display: inline-block;
+ background-color: #728DC1;
+ color: white;
+ text-align: center;
+ border-radius: 4px;
+ margin-left: 2px;
+ margin-right: 2px;
+}
+
+.icona {
+ width: 24px;
+ height: 22px;
+ display: inline-block;
+}
+
+.iconfopen {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderopen.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.iconfclosed {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('folderclosed.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+.icondoc {
+ width: 24px;
+ height: 18px;
+ margin-bottom: 4px;
+ background-image:url('doc.png');
+ background-position: 0px -4px;
+ background-repeat: repeat-y;
+ vertical-align:top;
+ display: inline-block;
+}
+
+table.directory {
+ font: 400 14px Roboto,sans-serif;
+}
+
+/* @end */
+
+div.dynheader {
+ margin-top: 8px;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+address {
+ font-style: normal;
+ color: #2A3D61;
+}
+
+table.doxtable caption {
+ caption-side: top;
+}
+
+table.doxtable {
+ border-collapse:collapse;
+ margin-top: 4px;
+ margin-bottom: 4px;
+}
+
+table.doxtable td, table.doxtable th {
+ border: 1px solid #2D4068;
+ padding: 3px 7px 2px;
+}
+
+table.doxtable th {
+ background-color: #374F7F;
+ color: #FFFFFF;
+ font-size: 110%;
+ padding-bottom: 4px;
+ padding-top: 5px;
+}
+
+table.fieldtable {
+ /*width: 100%;*/
+ margin-bottom: 10px;
+ border: 1px solid #A8B8D9;
+ border-spacing: 0px;
+ -moz-border-radius: 4px;
+ -webkit-border-radius: 4px;
+ border-radius: 4px;
+ -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px;
+ -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+ box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15);
+}
+
+.fieldtable td, .fieldtable th {
+ padding: 3px 7px 2px;
+}
+
+.fieldtable td.fieldtype, .fieldtable td.fieldname {
+ white-space: nowrap;
+ border-right: 1px solid #A8B8D9;
+ border-bottom: 1px solid #A8B8D9;
+ vertical-align: top;
+}
+
+.fieldtable td.fieldname {
+ padding-top: 3px;
+}
+
+.fieldtable td.fielddoc {
+ border-bottom: 1px solid #A8B8D9;
+ /*width: 100%;*/
+}
+
+.fieldtable td.fielddoc p:first-child {
+ margin-top: 0px;
+}
+
+.fieldtable td.fielddoc p:last-child {
+ margin-bottom: 2px;
+}
+
+.fieldtable tr:last-child td {
+ border-bottom: none;
+}
+
+.fieldtable th {
+ background-image:url('nav_f.png');
+ background-repeat:repeat-x;
+ background-color: #E2E8F2;
+ font-size: 90%;
+ color: #253555;
+ padding-bottom: 4px;
+ padding-top: 5px;
+ text-align:left;
+ -moz-border-radius-topleft: 4px;
+ -moz-border-radius-topright: 4px;
+ -webkit-border-top-left-radius: 4px;
+ -webkit-border-top-right-radius: 4px;
+ border-top-left-radius: 4px;
+ border-top-right-radius: 4px;
+ border-bottom: 1px solid #A8B8D9;
+}
+
+
+.tabsearch {
+ top: 0px;
+ left: 10px;
+ height: 36px;
+ background-image: url('tab_b.png');
+ z-index: 101;
+ overflow: hidden;
+ font-size: 13px;
+}
+
+.navpath ul
+{
+ font-size: 11px;
+ background-image:url('tab_b.png');
+ background-repeat:repeat-x;
+ background-position: 0 -5px;
+ height:30px;
+ line-height:30px;
+ color:#8AA0CC;
+ border:solid 1px #C2CDE4;
+ overflow:hidden;
+ margin:0px;
+ padding:0px;
+}
+
+.navpath li
+{
+ list-style-type:none;
+ float:left;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:url('bc_s.png');
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+}
+
+.navpath li.navelem a
+{
+ height:32px;
+ display:block;
+ text-decoration: none;
+ outline: none;
+ color: #283A5D;
+ font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ text-decoration: none;
+}
+
+.navpath li.navelem a:hover
+{
+ color:#6884BD;
+}
+
+.navpath li.footer
+{
+ list-style-type:none;
+ float:right;
+ padding-left:10px;
+ padding-right:15px;
+ background-image:none;
+ background-repeat:no-repeat;
+ background-position:right;
+ color:#364D7C;
+ font-size: 8pt;
+}
+
+
+div.summary
+{
+ float: right;
+ font-size: 8pt;
+ padding-right: 5px;
+ width: 50%;
+ text-align: right;
+}
+
+div.summary a
+{
+ white-space: nowrap;
+}
+
+table.classindex
+{
+ margin: 10px;
+ white-space: nowrap;
+ margin-left: 3%;
+ margin-right: 3%;
+ width: 94%;
+ border: 0;
+ border-spacing: 0;
+ padding: 0;
+}
+
+div.ingroups
+{
+ font-size: 8pt;
+ width: 50%;
+ text-align: left;
+}
+
+div.ingroups a
+{
+ white-space: nowrap;
+}
+
+div.header
+{
+ background-image:url('nav_h.png');
+ background-repeat:repeat-x;
+ background-color: #F9FAFC;
+ margin: 0px;
+ border-bottom: 1px solid #C4CFE5;
+}
+
+div.headertitle
+{
+ padding: 5px 5px 5px 10px;
+}
+
+dl
+{
+ padding: 0 0 0 10px;
+}
+
+/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug */
+dl.section
+{
+ margin-left: 0px;
+ padding-left: 0px;
+}
+
+dl.note
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #D0C000;
+}
+
+dl.warning, dl.attention
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #FF0000;
+}
+
+dl.pre, dl.post, dl.invariant
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #00D000;
+}
+
+dl.deprecated
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #505050;
+}
+
+dl.todo
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #00C0E0;
+}
+
+dl.test
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #3030E0;
+}
+
+dl.bug
+{
+ margin-left:-7px;
+ padding-left: 3px;
+ border-left:4px solid;
+ border-color: #C08050;
+}
+
+dl.section dd {
+ margin-bottom: 6px;
+}
+
+
+#projectlogo
+{
+ text-align: center;
+ vertical-align: bottom;
+ border-collapse: separate;
+}
+
+#projectlogo img
+{
+ border: 0px none;
+}
+
+#projectalign
+{
+ vertical-align: middle;
+}
+
+#projectname
+{
+ font: 300% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 2px 0px;
+}
+
+#projectbrief
+{
+ font: 120% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#projectnumber
+{
+ font: 50% Tahoma, Arial,sans-serif;
+ margin: 0px;
+ padding: 0px;
+}
+
+#titlearea
+{
+ padding: 0px;
+ margin: 0px;
+ width: 100%;
+ border-bottom: 1px solid #5373B4;
+}
+
+.image
+{
+ text-align: center;
+}
+
+.dotgraph
+{
+ text-align: center;
+}
+
+.mscgraph
+{
+ text-align: center;
+}
+
+.diagraph
+{
+ text-align: center;
+}
+
+.caption
+{
+ font-weight: bold;
+}
+
+div.zoom
+{
+ border: 1px solid #90A5CE;
+}
+
+dl.citelist {
+ margin-bottom:50px;
+}
+
+dl.citelist dt {
+ color:#334975;
+ float:left;
+ font-weight:bold;
+ margin-right:10px;
+ padding:5px;
+}
+
+dl.citelist dd {
+ margin:2px 0;
+ padding:5px 0;
+}
+
+div.toc {
+ padding: 14px 25px;
+ background-color: #F4F6FA;
+ border: 1px solid #D8DFEE;
+ border-radius: 7px 7px 7px 7px;
+ float: right;
+ height: auto;
+ margin: 0 8px 10px 10px;
+ width: 200px;
+}
+
+div.toc li {
+ background: url("bdwn.png") no-repeat scroll 0 5px transparent;
+ font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif;
+ margin-top: 5px;
+ padding-left: 10px;
+ padding-top: 2px;
+}
+
+div.toc h3 {
+ font: bold 12px/1.2 Arial,FreeSans,sans-serif;
+ color: #4665A2;
+ border-bottom: 0 none;
+ margin: 0;
+}
+
+div.toc ul {
+ list-style: none outside none;
+ border: medium none;
+ padding: 0px;
+}
+
+div.toc li.level1 {
+ margin-left: 0px;
+}
+
+div.toc li.level2 {
+ margin-left: 15px;
+}
+
+div.toc li.level3 {
+ margin-left: 30px;
+}
+
+div.toc li.level4 {
+ margin-left: 45px;
+}
+
+.inherit_header {
+ font-weight: bold;
+ color: gray;
+ cursor: pointer;
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+.inherit_header td {
+ padding: 6px 0px 2px 5px;
+}
+
+.inherit {
+ display: none;
+}
+
+tr.heading h2 {
+ margin-top: 12px;
+ margin-bottom: 4px;
+}
+
+/* tooltip related style info */
+
+.ttc {
+ position: absolute;
+ display: none;
+}
+
+#powerTip {
+ cursor: default;
+ white-space: nowrap;
+ background-color: white;
+ border: 1px solid gray;
+ border-radius: 4px 4px 4px 4px;
+ box-shadow: 1px 1px 7px gray;
+ display: none;
+ font-size: smaller;
+ max-width: 80%;
+ opacity: 0.9;
+ padding: 1ex 1em 1em;
+ position: absolute;
+ z-index: 2147483647;
+}
+
+#powerTip div.ttdoc {
+ color: grey;
+ font-style: italic;
+}
+
+#powerTip div.ttname a {
+ font-weight: bold;
+}
+
+#powerTip div.ttname {
+ font-weight: bold;
+}
+
+#powerTip div.ttdeci {
+ color: #006318;
+}
+
+#powerTip div {
+ margin: 0px;
+ padding: 0px;
+ font: 12px/16px Roboto,sans-serif;
+}
+
+#powerTip:before, #powerTip:after {
+ content: "";
+ position: absolute;
+ margin: 0px;
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.w:after, #powerTip.w:before,
+#powerTip.e:after, #powerTip.e:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.nw:after, #powerTip.nw:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ border: solid transparent;
+ content: " ";
+ height: 0;
+ width: 0;
+ position: absolute;
+}
+
+#powerTip.n:after, #powerTip.s:after,
+#powerTip.w:after, #powerTip.e:after,
+#powerTip.nw:after, #powerTip.ne:after,
+#powerTip.sw:after, #powerTip.se:after {
+ border-color: rgba(255, 255, 255, 0);
+}
+
+#powerTip.n:before, #powerTip.s:before,
+#powerTip.w:before, #powerTip.e:before,
+#powerTip.nw:before, #powerTip.ne:before,
+#powerTip.sw:before, #powerTip.se:before {
+ border-color: rgba(128, 128, 128, 0);
+}
+
+#powerTip.n:after, #powerTip.n:before,
+#powerTip.ne:after, #powerTip.ne:before,
+#powerTip.nw:after, #powerTip.nw:before {
+ top: 100%;
+}
+
+#powerTip.n:after, #powerTip.ne:after, #powerTip.nw:after {
+ border-top-color: #ffffff;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+#powerTip.n:before {
+ border-top-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+#powerTip.n:after, #powerTip.n:before {
+ left: 50%;
+}
+
+#powerTip.nw:after, #powerTip.nw:before {
+ right: 14px;
+}
+
+#powerTip.ne:after, #powerTip.ne:before {
+ left: 14px;
+}
+
+#powerTip.s:after, #powerTip.s:before,
+#powerTip.se:after, #powerTip.se:before,
+#powerTip.sw:after, #powerTip.sw:before {
+ bottom: 100%;
+}
+
+#powerTip.s:after, #powerTip.se:after, #powerTip.sw:after {
+ border-bottom-color: #ffffff;
+ border-width: 10px;
+ margin: 0px -10px;
+}
+
+#powerTip.s:before, #powerTip.se:before, #powerTip.sw:before {
+ border-bottom-color: #808080;
+ border-width: 11px;
+ margin: 0px -11px;
+}
+
+#powerTip.s:after, #powerTip.s:before {
+ left: 50%;
+}
+
+#powerTip.sw:after, #powerTip.sw:before {
+ right: 14px;
+}
+
+#powerTip.se:after, #powerTip.se:before {
+ left: 14px;
+}
+
+#powerTip.e:after, #powerTip.e:before {
+ left: 100%;
+}
+#powerTip.e:after {
+ border-left-color: #ffffff;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.e:before {
+ border-left-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+#powerTip.w:after, #powerTip.w:before {
+ right: 100%;
+}
+#powerTip.w:after {
+ border-right-color: #ffffff;
+ border-width: 10px;
+ top: 50%;
+ margin-top: -10px;
+}
+#powerTip.w:before {
+ border-right-color: #808080;
+ border-width: 11px;
+ top: 50%;
+ margin-top: -11px;
+}
+
+@media print
+{
+ #top { display: none; }
+ #side-nav { display: none; }
+ #nav-path { display: none; }
+ body { overflow:visible; }
+ h1, h2, h3, h4, h5, h6 { page-break-after: avoid; }
+ .summary { display: none; }
+ .memitem { page-break-inside: avoid; }
+ #doc-content
+ {
+ margin-left:0 !important;
+ height:auto !important;
+ width:auto !important;
+ overflow:inherit;
+ display:inline;
+ }
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.png
new file mode 100644
index 0000000000000000000000000000000000000000..3ff17d807fd8aa003bed8bb2a69e8f0909592fd1
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/files.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/files.html
new file mode 100644
index 0000000000000000000000000000000000000000..d8829576332f4ac1815de9bfd06ffa7ef800bf79
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/files.html
@@ -0,0 +1,54 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: File List</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="header">
+ <div class="headertitle">
+<div class="title">File List</div> </div>
+</div><!--header-->
+<div class="contents">
+<div class="textblock">Here is a list of all documented files with brief descriptions:</div><div class="directory">
+<div class="levels">[detail level <span onclick="javascript:toggleLevel(1);">1</span><span onclick="javascript:toggleLevel(2);">2</span>]</div><table class="directory">
+<tr id="row_0_" class="even"><td class="entry"><span style="width:0px;display:inline-block;"> </span><span id="arr_0_" class="arrow" onclick="toggleFolder('0_')">▼</span><span id="img_0_" class="iconfopen" onclick="toggleFolder('0_')"> </span><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html" target="_self">src</a></td><td class="desc"></td></tr>
+<tr id="row_0_0_"><td class="entry"><span style="width:32px;display:inline-block;"> </span><a href="i2cpwm__controller_8cpp_source.html"><span class="icondoc"></span></a><a class="el" href="i2cpwm__controller_8cpp.html" target="_self">i2cpwm_controller.cpp</a></td><td class="desc">Controller for I2C interfaced 16 channel PWM boards with PCA9685 chip </td></tr>
+</table>
+</div><!-- directory -->
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderclosed.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderclosed.png
new file mode 100644
index 0000000000000000000000000000000000000000..bb8ab35edce8e97554e360005ee9fc5bffb36e66
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderclosed.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderopen.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderopen.png
new file mode 100644
index 0000000000000000000000000000000000000000..d6c7f676a3b3ef8c2c307d319dff3c6a604eb227
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderopen.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals.html
new file mode 100644
index 0000000000000000000000000000000000000000..825faf643a937b5f118797a59ec3f2c3aa1fd509
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals.html
@@ -0,0 +1,67 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Globals</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="contents">
+<div class="textblock">Here is a list of all documented functions, variables, defines, enums, and typedefs with links to the documentation:</div><ul>
+<li>config_drive_mode()
+: <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">i2cpwm_controller.cpp</a>
+</li>
+<li>config_servos()
+: <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_absolute()
+: <a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_drive()
+: <a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_proportional()
+: <a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">i2cpwm_controller.cpp</a>
+</li>
+<li>set_pwm_frequency()
+: <a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">i2cpwm_controller.cpp</a>
+</li>
+<li>stop_servos()
+: <a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e">i2cpwm_controller.cpp</a>
+</li>
+</ul>
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals_func.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals_func.html
new file mode 100644
index 0000000000000000000000000000000000000000..041593e65108ae6f167e622ad4c546ea36c2f5ab
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals_func.html
@@ -0,0 +1,67 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Globals</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="contents">
+ <ul>
+<li>config_drive_mode()
+: <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">i2cpwm_controller.cpp</a>
+</li>
+<li>config_servos()
+: <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_absolute()
+: <a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_drive()
+: <a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">i2cpwm_controller.cpp</a>
+</li>
+<li>servos_proportional()
+: <a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">i2cpwm_controller.cpp</a>
+</li>
+<li>set_pwm_frequency()
+: <a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">i2cpwm_controller.cpp</a>
+</li>
+<li>stop_servos()
+: <a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e">i2cpwm_controller.cpp</a>
+</li>
+</ul>
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___services.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___services.html
new file mode 100644
index 0000000000000000000000000000000000000000..55101a250fe74057eda7996a60af3f8d2de58d87
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___services.html
@@ -0,0 +1,259 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Services interfaces provided by this package</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="header">
+ <div class="summary">
+<a href="#func-members">Functions</a> </div>
+ <div class="headertitle">
+<div class="title">Services interfaces provided by this package</div> </div>
+</div><!--header-->
+<div class="contents">
+<table class="memberdecls">
+<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="func-members"></a>
+Functions</h2></td></tr>
+<tr class="memitem:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">set_pwm_frequency</a> (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)</td></tr>
+<tr class="memdesc:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="mdescLeft"> </td><td class="mdescRight">service to set set PWM frequency <a href="#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">More...</a><br /></td></tr>
+<tr class="separator:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">config_servos</a> (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)</td></tr>
+<tr class="memdesc:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="mdescLeft"> </td><td class="mdescRight">store configuration data for servos on the active board <a href="#gabf512716d4497c6a4cb7bf9c5d94e2f1">More...</a><br /></td></tr>
+<tr class="separator:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">config_drive_mode</a> (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)</td></tr>
+<tr class="memdesc:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="mdescLeft"> </td><td class="mdescRight">set drive mode and drive servos <a href="#gae3222f92a0cd33b0d0a2fcfd6d563ba9">More...</a><br /></td></tr>
+<tr class="separator:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:ga4583b417d172bde68e75db603da3362e"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e">stop_servos</a> (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)</td></tr>
+<tr class="memdesc:ga4583b417d172bde68e75db603da3362e"><td class="mdescLeft"> </td><td class="mdescRight">service to stop all servos on all boards <a href="#ga4583b417d172bde68e75db603da3362e">More...</a><br /></td></tr>
+<tr class="separator:ga4583b417d172bde68e75db603da3362e"><td class="memSeparator" colspan="2"> </td></tr>
+</table>
+<a name="details" id="details"></a><h2 class="groupheader">Detailed Description</h2>
+<h2 class="groupheader">Function Documentation</h2>
+<a class="anchor" id="ga948cb0dd7ba8c2ac4c7f2d2d93940d85"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">bool set_pwm_frequency </td>
+ <td>(</td>
+ <td class="paramtype">i2cpwm_board::IntValue::Request & </td>
+ <td class="paramname"><em>req</em>, </td>
+ </tr>
+ <tr>
+ <td class="paramkey"></td>
+ <td></td>
+ <td class="paramtype">i2cpwm_board::IntValue::Response & </td>
+ <td class="paramname"><em>res</em> </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td>)</td>
+ <td></td><td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>service to set set PWM frequency </p>
+<p>The PWM boards drive LED and servos using pulse width modulation. The 12 bit interface means values are 0..4096. The size of the minimum width is determined by the frequency. is service is needed when using a board configured other than with the default I2C address and when using multiple boards.</p>
+<p>If using the set_active_board() service, it must be used before using other services or topics from this package.</p>
+<p><b>Warning:</b> Changing the frequency will affect any active servos.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramdir">[in]</td><td class="paramname">req</td><td>an Int16 value for the requested pulse frequency </td></tr>
+ <tr><td class="paramdir">[out]</td><td class="paramname">res</td><td>the return value will be the new active frequency </td></tr>
+ </table>
+ </dd>
+</dl>
+<dl class="section return"><dt>Returns</dt><dd>true</dd></dl>
+<p><b>i2cpwm_board::IntValue</b> </p><div class="fragment"><div class="line"># there are a few services whic take a single integer as input</div><div class="line"># these services share the IntValue service definition</div><div class="line"></div><div class="line">int16 value</div><div class="line">---</div><div class="line">int16 error</div><div class="line"></div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # Analog RC servos are most often designed for 20ms pulses. This is achieved with a frequency of 50Hz.</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # This software defaults to 50Hz. Use the set_pwm_frequncy() to change this frequency value. </div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # It may be necessary or convenient to change the PWM frequency if using DC motors connected to PWM controllers. </div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> # It may also be convenient if using PWM to control LEDs.</div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> </div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> rosservice call /set_pwm_frequency "{value: 50}"</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l01172">1172</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+<a class="anchor" id="gabf512716d4497c6a4cb7bf9c5d94e2f1"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">bool config_servos </td>
+ <td>(</td>
+ <td class="paramtype">i2cpwm_board::ServosConfig::Request & </td>
+ <td class="paramname"><em>req</em>, </td>
+ </tr>
+ <tr>
+ <td class="paramkey"></td>
+ <td></td>
+ <td class="paramtype">i2cpwm_board::ServosConfig::Response & </td>
+ <td class="paramname"><em>res</em> </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td>)</td>
+ <td></td><td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>store configuration data for servos on the active board </p>
+<p>A service to set each servo's center value, direction of rotation (1 for forward and -1 for reverse motion), the center or nul value, range, and direction of one or more servos. and range between full left and right or maximun forward and backward speed.</p>
+<p>Setting these data are required before sending messages to the <a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53" title="subscriber topic to move servos in the range of ±1.0 ">servos_proportional()</a> topic as well as the <a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6" title="subscriber topic to move servos based on a drive mode ">servos_drive()</a> topic.</p>
+<p>If more than one PWM board is present, the set_active_board() service is used to switch between boards prior to configuring servos.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramdir">[in]</td><td class="paramname">req</td><td>an array of 'ServoConfig' which consists of a servo number (one based), center(0..4096), range(0..4096), and direction (±1). </td></tr>
+ <tr><td class="paramdir">[out]</td><td class="paramname">res</td><td>integer non-zero if an error occured </td></tr>
+ </table>
+ </dd>
+</dl>
+<dl class="section return"><dt>Returns</dt><dd>true</dd></dl>
+<p><b>i2cpwm_board::ServosConfig</b> </p><div class="fragment"><div class="line"># the servos_config service is used to assign useful configuration data to servos</div><div class="line"># the tollerance of electronis varies in RC servos so it is important to calibate</div><div class="line"># each servo, indicating its PWM value for direction of rotation, centering, and</div><div class="line"># range which is used to scale servo motion a standard ±1000 scale</div><div class="line"></div><div class="line">ServoConfig[] servos</div><div class="line">---</div><div class="line">int16 error</div></div><!-- fragment --><p> <b>i2cpwm_board::ServoConfig</b> </p><div class="fragment"><div class="line"># the ServoConfig message is used to assign specific configuration</div><div class="line"># data to a servo. the data is needed to normalize servos to</div><div class="line"># common values to isolate variations from the rest of the user's</div><div class="line"># robot motion code. </div><div class="line"></div><div class="line">int16 servo</div><div class="line">int16 center</div><div class="line">int16 range</div><div class="line">int16 direction</div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # this example makes the following assumptions about the hardware</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # the servos have been connected to the PWM board starting with the first connector and proceeding in order</div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # any drive servos used for the left side servos are mounted opposite of those for the right side</div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> # this example uses 2 servos</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> # the first servo is the left and the second servo is the right</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> </div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> # configure two continuous rotation servos associated with the drive system - these servos were determined to have a ragee of ±50</div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> </div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span>  {servo: 2, center: 336, range: 108, direction: 1}]"</div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> </div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> </div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]" </div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l01228">1228</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+<a class="anchor" id="gae3222f92a0cd33b0d0a2fcfd6d563ba9"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">bool config_drive_mode </td>
+ <td>(</td>
+ <td class="paramtype">i2cpwm_board::DriveMode::Request & </td>
+ <td class="paramname"><em>req</em>, </td>
+ </tr>
+ <tr>
+ <td class="paramkey"></td>
+ <td></td>
+ <td class="paramtype">i2cpwm_board::DriveMode::Response & </td>
+ <td class="paramname"><em>res</em> </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td>)</td>
+ <td></td><td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>set drive mode and drive servos </p>
+<p>A service to set the desired drive mode. It must be called before messages are handled by the <a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6" title="subscriber topic to move servos based on a drive mode ">servos_drive()</a> topic. Setting these data are required before sending messages to the <a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53" title="subscriber topic to move servos in the range of ±1.0 ">servos_proportional()</a> topic.</p>
+<p>The drive mode consists of a string value for the type of drive desired: ackerman, differential, or mecanum. For each mode, the drive servos must be specified.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramname">req</td><td>[in] DriveMode configuration data </td></tr>
+ <tr><td class="paramname">res</td><td>[out] non-zero on error </td></tr>
+ </table>
+ </dd>
+</dl>
+<dl class="section return"><dt>Returns</dt><dd>true</dd></dl>
+<p>The DriveMode input requires drive system details included: wheel RPM, wheel radius, and track width. ROS uses meters for measurements. The values of radius and track are expected in meters.</p>
+<p><em>A scale factor is available if necessary to compensate for linear vector values.</em></p>
+<p>The mode string is one of the following drive systems:</p><ol type="1">
+<li>'ackerman' - (automobile steering) requires minimum of one servo for drive and uses some other servo for stearing..</li>
+<li>'differential' - requires multiples of two servos, designated as left and right.</li>
+<li>'mecanum' - requires multiples of four servos, designated as left-front, right-front, left-rear, and right-rear.</li>
+</ol>
+<p>The servo message is used for indicating which servos are used for the drive system. The message consists of 'servo' number, and data 'value'.</p>
+<p>The 'value' field indicates the positon the corresponding servo within the drive system. The applicable servos are assigned positions as follows:</p>
+<table class="doxtable">
+<tr>
+<th>positon </th><th>ackerman </th><th>differential </th><th>mecanum </th></tr>
+<tr>
+<td>position 1 corresponds to </td><td>drive </td><td>left </td><td>left-front </td></tr>
+<tr>
+<td>position 2 corresponds to </td><td></td><td>right </td><td>right-front </td></tr>
+<tr>
+<td>position 3 corresponds to </td><td></td><td></td><td>left-rear </td></tr>
+<tr>
+<td>position 4 corresponds to </td><td></td><td></td><td>right-rear </td></tr>
+</table>
+<p><b>i2cpwm_board::DriveMode</b> </p><div class="fragment"><div class="line"># the drive_mode service is used to assigned servos to various drive modes</div><div class="line"># the drive modes determine how the assigned servos respond to geometry_msgs::Twist messages</div><div class="line"># drive modes are one of: ackerman, differential, or mecanum</div><div class="line"># to accurately convert velocity in m/s the controller needs to know three values:</div><div class="line"># the RPM - the maximum output speed available from the drive motors</div><div class="line"># the radius - the drive wheel radius in meters</div><div class="line"># the track - the distance between the left and right wheels in meters</div><div class="line"># use the scale value to adjust incoming Twist values as needed to match the servo/motor capability</div><div class="line"></div><div class="line"></div><div class="line">string mode</div><div class="line">float32 rpm</div><div class="line">float32 radius</div><div class="line">float32 track</div><div class="line">float32 scale</div><div class="line">Position[] servos</div><div class="line">---</div><div class="line">int16 error</div></div><!-- fragment --><p> <b>i2cpwm_board::Position</b> </p><div class="fragment"><div class="line"># the position message is used when configuring drive mode to</div><div class="line"># assign a servo to a specific wheel positon in the drive system</div><div class="line"># postions are specific toe the desired drive mode</div><div class="line"># ackerman has only one position</div><div class="line"># 1 = drive</div><div class="line"># differential has two positons</div><div class="line"># 1 = left, 2 = right</div><div class="line"># mecanum has four positions</div><div class="line"># 1 = front left, 2 = front right, 3 = rear left, 4 = rear right</div><div class="line"># multiple servos/motors may be used for each positon</div><div class="line"></div><div class="line">int16 servo</div><div class="line">int16 position</div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # this example makes the following assumptions about the hardware</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # the servos have been connected to the PWM board starting with the first connector and proceeding in order</div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # any drive servos used for the left side servos are mounted opposite of those for the right side</div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> # ROS used m/s (meters per second) as th standard unit for velocity.</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> # The geometry_msgs::Twist linear and angular vectors are in m/s and radians/s respectively.</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> </div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> # differential drive example</div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> # this example uses 2 servos</div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> # the first servo is the left and the second servo is the right</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span> </div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \</div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span>  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> </div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> # this example uses 4 servos</div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> # there are two servos for the left and two fors the right</div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> </div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \</div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span>  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \</div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  {servo: 3, value: 1}, {servo: 4, value: 2}]}"</div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span> </div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span> # mecanum drive example</div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span> # this example uses 4 servos</div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span> </div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span> rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \</div><div class="line"><a name="l00026"></a><span class="lineno"> 26</span>  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \</div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span>  {servo: 3, value: 1}, {servo: 4, value: 2}]}"</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l01327">1327</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+<a class="anchor" id="ga4583b417d172bde68e75db603da3362e"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">bool stop_servos </td>
+ <td>(</td>
+ <td class="paramtype">std_srvs::Empty::Request & </td>
+ <td class="paramname"><em>req</em>, </td>
+ </tr>
+ <tr>
+ <td class="paramkey"></td>
+ <td></td>
+ <td class="paramtype">std_srvs::Empty::Response & </td>
+ <td class="paramname"><em>res</em> </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td>)</td>
+ <td></td><td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>service to stop all servos on all boards </p>
+<p>A service to stop all of the servos on all of the PWM boards and set their power state to off / coast.</p>
+<p>This is different from setting each servo to its center value. A centered servo is still under power and it's in a brake state.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramname">req</td><td>is empty </td></tr>
+ <tr><td class="paramname">res</td><td>is empty </td></tr>
+ </table>
+ </dd>
+</dl>
+<dl class="section return"><dt>Returns</dt><dd>true</dd></dl>
+<p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # stop all servos on all boards and setting them to coast rather than brake</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> </div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> rosservice call /stop_servos</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l01370">1370</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___topics.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___topics.html
new file mode 100644
index 0000000000000000000000000000000000000000..a95ff4a144e5c2d06ed08ff037c207750b8abbb2
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___topics.html
@@ -0,0 +1,158 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Topics with subscribers provided by this package</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="header">
+ <div class="summary">
+<a href="#func-members">Functions</a> </div>
+ <div class="headertitle">
+<div class="title">Topics with subscribers provided by this package</div> </div>
+</div><!--header-->
+<div class="contents">
+<table class="memberdecls">
+<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="func-members"></a>
+Functions</h2></td></tr>
+<tr class="memitem:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">servos_absolute</a> (const i2cpwm_board::ServoArray::ConstPtr &msg)</td></tr>
+<tr class="memdesc:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos in a physical position <a href="#gaa3fe8c6f5a98243c302c0d27cea219d9">More...</a><br /></td></tr>
+<tr class="separator:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:ga1262b2e51072c237c51b6de4cb199e53"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">servos_proportional</a> (const i2cpwm_board::ServoArray::ConstPtr &msg)</td></tr>
+<tr class="memdesc:ga1262b2e51072c237c51b6de4cb199e53"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos in the range of ±1.0 <a href="#ga1262b2e51072c237c51b6de4cb199e53">More...</a><br /></td></tr>
+<tr class="separator:ga1262b2e51072c237c51b6de4cb199e53"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gae257665eb64139d2720ed332e61507c6"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">servos_drive</a> (const geometry_msgs::Twist::ConstPtr &msg)</td></tr>
+<tr class="memdesc:gae257665eb64139d2720ed332e61507c6"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos based on a drive mode <a href="#gae257665eb64139d2720ed332e61507c6">More...</a><br /></td></tr>
+<tr class="separator:gae257665eb64139d2720ed332e61507c6"><td class="memSeparator" colspan="2"> </td></tr>
+</table>
+<a name="details" id="details"></a><h2 class="groupheader">Detailed Description</h2>
+<h2 class="groupheader">Function Documentation</h2>
+<a class="anchor" id="gaa3fe8c6f5a98243c302c0d27cea219d9"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">void servos_absolute </td>
+ <td>(</td>
+ <td class="paramtype">const i2cpwm_board::ServoArray::ConstPtr & </td>
+ <td class="paramname"><em>msg</em></td><td>)</td>
+ <td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>subscriber topic to move servos in a physical position </p>
+<p>Subscriber for the servos_absolute topic which processes one or more servos and sets their physical pulse value.</p>
+<p>When working with a continuous rotation servo, the topic is used to find the center position of a servo by sending successive values until a stopped position is identified. The topic is also used to find the range of a servo - the fastest forward and fasted reverse values. The difference between these two is the servo's range. Due to variences in servos, each will likely have a slightly different center value.</p>
+<p>When working with a fixed 180 degree rotation servo, the topic is used to find the center position of a servo by sending successive values until a the desired middle is identified. The topic is also used to find the range of a servo - the maximum clockwise and anti clockwise positions. The difference between these two is the servo's range. If the servo rotates slightly more in one direction from center than the other, then '2X' the lesser value should be used as the range to preserve the middle stop position.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramname">msg</td><td>a 'ServoArray' message (array of one or more 'Servo') where the servo:value is the pulse position/speed</td></tr>
+ </table>
+ </dd>
+</dl>
+<p><b>i2cpwm_board::ServoArray</b> </p><div class="fragment"><div class="line"># the ServoArray message is commonly used message in this package to</div><div class="line"># handle multiple assignments of values to servos. the purpose of</div><div class="line"># the value is dependent on the topic or service being called</div><div class="line"></div><div class="line">Servo[] servos</div></div><!-- fragment --><p> <b>i2cpwm_board::Servo</b> </p><div class="fragment"><div class="line"># the Servo message is commonly used message in this package to</div><div class="line"># assign a value to a specific servo. the purpose of the value is</div><div class="line"># dependent on the topic or service being called</div><div class="line"></div><div class="line">int16 servo</div><div class="line">float32 value</div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # this example makes the following assumptions about the hardware</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # the servos have been connected to the PWM board starting with the first connector and proceeding in order</div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # any drive servos used for the left side servos are mounted opposite of those for the right side</div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> # the follow message sets the PWM value of the first two servos to 250 and 350 respectively.</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> # depending on center value of each servo, these values may casue forward or backward rotation</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> </div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 250}, {servo: 2, value: 350}]}"</div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> </div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> # the following messages are an example of finding a continuous servo's center</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span> # in this example the center is found to be 333</div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> </div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 300}]}"</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 350}]}"</div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 320}]}"</div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 330}]}"</div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 335}]}"</div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 333}]}"</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l00797">797</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+<a class="anchor" id="ga1262b2e51072c237c51b6de4cb199e53"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">void servos_proportional </td>
+ <td>(</td>
+ <td class="paramtype">const i2cpwm_board::ServoArray::ConstPtr & </td>
+ <td class="paramname"><em>msg</em></td><td>)</td>
+ <td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>subscriber topic to move servos in the range of ±1.0 </p>
+<p>Subscriber for controlling servos using proportional values. This topic processes one or more servos and sets their physical pulse value based on each servos physical range proportional to a range of ±1.0.</p>
+<p>When working with a continuous rotation servo, the topic is used to adjust the speed of the servo.</p>
+<p>When working with a fixed 180 degree rotation servo, the topic is used to adjust the position of the servo.</p>
+<p>This topic requires the use of the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> service. Once the configuration of the servos - center position, direction of rotation, and PWM range - has been set, these data are to convert the proportional value to a physical PWM value specific to each servo.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramname">msg</td><td>a 'ServoArray' message (array of one or more 'Servo') where the servo:value is a relative position/speed</td></tr>
+ </table>
+ </dd>
+</dl>
+<p><b>i2cpwm_board::ServoArray Message</b> </p><div class="fragment"><div class="line"># the ServoArray message is commonly used message in this package to</div><div class="line"># handle multiple assignments of values to servos. the purpose of</div><div class="line"># the value is dependent on the topic or service being called</div><div class="line"></div><div class="line">Servo[] servos</div></div><!-- fragment --><p> <b>i2cpwm_board::Servo Message</b> </p><div class="fragment"><div class="line"># the Servo message is commonly used message in this package to</div><div class="line"># assign a value to a specific servo. the purpose of the value is</div><div class="line"># dependent on the topic or service being called</div><div class="line"></div><div class="line">int16 servo</div><div class="line">float32 value</div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # this example makes the following assumptions about the hardware</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # the servos have been connected to the PWM board starting with the first connector and proceeding in order</div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # any drive servos used for the left side servos are mounted opposite of those for the right side</div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> # this example uses 2 servos</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> # the first servo is the left and the second servo is the right</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> </div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> # configure two continuous rotation servos associated with the drive system - these servos were determined to have a ragee of ±50</div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> </div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span>  {servo: 2, center: 336, range: 108, direction: 1}]"</div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> </div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> # drive both servos forward at 40% of maximum speed</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> </div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 0.40}, {servo: 2, value: 0.40}]}"</div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> </div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188</div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> </div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span> rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]" </div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span> </div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span> # drive the arm servo to its 45 degree position and then to its -45 degree position</div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span> </div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span> rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: 0.50}]}"</div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span> rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: -0.50}]}"</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l00867">867</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+<a class="anchor" id="gae257665eb64139d2720ed332e61507c6"></a>
+<div class="memitem">
+<div class="memproto">
+ <table class="memname">
+ <tr>
+ <td class="memname">void servos_drive </td>
+ <td>(</td>
+ <td class="paramtype">const geometry_msgs::Twist::ConstPtr & </td>
+ <td class="paramname"><em>msg</em></td><td>)</td>
+ <td></td>
+ </tr>
+ </table>
+</div><div class="memdoc">
+
+<p><code>#include <<a class="el" href="i2cpwm__controller_8cpp.html">src/i2cpwm_controller.cpp</a>></code></p>
+
+<p>subscriber topic to move servos based on a drive mode </p>
+<p>Subscriber for controlling a group of servos in concert based on a geometry_msgs::Twist message.</p>
+<p>This topic requires the use the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> service to configure the servos for proportional control and the use of the <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9" title="set drive mode and drive servos ">config_drive_mode()</a> to speccify the desired type of drive and to assign individual servos to the positions in the drive system.</p>
+<dl class="params"><dt>Parameters</dt><dd>
+ <table class="params">
+ <tr><td class="paramname">msg</td><td>a geometry Twist message</td></tr>
+ </table>
+ </dd>
+</dl>
+<p><b>geometry_msgs::Twist</b> </p><div class="fragment"><div class="line"># This expresses velocity in free space broken into its linear and angular parts.</div><div class="line">Vector3 linear</div><div class="line">Vector3 angular</div></div><!-- fragment --><p> <b>geometry_msgs::Vector3</b> </p><div class="fragment"><div class="line"># This represents a vector in free space. </div><div class="line"># It is only meant to represent a direction. Therefore, it does not</div><div class="line"># make sense to apply a translation to it (e.g., when applying a </div><div class="line"># generic rigid transformation to a Vector3, tf2 will only apply the</div><div class="line"># rotation). If you want your data to be translatable too, use the</div><div class="line"># geometry_msgs/Point message instead.</div><div class="line"></div><div class="line">float64 x</div><div class="line">float64 y</div><div class="line">float64 z</div></div><!-- fragment --><p><b>Example</b> </p><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> # this example makes the following assumptions about the hardware</div><div class="line"><a name="l00002"></a><span class="lineno"> 2</span> # the servos have been connected to the PWM board starting with the first connector and proceeding in order</div><div class="line"><a name="l00003"></a><span class="lineno"> 3</span> # any drive servos used for the left side servos are mounted opposite of those for the right side</div><div class="line"><a name="l00004"></a><span class="lineno"> 4</span> </div><div class="line"><a name="l00005"></a><span class="lineno"> 5</span> # ROS used m/s (meters per second) as th standard unit for velocity.</div><div class="line"><a name="l00006"></a><span class="lineno"> 6</span> # The geometry_msgs::Twist linear and angular vectors are in m/s and radians/s respectively.</div><div class="line"><a name="l00007"></a><span class="lineno"> 7</span> </div><div class="line"><a name="l00008"></a><span class="lineno"> 8</span> # a typical high-speed servo is capable of 0.16-0.2 sec/60deg or aproximately 50-65 RPM.</div><div class="line"><a name="l00009"></a><span class="lineno"> 9</span> # using wheel with 11 cm diameter results in a circumference of 0.35 meters</div><div class="line"><a name="l00010"></a><span class="lineno"> 10</span> # the theoretical maximum speed of this combination is 0.30 m/s - 0.40 m/s</div><div class="line"><a name="l00011"></a><span class="lineno"> 11</span> </div><div class="line"><a name="l00012"></a><span class="lineno"> 12</span> </div><div class="line"><a name="l00013"></a><span class="lineno"> 13</span> # differential drive example</div><div class="line"><a name="l00014"></a><span class="lineno"> 14</span> # this example uses 2 servos</div><div class="line"><a name="l00015"></a><span class="lineno"> 15</span> # the first servo is the left and the second servo is the right</div><div class="line"><a name="l00016"></a><span class="lineno"> 16</span> </div><div class="line"><a name="l00017"></a><span class="lineno"> 17</span> # configure drive mode for two RC servos attached to 110mm diameter wheels </div><div class="line"><a name="l00018"></a><span class="lineno"> 18</span> </div><div class="line"><a name="l00019"></a><span class="lineno"> 19</span> rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \</div><div class="line"><a name="l00020"></a><span class="lineno"> 20</span>  {servo: 2, center: 336, range: 108, direction: 1}]"</div><div class="line"><a name="l00021"></a><span class="lineno"> 21</span> </div><div class="line"><a name="l00022"></a><span class="lineno"> 22</span> rosservice call /config_drive_mode "{mode: differential, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \</div><div class="line"><a name="l00023"></a><span class="lineno"> 23</span>  servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"</div><div class="line"><a name="l00024"></a><span class="lineno"> 24</span> </div><div class="line"><a name="l00025"></a><span class="lineno"> 25</span> # moving forward at 0.35 m/s (or best speed)</div><div class="line"><a name="l00026"></a><span class="lineno"> 26</span> </div><div class="line"><a name="l00027"></a><span class="lineno"> 27</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"</div><div class="line"><a name="l00028"></a><span class="lineno"> 28</span> </div><div class="line"><a name="l00029"></a><span class="lineno"> 29</span> # left 45 degrees per second turn while moving forward at 0.35 m/s (or best speed)</div><div class="line"><a name="l00030"></a><span class="lineno"> 30</span> </div><div class="line"><a name="l00031"></a><span class="lineno"> 31</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, -0.7854]}"</div><div class="line"><a name="l00032"></a><span class="lineno"> 32</span> </div><div class="line"><a name="l00033"></a><span class="lineno"> 33</span> # pivoting clockwise at 90 degrees per second</div><div class="line"><a name="l00034"></a><span class="lineno"> 34</span> </div><div class="line"><a name="l00035"></a><span class="lineno"> 35</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"</div><div class="line"><a name="l00036"></a><span class="lineno"> 36</span> </div><div class="line"><a name="l00037"></a><span class="lineno"> 37</span> </div><div class="line"><a name="l00038"></a><span class="lineno"> 38</span> # a mecanum drive example</div><div class="line"><a name="l00039"></a><span class="lineno"> 39</span> # this example assumes there are 4 servos</div><div class="line"><a name="l00040"></a><span class="lineno"> 40</span> # the servos are 1, 2, 3, & 4 and correspond to left-front, right-front, left-rear, and right-rear</div><div class="line"><a name="l00041"></a><span class="lineno"> 41</span> </div><div class="line"><a name="l00042"></a><span class="lineno"> 42</span> # configure drive mode for four servos</div><div class="line"><a name="l00043"></a><span class="lineno"> 43</span> </div><div class="line"><a name="l00044"></a><span class="lineno"> 44</span> rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: 1}, \</div><div class="line"><a name="l00045"></a><span class="lineno"> 45</span>  {servo: 2, center: 336, range: 108, direction: -1}, \</div><div class="line"><a name="l00046"></a><span class="lineno"> 46</span>  {servo: 3, center: 337, range: 102, direction: -1}, \</div><div class="line"><a name="l00047"></a><span class="lineno"> 47</span>  {servo: 4, center: 330, range: 100, direction: -1}]"</div><div class="line"><a name="l00048"></a><span class="lineno"> 48</span> </div><div class="line"><a name="l00049"></a><span class="lineno"> 49</span> rosservice call /config_drive_mode "{mode: mecanum, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \</div><div class="line"><a name="l00050"></a><span class="lineno"> 50</span>  servos: [{servo: 1, value: 2}, {servo: 2, value: 1}, \</div><div class="line"><a name="l00051"></a><span class="lineno"> 51</span>  {servo: 3, value: 3}, {servo: 4, value: 4}]}" </div><div class="line"><a name="l00052"></a><span class="lineno"> 52</span> </div><div class="line"><a name="l00053"></a><span class="lineno"> 53</span> # moving forward at full speed</div><div class="line"><a name="l00054"></a><span class="lineno"> 54</span> </div><div class="line"><a name="l00055"></a><span class="lineno"> 55</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35.0, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"</div><div class="line"><a name="l00056"></a><span class="lineno"> 56</span> </div><div class="line"><a name="l00057"></a><span class="lineno"> 57</span> # moving forward and to the right while always facing forward</div><div class="line"><a name="l00058"></a><span class="lineno"> 58</span> </div><div class="line"><a name="l00059"></a><span class="lineno"> 59</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.15, 0.0], angular: [0.0, 0.0, 0.0]}"</div><div class="line"><a name="l00060"></a><span class="lineno"> 60</span> </div><div class="line"><a name="l00061"></a><span class="lineno"> 61</span> # right turn while moving forward</div><div class="line"><a name="l00062"></a><span class="lineno"> 62</span> </div><div class="line"><a name="l00063"></a><span class="lineno"> 63</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.7854]}"</div><div class="line"><a name="l00064"></a><span class="lineno"> 64</span> </div><div class="line"><a name="l00065"></a><span class="lineno"> 65</span> # pivoting clockwise</div><div class="line"><a name="l00066"></a><span class="lineno"> 66</span> </div><div class="line"><a name="l00067"></a><span class="lineno"> 67</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"</div><div class="line"><a name="l00068"></a><span class="lineno"> 68</span> </div><div class="line"><a name="l00069"></a><span class="lineno"> 69</span> # moving sideways to the right</div><div class="line"><a name="l00070"></a><span class="lineno"> 70</span> </div><div class="line"><a name="l00071"></a><span class="lineno"> 71</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.2, 0.0], angular: [0.0, 0.0, 0.0]}"</div><div class="line"><a name="l00072"></a><span class="lineno"> 72</span> </div><div class="line"><a name="l00073"></a><span class="lineno"> 73</span> # moving sideways to the left</div><div class="line"><a name="l00074"></a><span class="lineno"> 74</span> </div><div class="line"><a name="l00075"></a><span class="lineno"> 75</span> rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, -0.2, 0.0], angular: [0.0, 0.0, 0.0]}"</div></div><!-- fragment -->
+<p>Definition at line <a class="el" href="i2cpwm__controller_8cpp_source.html#l00976">976</a> of file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+
+</div>
+</div>
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html
new file mode 100644
index 0000000000000000000000000000000000000000..98ba1df6f80b4adc67adc54db0de6b0f6400b05d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html
@@ -0,0 +1,121 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: src/i2cpwm_controller.cpp File Reference</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+<div id="nav-path" class="navpath">
+ <ul>
+<li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li> </ul>
+</div>
+</div><!-- top -->
+<div class="header">
+ <div class="summary">
+<a href="#func-members">Functions</a> </div>
+ <div class="headertitle">
+<div class="title">i2cpwm_controller.cpp File Reference</div> </div>
+</div><!--header-->
+<div class="contents">
+
+<p>controller for I2C interfaced 16 channel PWM boards with PCA9685 chip
+<a href="#details">More...</a></p>
+<div class="textblock"><code>#include <stdio.h></code><br />
+<code>#include <stdlib.h></code><br />
+<code>#include <string.h></code><br />
+<code>#include <math.h></code><br />
+<code>#include <fcntl.h></code><br />
+<code>#include <errno.h></code><br />
+<code>#include <unistd.h></code><br />
+<code>#include <sys/ioctl.h></code><br />
+<code>#include <sys/types.h></code><br />
+<code>#include <sys/stat.h></code><br />
+<code>#include <linux/i2c-dev.h></code><br />
+<code>#include <ros/ros.h></code><br />
+<code>#include <ros/console.h></code><br />
+<code>#include <std_srvs/Empty.h></code><br />
+<code>#include <geometry_msgs/Twist.h></code><br />
+<code>#include "i2cpwm_board/Servo.h"</code><br />
+<code>#include "i2cpwm_board/ServoArray.h"</code><br />
+<code>#include "i2cpwm_board/ServoConfig.h"</code><br />
+<code>#include "i2cpwm_board/ServoConfigArray.h"</code><br />
+<code>#include "i2cpwm_board/ServosConfig.h"</code><br />
+<code>#include "i2cpwm_board/DriveMode.h"</code><br />
+<code>#include "i2cpwm_board/Position.h"</code><br />
+<code>#include "i2cpwm_board/PositionArray.h"</code><br />
+<code>#include "i2cpwm_board/IntValue.h"</code><br />
+</div>
+<p><a href="i2cpwm__controller_8cpp_source.html">Go to the source code of this file.</a></p>
+<table class="memberdecls">
+<tr class="heading"><td colspan="2"><h2 class="groupheader"><a name="func-members"></a>
+Functions</h2></td></tr>
+<tr class="memitem:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">servos_absolute</a> (const i2cpwm_board::ServoArray::ConstPtr &msg)</td></tr>
+<tr class="memdesc:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos in a physical position <a href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">More...</a><br /></td></tr>
+<tr class="separator:gaa3fe8c6f5a98243c302c0d27cea219d9"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:ga1262b2e51072c237c51b6de4cb199e53"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">servos_proportional</a> (const i2cpwm_board::ServoArray::ConstPtr &msg)</td></tr>
+<tr class="memdesc:ga1262b2e51072c237c51b6de4cb199e53"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos in the range of ±1.0 <a href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">More...</a><br /></td></tr>
+<tr class="separator:ga1262b2e51072c237c51b6de4cb199e53"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gae257665eb64139d2720ed332e61507c6"><td class="memItemLeft" align="right" valign="top">void </td><td class="memItemRight" valign="bottom"><a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">servos_drive</a> (const geometry_msgs::Twist::ConstPtr &msg)</td></tr>
+<tr class="memdesc:gae257665eb64139d2720ed332e61507c6"><td class="mdescLeft"> </td><td class="mdescRight">subscriber topic to move servos based on a drive mode <a href="group___topics.html#gae257665eb64139d2720ed332e61507c6">More...</a><br /></td></tr>
+<tr class="separator:gae257665eb64139d2720ed332e61507c6"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">set_pwm_frequency</a> (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)</td></tr>
+<tr class="memdesc:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="mdescLeft"> </td><td class="mdescRight">service to set set PWM frequency <a href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">More...</a><br /></td></tr>
+<tr class="separator:ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">config_servos</a> (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)</td></tr>
+<tr class="memdesc:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="mdescLeft"> </td><td class="mdescRight">store configuration data for servos on the active board <a href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">More...</a><br /></td></tr>
+<tr class="separator:gabf512716d4497c6a4cb7bf9c5d94e2f1"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">config_drive_mode</a> (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)</td></tr>
+<tr class="memdesc:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="mdescLeft"> </td><td class="mdescRight">set drive mode and drive servos <a href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">More...</a><br /></td></tr>
+<tr class="separator:gae3222f92a0cd33b0d0a2fcfd6d563ba9"><td class="memSeparator" colspan="2"> </td></tr>
+<tr class="memitem:ga4583b417d172bde68e75db603da3362e"><td class="memItemLeft" align="right" valign="top">bool </td><td class="memItemRight" valign="bottom"><a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e">stop_servos</a> (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)</td></tr>
+<tr class="memdesc:ga4583b417d172bde68e75db603da3362e"><td class="mdescLeft"> </td><td class="mdescRight">service to stop all servos on all boards <a href="group___services.html#ga4583b417d172bde68e75db603da3362e">More...</a><br /></td></tr>
+<tr class="separator:ga4583b417d172bde68e75db603da3362e"><td class="memSeparator" colspan="2"> </td></tr>
+</table>
+<a name="details" id="details"></a><h2 class="groupheader">Detailed Description</h2>
+<div class="textblock"><p>controller for I2C interfaced 16 channel PWM boards with PCA9685 chip </p>
+<dl class="section author"><dt>Author</dt><dd>Bradan Lane Studio <a href="#" onclick="location.href='mai'+'lto:'+'inf'+'o@'+'bra'+'da'+'nla'+'ne'+'.co'+'m'; return false;">info@<span style="display: none;">.nosp@m.</span>brad<span style="display: none;">.nosp@m.</span>anlan<span style="display: none;">.nosp@m.</span>e.co<span style="display: none;">.nosp@m.</span>m</a> </dd></dl>
+<dl class="section copyright"><dt>Copyright</dt><dd>Copyright (c) 2016, Bradan Lane Studio</dd></dl>
+<p>Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:</p><ul>
+<li>Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.</li>
+<li>Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.</li>
+<li>The name of Bradan Lane, Bradan Lane Studio nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.</li>
+</ul>
+<p>THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BRADAN LANE STUDIOS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</p>
+<p>Please send comments, questions, or patches to <a href="#" onclick="location.href='mai'+'lto:'+'inf'+'o@'+'bra'+'da'+'nla'+'ne'+'.co'+'m'; return false;">info@<span style="display: none;">.nosp@m.</span>brad<span style="display: none;">.nosp@m.</span>anlan<span style="display: none;">.nosp@m.</span>e.co<span style="display: none;">.nosp@m.</span>m</a> </p>
+
+<p>Definition in file <a class="el" href="i2cpwm__controller_8cpp_source.html">i2cpwm_controller.cpp</a>.</p>
+</div></div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html
new file mode 100644
index 0000000000000000000000000000000000000000..97bd64776f9f44264b2dfa047a8380d84e340049
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html
@@ -0,0 +1,59 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: src/i2cpwm_controller.cpp Source File</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+<div id="nav-path" class="navpath">
+ <ul>
+<li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li> </ul>
+</div>
+</div><!-- top -->
+<div class="header">
+ <div class="headertitle">
+<div class="title">i2cpwm_controller.cpp</div> </div>
+</div><!--header-->
+<div class="contents">
+<a href="i2cpwm__controller_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno"> 1</span> </div><div class="line"><a name="l00169"></a><span class="lineno"> 169</span> <span class="preprocessor">#include <stdio.h></span></div><div class="line"><a name="l00170"></a><span class="lineno"> 170</span> <span class="preprocessor">#include <stdlib.h></span></div><div class="line"><a name="l00171"></a><span class="lineno"> 171</span> <span class="preprocessor">#include <string.h></span></div><div class="line"><a name="l00172"></a><span class="lineno"> 172</span> <span class="preprocessor">#include <math.h></span></div><div class="line"><a name="l00173"></a><span class="lineno"> 173</span> <span class="preprocessor">#include <fcntl.h></span></div><div class="line"><a name="l00174"></a><span class="lineno"> 174</span> <span class="preprocessor">#include <errno.h></span></div><div class="line"><a name="l00175"></a><span class="lineno"> 175</span> <span class="preprocessor">#include <unistd.h></span></div><div class="line"><a name="l00176"></a><span class="lineno"> 176</span> <span class="preprocessor">#include <sys/ioctl.h></span></div><div class="line"><a name="l00177"></a><span class="lineno"> 177</span> <span class="preprocessor">#include <sys/types.h></span></div><div class="line"><a name="l00178"></a><span class="lineno"> 178</span> <span class="preprocessor">#include <sys/stat.h></span></div><div class="line"><a name="l00179"></a><span class="lineno"> 179</span> <span class="preprocessor">#include <linux/i2c-dev.h></span></div><div class="line"><a name="l00180"></a><span class="lineno"> 180</span> </div><div class="line"><a name="l00181"></a><span class="lineno"> 181</span> <span class="preprocessor">#include <ros/ros.h></span></div><div class="line"><a name="l00182"></a><span class="lineno"> 182</span> <span class="preprocessor">#include <ros/console.h></span></div><div class="line"><a name="l00183"></a><span class="lineno"> 183</span> </div><div class="line"><a name="l00184"></a><span class="lineno"> 184</span> <span class="comment">// messages used for any service with no parameters</span></div><div class="line"><a name="l00185"></a><span class="lineno"> 185</span> <span class="preprocessor">#include <std_srvs/Empty.h></span></div><div class="line"><a name="l00186"></a><span class="lineno"> 186</span> <span class="comment">// messages used for drive movement topic</span></div><div class="line"><a name="l00187"></a><span class="lineno"> 187</span> <span class="preprocessor">#include <geometry_msgs/Twist.h></span></div><div class="line"><a name="l00188"></a><span class="lineno"> 188</span> </div><div class="line"><a name="l00189"></a><span class="lineno"> 189</span> <span class="comment">// messages used for the absolute and proportional movement topics</span></div><div class="line"><a name="l00190"></a><span class="lineno"> 190</span> <span class="preprocessor">#include "i2cpwm_board/Servo.h"</span></div><div class="line"><a name="l00191"></a><span class="lineno"> 191</span> <span class="preprocessor">#include "i2cpwm_board/ServoArray.h"</span></div><div class="line"><a name="l00192"></a><span class="lineno"> 192</span> <span class="comment">// messages used for the servo setup service</span></div><div class="line"><a name="l00193"></a><span class="lineno"> 193</span> <span class="preprocessor">#include "i2cpwm_board/ServoConfig.h"</span></div><div class="line"><a name="l00194"></a><span class="lineno"> 194</span> <span class="preprocessor">#include "i2cpwm_board/ServoConfigArray.h"</span></div><div class="line"><a name="l00195"></a><span class="lineno"> 195</span> <span class="comment">// request/response of the servo setup service</span></div><div class="line"><a name="l00196"></a><span class="lineno"> 196</span> <span class="preprocessor">#include "i2cpwm_board/ServosConfig.h"</span></div><div class="line"><a name="l00197"></a><span class="lineno"> 197</span> <span class="comment">// request/response of the drive mode service</span></div><div class="line"><a name="l00198"></a><span class="lineno"> 198</span> <span class="preprocessor">#include "i2cpwm_board/DriveMode.h"</span></div><div class="line"><a name="l00199"></a><span class="lineno"> 199</span> <span class="preprocessor">#include "i2cpwm_board/Position.h"</span></div><div class="line"><a name="l00200"></a><span class="lineno"> 200</span> <span class="preprocessor">#include "i2cpwm_board/PositionArray.h"</span></div><div class="line"><a name="l00201"></a><span class="lineno"> 201</span> <span class="comment">// request/response of the integer parameter services</span></div><div class="line"><a name="l00202"></a><span class="lineno"> 202</span> <span class="preprocessor">#include "i2cpwm_board/IntValue.h"</span></div><div class="line"><a name="l00203"></a><span class="lineno"> 203</span> </div><div class="line"><a name="l00204"></a><span class="lineno"> 204</span> </div><div class="line"><a name="l00206"></a><span class="lineno"> 206</span> </div><div class="line"><a name="l00207"></a><span class="lineno"> 207</span> <span class="keyword">typedef</span> <span class="keyword">struct </span>_servo_config {</div><div class="line"><a name="l00208"></a><span class="lineno"> 208</span>  <span class="keywordtype">int</span> center;</div><div class="line"><a name="l00209"></a><span class="lineno"> 209</span>  <span class="keywordtype">int</span> range;</div><div class="line"><a name="l00210"></a><span class="lineno"> 210</span>  <span class="keywordtype">int</span> direction;</div><div class="line"><a name="l00211"></a><span class="lineno"> 211</span>  <span class="keywordtype">int</span> mode_pos;</div><div class="line"><a name="l00212"></a><span class="lineno"> 212</span> } servo_config;</div><div class="line"><a name="l00213"></a><span class="lineno"> 213</span> </div><div class="line"><a name="l00214"></a><span class="lineno"> 214</span> <span class="keyword">typedef</span> <span class="keyword">struct </span>_drive_mode {</div><div class="line"><a name="l00215"></a><span class="lineno"> 215</span>  <span class="keywordtype">int</span> mode;</div><div class="line"><a name="l00216"></a><span class="lineno"> 216</span>  <span class="keywordtype">float</span> rpm;</div><div class="line"><a name="l00217"></a><span class="lineno"> 217</span>  <span class="keywordtype">float</span> radius;</div><div class="line"><a name="l00218"></a><span class="lineno"> 218</span>  <span class="keywordtype">float</span> track;</div><div class="line"><a name="l00219"></a><span class="lineno"> 219</span>  <span class="keywordtype">float</span> scale;</div><div class="line"><a name="l00220"></a><span class="lineno"> 220</span> } drive_mode;</div><div class="line"><a name="l00221"></a><span class="lineno"> 221</span> </div><div class="line"><a name="l00222"></a><span class="lineno"> 222</span> <span class="keyword">enum</span> drive_modes {</div><div class="line"><a name="l00223"></a><span class="lineno"> 223</span>  MODE_UNDEFINED = 0,</div><div class="line"><a name="l00224"></a><span class="lineno"> 224</span>  MODE_ACKERMAN = 1,</div><div class="line"><a name="l00225"></a><span class="lineno"> 225</span>  MODE_DIFFERENTIAL = 2,</div><div class="line"><a name="l00226"></a><span class="lineno"> 226</span>  MODE_MECANUM = 3,</div><div class="line"><a name="l00227"></a><span class="lineno"> 227</span>  MODE_INVALID = 4</div><div class="line"><a name="l00228"></a><span class="lineno"> 228</span> };</div><div class="line"><a name="l00229"></a><span class="lineno"> 229</span> </div><div class="line"><a name="l00230"></a><span class="lineno"> 230</span> <span class="keyword">enum</span> drive_mode_positions {</div><div class="line"><a name="l00231"></a><span class="lineno"> 231</span>  POSITION_UNDEFINED = 0,</div><div class="line"><a name="l00232"></a><span class="lineno"> 232</span>  POSITION_LEFTFRONT = 1,</div><div class="line"><a name="l00233"></a><span class="lineno"> 233</span>  POSITION_RIGHTFRONT = 2,</div><div class="line"><a name="l00234"></a><span class="lineno"> 234</span>  POSITION_LEFTREAR = 3,</div><div class="line"><a name="l00235"></a><span class="lineno"> 235</span>  POSITION_RIGHTREAR = 4,</div><div class="line"><a name="l00236"></a><span class="lineno"> 236</span>  POSITION_INVALID = 5</div><div class="line"><a name="l00237"></a><span class="lineno"> 237</span> };</div><div class="line"><a name="l00238"></a><span class="lineno"> 238</span> </div><div class="line"><a name="l00239"></a><span class="lineno"> 239</span> <span class="preprocessor">#define _BASE_ADDR 0x40</span></div><div class="line"><a name="l00240"></a><span class="lineno"> 240</span> <span class="preprocessor">#ifndef _PI</span></div><div class="line"><a name="l00241"></a><span class="lineno"> 241</span> <span class="preprocessor">#define _PI 3.14159265358979323846</span></div><div class="line"><a name="l00242"></a><span class="lineno"> 242</span> <span class="preprocessor">#endif</span></div><div class="line"><a name="l00243"></a><span class="lineno"> 243</span> <span class="preprocessor">#define _CONST(s) ((char*)(s))</span></div><div class="line"><a name="l00244"></a><span class="lineno"> 244</span> </div><div class="line"><a name="l00245"></a><span class="lineno"> 245</span> <span class="keyword">enum</span> pwm_regs {</div><div class="line"><a name="l00246"></a><span class="lineno"> 246</span>  <span class="comment">// Registers/etc.</span></div><div class="line"><a name="l00247"></a><span class="lineno"> 247</span>  __MODE1 = 0x00,</div><div class="line"><a name="l00248"></a><span class="lineno"> 248</span>  __MODE2 = 0x01,</div><div class="line"><a name="l00249"></a><span class="lineno"> 249</span>  __SUBADR1 = 0x02, <span class="comment">// enable sub address 1 support</span></div><div class="line"><a name="l00250"></a><span class="lineno"> 250</span>  __SUBADR2 = 0x03, <span class="comment">// enable sub address 2 support</span></div><div class="line"><a name="l00251"></a><span class="lineno"> 251</span>  __SUBADR3 = 0x04, <span class="comment">// enable sub address 2 support</span></div><div class="line"><a name="l00252"></a><span class="lineno"> 252</span>  __PRESCALE = 0xFE,</div><div class="line"><a name="l00253"></a><span class="lineno"> 253</span>  __CHANNEL_ON_L = 0x06,</div><div class="line"><a name="l00254"></a><span class="lineno"> 254</span>  __CHANNEL_ON_H = 0x07,</div><div class="line"><a name="l00255"></a><span class="lineno"> 255</span>  __CHANNEL_OFF_L = 0x08,</div><div class="line"><a name="l00256"></a><span class="lineno"> 256</span>  __CHANNEL_OFF_H = 0x09,</div><div class="line"><a name="l00257"></a><span class="lineno"> 257</span>  __ALL_CHANNELS_ON_L = 0xFA,</div><div class="line"><a name="l00258"></a><span class="lineno"> 258</span>  __ALL_CHANNELS_ON_H = 0xFB,</div><div class="line"><a name="l00259"></a><span class="lineno"> 259</span>  __ALL_CHANNELS_OFF_L = 0xFC,</div><div class="line"><a name="l00260"></a><span class="lineno"> 260</span>  __ALL_CHANNELS_OFF_H = 0xFD,</div><div class="line"><a name="l00261"></a><span class="lineno"> 261</span>  __RESTART = 0x80,</div><div class="line"><a name="l00262"></a><span class="lineno"> 262</span>  __SLEEP = 0x10, <span class="comment">// enable low power mode</span></div><div class="line"><a name="l00263"></a><span class="lineno"> 263</span>  __ALLCALL = 0x01,</div><div class="line"><a name="l00264"></a><span class="lineno"> 264</span>  __INVRT = 0x10, <span class="comment">// invert the output control logic</span></div><div class="line"><a name="l00265"></a><span class="lineno"> 265</span>  __OUTDRV = 0x04</div><div class="line"><a name="l00266"></a><span class="lineno"> 266</span> };</div><div class="line"><a name="l00267"></a><span class="lineno"> 267</span> </div><div class="line"><a name="l00268"></a><span class="lineno"> 268</span> <span class="preprocessor">#define MAX_BOARDS 62</span></div><div class="line"><a name="l00269"></a><span class="lineno"> 269</span> <span class="preprocessor">#define MAX_SERVOS (16*MAX_BOARDS)</span></div><div class="line"><a name="l00270"></a><span class="lineno"> 270</span> </div><div class="line"><a name="l00271"></a><span class="lineno"> 271</span> servo_config _servo_configs[MAX_SERVOS]; <span class="comment">// we can support up to 62 boards (1..62), each with 16 PWM devices (1..16)</span></div><div class="line"><a name="l00272"></a><span class="lineno"> 272</span> drive_mode _active_drive; <span class="comment">// used when converting Twist geometry to PWM values and which servos are for motion</span></div><div class="line"><a name="l00273"></a><span class="lineno"> 273</span> <span class="keywordtype">int</span> _last_servo = -1;</div><div class="line"><a name="l00274"></a><span class="lineno"> 274</span> </div><div class="line"><a name="l00275"></a><span class="lineno"> 275</span> <span class="keywordtype">int</span> _pwm_boards[MAX_BOARDS]; <span class="comment">// we can support up to 62 boards (1..62)</span></div><div class="line"><a name="l00276"></a><span class="lineno"> 276</span> <span class="keywordtype">int</span> _active_board = 0; <span class="comment">// used to determine if I2C SLAVE change is needed</span></div><div class="line"><a name="l00277"></a><span class="lineno"> 277</span> <span class="keywordtype">int</span> _controller_io_handle; <span class="comment">// linux file handle for I2C</span></div><div class="line"><a name="l00278"></a><span class="lineno"> 278</span> </div><div class="line"><a name="l00279"></a><span class="lineno"> 279</span> <span class="keywordtype">int</span> _pwm_frequency = 50; <span class="comment">// frequency determines the size of a pulse width; higher numbers make RC servos buzz</span></div><div class="line"><a name="l00280"></a><span class="lineno"> 280</span> </div><div class="line"><a name="l00281"></a><span class="lineno"> 281</span> </div><div class="line"><a name="l00283"></a><span class="lineno"> 283</span> </div><div class="line"><a name="l00284"></a><span class="lineno"> 284</span> </div><div class="line"><a name="l00285"></a><span class="lineno"> 285</span> </div><div class="line"><a name="l00286"></a><span class="lineno"> 286</span> <span class="comment">//* ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l00287"></a><span class="lineno"> 287</span> <span class="comment">// local private methods</span></div><div class="line"><a name="l00288"></a><span class="lineno"> 288</span> <span class="comment">//* ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l00289"></a><span class="lineno"> 289</span> </div><div class="line"><a name="l00290"></a><span class="lineno"> 290</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _abs (<span class="keywordtype">float</span> v1)</div><div class="line"><a name="l00291"></a><span class="lineno"> 291</span> {</div><div class="line"><a name="l00292"></a><span class="lineno"> 292</span>  <span class="keywordflow">if</span> (v1 < 0)</div><div class="line"><a name="l00293"></a><span class="lineno"> 293</span>  <span class="keywordflow">return</span> (0 - v1);</div><div class="line"><a name="l00294"></a><span class="lineno"> 294</span>  <span class="keywordflow">return</span> v1;</div><div class="line"><a name="l00295"></a><span class="lineno"> 295</span> }</div><div class="line"><a name="l00296"></a><span class="lineno"> 296</span> </div><div class="line"><a name="l00297"></a><span class="lineno"> 297</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _min (<span class="keywordtype">float</span> v1, <span class="keywordtype">float</span> v2)</div><div class="line"><a name="l00298"></a><span class="lineno"> 298</span> {</div><div class="line"><a name="l00299"></a><span class="lineno"> 299</span>  <span class="keywordflow">if</span> (v1 > v2)</div><div class="line"><a name="l00300"></a><span class="lineno"> 300</span>  <span class="keywordflow">return</span> v2;</div><div class="line"><a name="l00301"></a><span class="lineno"> 301</span>  <span class="keywordflow">return</span> v1;</div><div class="line"><a name="l00302"></a><span class="lineno"> 302</span> }</div><div class="line"><a name="l00303"></a><span class="lineno"> 303</span> </div><div class="line"><a name="l00304"></a><span class="lineno"> 304</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _max (<span class="keywordtype">float</span> v1, <span class="keywordtype">float</span> v2)</div><div class="line"><a name="l00305"></a><span class="lineno"> 305</span> {</div><div class="line"><a name="l00306"></a><span class="lineno"> 306</span>  <span class="keywordflow">if</span> (v1 < v2)</div><div class="line"><a name="l00307"></a><span class="lineno"> 307</span>  <span class="keywordflow">return</span> v2;</div><div class="line"><a name="l00308"></a><span class="lineno"> 308</span>  <span class="keywordflow">return</span> v1;</div><div class="line"><a name="l00309"></a><span class="lineno"> 309</span> }</div><div class="line"><a name="l00310"></a><span class="lineno"> 310</span> </div><div class="line"><a name="l00311"></a><span class="lineno"> 311</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _absmin (<span class="keywordtype">float</span> v1, <span class="keywordtype">float</span> v2)</div><div class="line"><a name="l00312"></a><span class="lineno"> 312</span> {</div><div class="line"><a name="l00313"></a><span class="lineno"> 313</span>  <span class="keywordtype">float</span> a1, a2;</div><div class="line"><a name="l00314"></a><span class="lineno"> 314</span>  <span class="keywordtype">float</span> sign = 1.0;</div><div class="line"><a name="l00315"></a><span class="lineno"> 315</span>  <span class="comment">// if (v1 < 0)</span></div><div class="line"><a name="l00316"></a><span class="lineno"> 316</span>  <span class="comment">// sign = -1.0;</span></div><div class="line"><a name="l00317"></a><span class="lineno"> 317</span>  a1 = _abs(v1);</div><div class="line"><a name="l00318"></a><span class="lineno"> 318</span>  a2 = _abs(v2);</div><div class="line"><a name="l00319"></a><span class="lineno"> 319</span>  <span class="keywordflow">if</span> (a1 > a2)</div><div class="line"><a name="l00320"></a><span class="lineno"> 320</span>  <span class="keywordflow">return</span> (sign * a2);</div><div class="line"><a name="l00321"></a><span class="lineno"> 321</span>  <span class="keywordflow">return</span> v1;</div><div class="line"><a name="l00322"></a><span class="lineno"> 322</span> }</div><div class="line"><a name="l00323"></a><span class="lineno"> 323</span> </div><div class="line"><a name="l00324"></a><span class="lineno"> 324</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _absmax (<span class="keywordtype">float</span> v1, <span class="keywordtype">float</span> v2)</div><div class="line"><a name="l00325"></a><span class="lineno"> 325</span> {</div><div class="line"><a name="l00326"></a><span class="lineno"> 326</span>  <span class="keywordtype">float</span> a1, a2;</div><div class="line"><a name="l00327"></a><span class="lineno"> 327</span>  <span class="keywordtype">float</span> sign = 1.0;</div><div class="line"><a name="l00328"></a><span class="lineno"> 328</span>  <span class="comment">// if (v1 < 0)</span></div><div class="line"><a name="l00329"></a><span class="lineno"> 329</span>  <span class="comment">// sign = -1.0;</span></div><div class="line"><a name="l00330"></a><span class="lineno"> 330</span>  a1 = _abs(v1);</div><div class="line"><a name="l00331"></a><span class="lineno"> 331</span>  a2 = _abs(v2);</div><div class="line"><a name="l00332"></a><span class="lineno"> 332</span>  <span class="keywordflow">if</span> (a1 < a2)</div><div class="line"><a name="l00333"></a><span class="lineno"> 333</span>  <span class="keywordflow">return</span> (sign * a2);</div><div class="line"><a name="l00334"></a><span class="lineno"> 334</span>  <span class="keywordflow">return</span> v1;</div><div class="line"><a name="l00335"></a><span class="lineno"> 335</span> }</div><div class="line"><a name="l00336"></a><span class="lineno"> 336</span> </div><div class="line"><a name="l00337"></a><span class="lineno"> 337</span> </div><div class="line"><a name="l00338"></a><span class="lineno"> 338</span> </div><div class="line"><a name="l00349"></a><span class="lineno"> 349</span> <span class="keyword">static</span> <span class="keywordtype">int</span> _smoothing (<span class="keywordtype">float</span> speed)</div><div class="line"><a name="l00350"></a><span class="lineno"> 350</span> {</div><div class="line"><a name="l00351"></a><span class="lineno"> 351</span>  <span class="comment">/* if smoothing is desired, then remove the commented code */</span></div><div class="line"><a name="l00352"></a><span class="lineno"> 352</span>  <span class="comment">// speed = (cos(_PI*(((float)1.0 - speed))) + 1) / 2;</span></div><div class="line"><a name="l00353"></a><span class="lineno"> 353</span>  <span class="keywordflow">return</span> speed;</div><div class="line"><a name="l00354"></a><span class="lineno"> 354</span> }</div><div class="line"><a name="l00355"></a><span class="lineno"> 355</span>  </div><div class="line"><a name="l00356"></a><span class="lineno"> 356</span> </div><div class="line"><a name="l00363"></a><span class="lineno"> 363</span> <span class="keyword">static</span> <span class="keywordtype">float</span> _convert_mps_to_proportional (<span class="keywordtype">float</span> speed)</div><div class="line"><a name="l00364"></a><span class="lineno"> 364</span> {</div><div class="line"><a name="l00365"></a><span class="lineno"> 365</span>  <span class="comment">/* we use the drive mouter output rpm and wheel radius to compute the conversion */</span></div><div class="line"><a name="l00366"></a><span class="lineno"> 366</span> </div><div class="line"><a name="l00367"></a><span class="lineno"> 367</span>  <span class="keywordtype">float</span> initial, max_rate; <span class="comment">// the max m/s is ((rpm/60) * (2*PI*radius))</span></div><div class="line"><a name="l00368"></a><span class="lineno"> 368</span> </div><div class="line"><a name="l00369"></a><span class="lineno"> 369</span>  initial = speed;</div><div class="line"><a name="l00370"></a><span class="lineno"> 370</span>  </div><div class="line"><a name="l00371"></a><span class="lineno"> 371</span>  <span class="keywordflow">if</span> (_active_drive.rpm <= 0.0) {</div><div class="line"><a name="l00372"></a><span class="lineno"> 372</span>  ROS_ERROR(<span class="stringliteral">"Invalid active drive mode RPM %6.4f :: RPM must be greater than 0"</span>, _active_drive.rpm);</div><div class="line"><a name="l00373"></a><span class="lineno"> 373</span>  <span class="keywordflow">return</span> 0.0;</div><div class="line"><a name="l00374"></a><span class="lineno"> 374</span>  }</div><div class="line"><a name="l00375"></a><span class="lineno"> 375</span>  <span class="keywordflow">if</span> (_active_drive.radius <= 0.0) {</div><div class="line"><a name="l00376"></a><span class="lineno"> 376</span>  ROS_ERROR(<span class="stringliteral">"Invalid active drive mode radius %6.4f :: wheel radius must be greater than 0"</span>, _active_drive.radius);</div><div class="line"><a name="l00377"></a><span class="lineno"> 377</span>  <span class="keywordflow">return</span> 0.0;</div><div class="line"><a name="l00378"></a><span class="lineno"> 378</span>  }</div><div class="line"><a name="l00379"></a><span class="lineno"> 379</span> </div><div class="line"><a name="l00380"></a><span class="lineno"> 380</span>  max_rate = (_active_drive.radius * _PI * 2) * (_active_drive.rpm / 60.0);</div><div class="line"><a name="l00381"></a><span class="lineno"> 381</span> </div><div class="line"><a name="l00382"></a><span class="lineno"> 382</span>  speed = speed / max_rate;</div><div class="line"><a name="l00383"></a><span class="lineno"> 383</span>  <span class="comment">// speed = _absmin (speed, 1.0);</span></div><div class="line"><a name="l00384"></a><span class="lineno"> 384</span> </div><div class="line"><a name="l00385"></a><span class="lineno"> 385</span>  ROS_DEBUG(<span class="stringliteral">"%6.4f = convert_mps_to_proportional ( speed(%6.4f) / ((radus(%6.4f) * pi(%6.4f) * 2) * (rpm(%6.4f) / 60.0)) )"</span>, speed, initial, _active_drive.radius, _PI, _active_drive.rpm);</div><div class="line"><a name="l00386"></a><span class="lineno"> 386</span>  <span class="keywordflow">return</span> speed;</div><div class="line"><a name="l00387"></a><span class="lineno"> 387</span> }</div><div class="line"><a name="l00388"></a><span class="lineno"> 388</span> </div><div class="line"><a name="l00389"></a><span class="lineno"> 389</span> </div><div class="line"><a name="l00390"></a><span class="lineno"> 390</span> </div><div class="line"><a name="l00398"></a><span class="lineno"> 398</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _set_pwm_frequency (<span class="keywordtype">int</span> freq)</div><div class="line"><a name="l00399"></a><span class="lineno"> 399</span> {</div><div class="line"><a name="l00400"></a><span class="lineno"> 400</span>  <span class="keywordtype">int</span> prescale;</div><div class="line"><a name="l00401"></a><span class="lineno"> 401</span>  <span class="keywordtype">char</span> oldmode, newmode;</div><div class="line"><a name="l00402"></a><span class="lineno"> 402</span>  <span class="keywordtype">int</span> res;</div><div class="line"><a name="l00403"></a><span class="lineno"> 403</span> </div><div class="line"><a name="l00404"></a><span class="lineno"> 404</span>  _pwm_frequency = freq; <span class="comment">// save to global</span></div><div class="line"><a name="l00405"></a><span class="lineno"> 405</span>  </div><div class="line"><a name="l00406"></a><span class="lineno"> 406</span>  ROS_DEBUG(<span class="stringliteral">"_set_pwm_frequency prescale"</span>);</div><div class="line"><a name="l00407"></a><span class="lineno"> 407</span>  <span class="keywordtype">float</span> prescaleval = 25000000.0; <span class="comment">// 25MHz</span></div><div class="line"><a name="l00408"></a><span class="lineno"> 408</span>  prescaleval /= 4096.0;</div><div class="line"><a name="l00409"></a><span class="lineno"> 409</span>  prescaleval /= (float)freq;</div><div class="line"><a name="l00410"></a><span class="lineno"> 410</span>  prescaleval -= 1.0;</div><div class="line"><a name="l00411"></a><span class="lineno"> 411</span>  <span class="comment">//ROS_INFO("Estimated pre-scale: %6.4f", prescaleval);</span></div><div class="line"><a name="l00412"></a><span class="lineno"> 412</span>  prescale = floor(prescaleval + 0.5);</div><div class="line"><a name="l00413"></a><span class="lineno"> 413</span>  <span class="comment">// ROS_INFO("Final pre-scale: %d", prescale);</span></div><div class="line"><a name="l00414"></a><span class="lineno"> 414</span> </div><div class="line"><a name="l00415"></a><span class="lineno"> 415</span> </div><div class="line"><a name="l00416"></a><span class="lineno"> 416</span>  ROS_INFO(<span class="stringliteral">"Setting PWM frequency to %d Hz"</span>, freq);</div><div class="line"><a name="l00417"></a><span class="lineno"> 417</span> </div><div class="line"><a name="l00418"></a><span class="lineno"> 418</span>  nanosleep ((<span class="keyword">const</span> <span class="keyword">struct</span> timespec[]){{1, 000000L}}, NULL); </div><div class="line"><a name="l00419"></a><span class="lineno"> 419</span> </div><div class="line"><a name="l00420"></a><span class="lineno"> 420</span> </div><div class="line"><a name="l00421"></a><span class="lineno"> 421</span>  oldmode = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);</div><div class="line"><a name="l00422"></a><span class="lineno"> 422</span>  newmode = (oldmode & 0x7F) | 0x10; <span class="comment">// sleep</span></div><div class="line"><a name="l00423"></a><span class="lineno"> 423</span> </div><div class="line"><a name="l00424"></a><span class="lineno"> 424</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, newmode)) <span class="comment">// go to sleep</span></div><div class="line"><a name="l00425"></a><span class="lineno"> 425</span>  ROS_ERROR(<span class="stringliteral">"Unable to set PWM controller to sleep mode"</span>); </div><div class="line"><a name="l00426"></a><span class="lineno"> 426</span> </div><div class="line"><a name="l00427"></a><span class="lineno"> 427</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data(_controller_io_handle, __PRESCALE, (<span class="keywordtype">int</span>)(floor(prescale))))</div><div class="line"><a name="l00428"></a><span class="lineno"> 428</span>  ROS_ERROR(<span class="stringliteral">"Unable to set PWM controller prescale"</span>); </div><div class="line"><a name="l00429"></a><span class="lineno"> 429</span> </div><div class="line"><a name="l00430"></a><span class="lineno"> 430</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode))</div><div class="line"><a name="l00431"></a><span class="lineno"> 431</span>  ROS_ERROR(<span class="stringliteral">"Unable to set PWM controller to active mode"</span>); </div><div class="line"><a name="l00432"></a><span class="lineno"> 432</span> </div><div class="line"><a name="l00433"></a><span class="lineno"> 433</span>  nanosleep((<span class="keyword">const</span> <span class="keyword">struct</span> timespec[]){{0, 5000000L}}, NULL); <span class="comment">//sleep 5microsec,</span></div><div class="line"><a name="l00434"></a><span class="lineno"> 434</span> </div><div class="line"><a name="l00435"></a><span class="lineno"> 435</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode | 0x80))</div><div class="line"><a name="l00436"></a><span class="lineno"> 436</span>  ROS_ERROR(<span class="stringliteral">"Unable to restore PWM controller to active mode"</span>);</div><div class="line"><a name="l00437"></a><span class="lineno"> 437</span> }</div><div class="line"><a name="l00438"></a><span class="lineno"> 438</span> </div><div class="line"><a name="l00439"></a><span class="lineno"> 439</span> </div><div class="line"><a name="l00440"></a><span class="lineno"> 440</span> </div><div class="line"><a name="l00449"></a><span class="lineno"> 449</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _set_pwm_interval_all (<span class="keywordtype">int</span> start, <span class="keywordtype">int</span> end)</div><div class="line"><a name="l00450"></a><span class="lineno"> 450</span> {</div><div class="line"><a name="l00451"></a><span class="lineno"> 451</span>  <span class="comment">// the public API is ONE based and hardware is ZERO based</span></div><div class="line"><a name="l00452"></a><span class="lineno"> 452</span>  <span class="keywordflow">if</span> ((_active_board<1) || (_active_board>62)) {</div><div class="line"><a name="l00453"></a><span class="lineno"> 453</span>  ROS_ERROR(<span class="stringliteral">"Internal error - invalid active board number %d :: PWM board numbers must be between 1 and 62"</span>, _active_board);</div><div class="line"><a name="l00454"></a><span class="lineno"> 454</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00455"></a><span class="lineno"> 455</span>  }</div><div class="line"><a name="l00456"></a><span class="lineno"> 456</span>  <span class="keywordtype">int</span> board = _active_board - 1;</div><div class="line"><a name="l00457"></a><span class="lineno"> 457</span> </div><div class="line"><a name="l00458"></a><span class="lineno"> 458</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_L, start & 0xFF))</div><div class="line"><a name="l00459"></a><span class="lineno"> 459</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM start low byte for all servos on board %d"</span>, _active_board);</div><div class="line"><a name="l00460"></a><span class="lineno"> 460</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_H, start >> 8))</div><div class="line"><a name="l00461"></a><span class="lineno"> 461</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM start high byte for all servos on board %d"</span>, _active_board);</div><div class="line"><a name="l00462"></a><span class="lineno"> 462</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_L, end & 0xFF))</div><div class="line"><a name="l00463"></a><span class="lineno"> 463</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM end low byte for all servos on board %d"</span>, _active_board);</div><div class="line"><a name="l00464"></a><span class="lineno"> 464</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_H, end >> 8))</div><div class="line"><a name="l00465"></a><span class="lineno"> 465</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM end high byte for all servos on board %d"</span>, _active_board);</div><div class="line"><a name="l00466"></a><span class="lineno"> 466</span> }</div><div class="line"><a name="l00467"></a><span class="lineno"> 467</span> </div><div class="line"><a name="l00468"></a><span class="lineno"> 468</span> </div><div class="line"><a name="l00469"></a><span class="lineno"> 469</span> </div><div class="line"><a name="l00476"></a><span class="lineno"> 476</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _set_active_board (<span class="keywordtype">int</span> board)</div><div class="line"><a name="l00477"></a><span class="lineno"> 477</span> {</div><div class="line"><a name="l00478"></a><span class="lineno"> 478</span>  <span class="keywordtype">char</span> mode1res;</div><div class="line"><a name="l00479"></a><span class="lineno"> 479</span> </div><div class="line"><a name="l00480"></a><span class="lineno"> 480</span>  <span class="keywordflow">if</span> ((board<1) || (board>62)) {</div><div class="line"><a name="l00481"></a><span class="lineno"> 481</span>  ROS_ERROR(<span class="stringliteral">"Invalid board number %d :: board numbers must be between 1 and 62"</span>, board);</div><div class="line"><a name="l00482"></a><span class="lineno"> 482</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00483"></a><span class="lineno"> 483</span>  }</div><div class="line"><a name="l00484"></a><span class="lineno"> 484</span>  <span class="keywordflow">if</span> (_active_board != board) {</div><div class="line"><a name="l00485"></a><span class="lineno"> 485</span>  _active_board = board; <span class="comment">// save to global</span></div><div class="line"><a name="l00486"></a><span class="lineno"> 486</span>  </div><div class="line"><a name="l00487"></a><span class="lineno"> 487</span>  <span class="comment">// the public API is ONE based and hardware is ZERO based</span></div><div class="line"><a name="l00488"></a><span class="lineno"> 488</span>  board--;</div><div class="line"><a name="l00489"></a><span class="lineno"> 489</span>  </div><div class="line"><a name="l00490"></a><span class="lineno"> 490</span>  <span class="keywordflow">if</span> (0 > ioctl (_controller_io_handle, I2C_SLAVE, (_BASE_ADDR+(board)))) {</div><div class="line"><a name="l00491"></a><span class="lineno"> 491</span>  ROS_FATAL (<span class="stringliteral">"Failed to acquire bus access and/or talk to I2C slave at address 0x%02X"</span>, (_BASE_ADDR+board));</div><div class="line"><a name="l00492"></a><span class="lineno"> 492</span>  <span class="keywordflow">return</span>; <span class="comment">/* exit(1) */</span> <span class="comment">/* additional ERROR HANDLING information is available with 'errno' */</span></div><div class="line"><a name="l00493"></a><span class="lineno"> 493</span>  }</div><div class="line"><a name="l00494"></a><span class="lineno"> 494</span> </div><div class="line"><a name="l00495"></a><span class="lineno"> 495</span>  <span class="keywordflow">if</span> (_pwm_boards[board]<0) {</div><div class="line"><a name="l00496"></a><span class="lineno"> 496</span>  _pwm_boards[board] = 1;</div><div class="line"><a name="l00497"></a><span class="lineno"> 497</span> </div><div class="line"><a name="l00498"></a><span class="lineno"> 498</span>  <span class="comment">/* this is guess but I believe the following needs to be done on each board only once */</span></div><div class="line"><a name="l00499"></a><span class="lineno"> 499</span> </div><div class="line"><a name="l00500"></a><span class="lineno"> 500</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE2, __OUTDRV))</div><div class="line"><a name="l00501"></a><span class="lineno"> 501</span>  ROS_ERROR (<span class="stringliteral">"Failed to enable PWM outputs for totem-pole structure"</span>);</div><div class="line"><a name="l00502"></a><span class="lineno"> 502</span> </div><div class="line"><a name="l00503"></a><span class="lineno"> 503</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, __ALLCALL))</div><div class="line"><a name="l00504"></a><span class="lineno"> 504</span>  ROS_ERROR (<span class="stringliteral">"Failed to enable ALLCALL for PWM channels"</span>);</div><div class="line"><a name="l00505"></a><span class="lineno"> 505</span> </div><div class="line"><a name="l00506"></a><span class="lineno"> 506</span>  nanosleep ((<span class="keyword">const</span> <span class="keyword">struct</span> timespec[]){{0, 5000000L}}, NULL); <span class="comment">//sleep 5microsec, wait for osci</span></div><div class="line"><a name="l00507"></a><span class="lineno"> 507</span> </div><div class="line"><a name="l00508"></a><span class="lineno"> 508</span> </div><div class="line"><a name="l00509"></a><span class="lineno"> 509</span>  mode1res = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);</div><div class="line"><a name="l00510"></a><span class="lineno"> 510</span>  mode1res = mode1res & ~__SLEEP; <span class="comment">// # wake up (reset sleep)</span></div><div class="line"><a name="l00511"></a><span class="lineno"> 511</span> </div><div class="line"><a name="l00512"></a><span class="lineno"> 512</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, mode1res))</div><div class="line"><a name="l00513"></a><span class="lineno"> 513</span>  ROS_ERROR (<span class="stringliteral">"Failed to recover from low power mode"</span>);</div><div class="line"><a name="l00514"></a><span class="lineno"> 514</span> </div><div class="line"><a name="l00515"></a><span class="lineno"> 515</span>  nanosleep((<span class="keyword">const</span> <span class="keyword">struct</span> timespec[]){{0, 5000000L}}, NULL); <span class="comment">//sleep 5microsec, wait for osci</span></div><div class="line"><a name="l00516"></a><span class="lineno"> 516</span> </div><div class="line"><a name="l00517"></a><span class="lineno"> 517</span>  <span class="comment">// the first time we activate a board, we mark it and set all of its servo channels to 0</span></div><div class="line"><a name="l00518"></a><span class="lineno"> 518</span>  _set_pwm_interval_all (0, 0);</div><div class="line"><a name="l00519"></a><span class="lineno"> 519</span>  }</div><div class="line"><a name="l00520"></a><span class="lineno"> 520</span>  }</div><div class="line"><a name="l00521"></a><span class="lineno"> 521</span> }</div><div class="line"><a name="l00522"></a><span class="lineno"> 522</span> </div><div class="line"><a name="l00523"></a><span class="lineno"> 523</span> </div><div class="line"><a name="l00524"></a><span class="lineno"> 524</span> </div><div class="line"><a name="l00534"></a><span class="lineno"> 534</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _set_pwm_interval (<span class="keywordtype">int</span> servo, <span class="keywordtype">int</span> start, <span class="keywordtype">int</span> end)</div><div class="line"><a name="l00535"></a><span class="lineno"> 535</span> {</div><div class="line"><a name="l00536"></a><span class="lineno"> 536</span>  ROS_DEBUG(<span class="stringliteral">"_set_pwm_interval enter"</span>);</div><div class="line"><a name="l00537"></a><span class="lineno"> 537</span> </div><div class="line"><a name="l00538"></a><span class="lineno"> 538</span>  <span class="keywordflow">if</span> ((servo<1) || (servo>(MAX_SERVOS))) {</div><div class="line"><a name="l00539"></a><span class="lineno"> 539</span>  ROS_ERROR(<span class="stringliteral">"Invalid servo number %d :: servo numbers must be between 1 and %d"</span>, servo, MAX_BOARDS);</div><div class="line"><a name="l00540"></a><span class="lineno"> 540</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00541"></a><span class="lineno"> 541</span>  }</div><div class="line"><a name="l00542"></a><span class="lineno"> 542</span> </div><div class="line"><a name="l00543"></a><span class="lineno"> 543</span>  <span class="keywordtype">int</span> board = ((int)(servo/16))+1;</div><div class="line"><a name="l00544"></a><span class="lineno"> 544</span>  _set_active_board(board);</div><div class="line"><a name="l00545"></a><span class="lineno"> 545</span> </div><div class="line"><a name="l00546"></a><span class="lineno"> 546</span>  servo = servo % 16;</div><div class="line"><a name="l00547"></a><span class="lineno"> 547</span> </div><div class="line"><a name="l00548"></a><span class="lineno"> 548</span>  board = _active_board - 1;</div><div class="line"><a name="l00549"></a><span class="lineno"> 549</span> </div><div class="line"><a name="l00550"></a><span class="lineno"> 550</span>  ROS_DEBUG(<span class="stringliteral">"_set_pwm_interval board=%d"</span>, board);</div><div class="line"><a name="l00551"></a><span class="lineno"> 551</span>  <span class="comment">// the public API is ONE based and hardware is ZERO based</span></div><div class="line"><a name="l00552"></a><span class="lineno"> 552</span> </div><div class="line"><a name="l00553"></a><span class="lineno"> 553</span>  <span class="keywordtype">int</span> channel = servo - 1;</div><div class="line"><a name="l00554"></a><span class="lineno"> 554</span>  </div><div class="line"><a name="l00555"></a><span class="lineno"> 555</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_L+4*channel, start & 0xFF))</div><div class="line"><a name="l00556"></a><span class="lineno"> 556</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM start low byte on servo %d on board %d"</span>, servo, _active_board);</div><div class="line"><a name="l00557"></a><span class="lineno"> 557</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_H+4*channel, start >> 8))</div><div class="line"><a name="l00558"></a><span class="lineno"> 558</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM start high byte on servo %d on board %d"</span>, servo, _active_board);</div><div class="line"><a name="l00559"></a><span class="lineno"> 559</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_L+4*channel, end & 0xFF))</div><div class="line"><a name="l00560"></a><span class="lineno"> 560</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM end low byte on servo %d on board %d"</span>, servo, _active_board);</div><div class="line"><a name="l00561"></a><span class="lineno"> 561</span>  <span class="keywordflow">if</span> (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_H+4*channel, end >> 8))</div><div class="line"><a name="l00562"></a><span class="lineno"> 562</span>  ROS_ERROR (<span class="stringliteral">"Error setting PWM end high byte on servo %d on board %d"</span>, servo, _active_board);</div><div class="line"><a name="l00563"></a><span class="lineno"> 563</span> }</div><div class="line"><a name="l00564"></a><span class="lineno"> 564</span> </div><div class="line"><a name="l00565"></a><span class="lineno"> 565</span> </div><div class="line"><a name="l00566"></a><span class="lineno"> 566</span> </div><div class="line"><a name="l00575"></a><span class="lineno"> 575</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _set_pwm_interval_proportional (<span class="keywordtype">int</span> servo, <span class="keywordtype">float</span> value)</div><div class="line"><a name="l00576"></a><span class="lineno"> 576</span> {</div><div class="line"><a name="l00577"></a><span class="lineno"> 577</span>  <span class="keywordflow">if</span> ((value < -1.0) || (value > 1.0)) {</div><div class="line"><a name="l00578"></a><span class="lineno"> 578</span>  ROS_ERROR(<span class="stringliteral">"Invalid proportion value %f :: proportion values must be between -1.0 and 1.0"</span>, value);</div><div class="line"><a name="l00579"></a><span class="lineno"> 579</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00580"></a><span class="lineno"> 580</span>  }</div><div class="line"><a name="l00581"></a><span class="lineno"> 581</span> </div><div class="line"><a name="l00582"></a><span class="lineno"> 582</span>  servo_config* configp = &(_servo_configs[servo-1]);</div><div class="line"><a name="l00583"></a><span class="lineno"> 583</span>  </div><div class="line"><a name="l00584"></a><span class="lineno"> 584</span>  <span class="keywordflow">if</span> ((configp->center < 0) ||(configp->range < 0)) {</div><div class="line"><a name="l00585"></a><span class="lineno"> 585</span>  ROS_ERROR(<span class="stringliteral">"Missing servo configuration for servo[%d]"</span>, servo);</div><div class="line"><a name="l00586"></a><span class="lineno"> 586</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00587"></a><span class="lineno"> 587</span>  }</div><div class="line"><a name="l00588"></a><span class="lineno"> 588</span> </div><div class="line"><a name="l00589"></a><span class="lineno"> 589</span>  <span class="keywordtype">int</span> pos = (configp->direction * (((float)(configp->range) / 2) * value)) + configp->center;</div><div class="line"><a name="l00590"></a><span class="lineno"> 590</span>  </div><div class="line"><a name="l00591"></a><span class="lineno"> 591</span>  if ((pos < 0) || (pos > 4096)) {</div><div class="line"><a name="l00592"></a><span class="lineno"> 592</span>  ROS_ERROR(<span class="stringliteral">"Invalid computed position servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d"</span>, servo, configp->direction, configp->range, value, configp->center, pos);</div><div class="line"><a name="l00593"></a><span class="lineno"> 593</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00594"></a><span class="lineno"> 594</span>  }</div><div class="line"><a name="l00595"></a><span class="lineno"> 595</span>  _set_pwm_interval (servo, 0, pos);</div><div class="line"><a name="l00596"></a><span class="lineno"> 596</span>  ROS_DEBUG(<span class="stringliteral">"servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d"</span>, servo, configp->direction, configp->range, value, configp->center, pos);</div><div class="line"><a name="l00597"></a><span class="lineno"> 597</span> }</div><div class="line"><a name="l00598"></a><span class="lineno"> 598</span> </div><div class="line"><a name="l00599"></a><span class="lineno"> 599</span> </div><div class="line"><a name="l00600"></a><span class="lineno"> 600</span> </div><div class="line"><a name="l00610"></a><span class="lineno"> 610</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _config_servo (<span class="keywordtype">int</span> servo, <span class="keywordtype">int</span> center, <span class="keywordtype">int</span> range, <span class="keywordtype">int</span> direction)</div><div class="line"><a name="l00611"></a><span class="lineno"> 611</span> {</div><div class="line"><a name="l00612"></a><span class="lineno"> 612</span>  <span class="keywordflow">if</span> ((servo < 1) || (servo > (MAX_SERVOS))) {</div><div class="line"><a name="l00613"></a><span class="lineno"> 613</span>  ROS_ERROR(<span class="stringliteral">"Invalid servo number %d :: servo numbers must be between 1 and %d"</span>, servo, MAX_SERVOS);</div><div class="line"><a name="l00614"></a><span class="lineno"> 614</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00615"></a><span class="lineno"> 615</span>  }</div><div class="line"><a name="l00616"></a><span class="lineno"> 616</span> </div><div class="line"><a name="l00617"></a><span class="lineno"> 617</span>  <span class="keywordflow">if</span> ((center < 0) || (center > 4096))</div><div class="line"><a name="l00618"></a><span class="lineno"> 618</span>  ROS_ERROR(<span class="stringliteral">"Invalid center value %d :: center values must be between 0 and 4096"</span>, center);</div><div class="line"><a name="l00619"></a><span class="lineno"> 619</span> </div><div class="line"><a name="l00620"></a><span class="lineno"> 620</span>  <span class="keywordflow">if</span> ((center < 0) || (center > 4096))</div><div class="line"><a name="l00621"></a><span class="lineno"> 621</span>  ROS_ERROR(<span class="stringliteral">"Invalid range value %d :: range values must be between 0 and 4096"</span>, range);</div><div class="line"><a name="l00622"></a><span class="lineno"> 622</span> </div><div class="line"><a name="l00623"></a><span class="lineno"> 623</span>  <span class="keywordflow">if</span> (((center - (range/2)) < 0) || (((range/2) + center) > 4096))</div><div class="line"><a name="l00624"></a><span class="lineno"> 624</span>  ROS_ERROR(<span class="stringliteral">"Invalid range center combination %d ± %d :: range/2 ± center must be between 0 and 4096"</span>, center, (range/2));</div><div class="line"><a name="l00625"></a><span class="lineno"> 625</span> </div><div class="line"><a name="l00626"></a><span class="lineno"> 626</span>  _servo_configs[servo-1].center = center;</div><div class="line"><a name="l00627"></a><span class="lineno"> 627</span>  _servo_configs[servo-1].range = range;</div><div class="line"><a name="l00628"></a><span class="lineno"> 628</span>  _servo_configs[servo-1].direction = direction;</div><div class="line"><a name="l00629"></a><span class="lineno"> 629</span>  _servo_configs[servo-1].mode_pos = POSITION_UNDEFINED;</div><div class="line"><a name="l00630"></a><span class="lineno"> 630</span> </div><div class="line"><a name="l00631"></a><span class="lineno"> 631</span>  <span class="keywordflow">if</span> (servo > _last_servo) <span class="comment">// used for internal optimizations</span></div><div class="line"><a name="l00632"></a><span class="lineno"> 632</span>  _last_servo = servo;</div><div class="line"><a name="l00633"></a><span class="lineno"> 633</span> </div><div class="line"><a name="l00634"></a><span class="lineno"> 634</span>  ROS_INFO(<span class="stringliteral">"Servo #%d configured: center=%d, range=%d, direction=%d"</span>, servo, center, range, direction);</div><div class="line"><a name="l00635"></a><span class="lineno"> 635</span> }</div><div class="line"><a name="l00636"></a><span class="lineno"> 636</span> </div><div class="line"><a name="l00637"></a><span class="lineno"> 637</span> </div><div class="line"><a name="l00638"></a><span class="lineno"> 638</span> <span class="keyword">static</span> <span class="keywordtype">int</span> _config_servo_position (<span class="keywordtype">int</span> servo, <span class="keywordtype">int</span> position)</div><div class="line"><a name="l00639"></a><span class="lineno"> 639</span> {</div><div class="line"><a name="l00640"></a><span class="lineno"> 640</span>  <span class="keywordflow">if</span> ((servo < 1) || (servo > (MAX_SERVOS))) {</div><div class="line"><a name="l00641"></a><span class="lineno"> 641</span>  ROS_ERROR(<span class="stringliteral">"Invalid servo number %d :: servo numbers must be between 1 and %d"</span>, servo, MAX_SERVOS);</div><div class="line"><a name="l00642"></a><span class="lineno"> 642</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00643"></a><span class="lineno"> 643</span>  }</div><div class="line"><a name="l00644"></a><span class="lineno"> 644</span>  <span class="keywordflow">if</span> ((position < POSITION_UNDEFINED) || (position > POSITION_RIGHTREAR)) {</div><div class="line"><a name="l00645"></a><span class="lineno"> 645</span>  ROS_ERROR(<span class="stringliteral">"Invalid drive mode position %d :: positions are 0 = non-drive, 1 = left front, 2 = right front, 3 = left rear, and 4 = right rear"</span>, position);</div><div class="line"><a name="l00646"></a><span class="lineno"> 646</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00647"></a><span class="lineno"> 647</span>  }</div><div class="line"><a name="l00648"></a><span class="lineno"> 648</span>  _servo_configs[servo-1].mode_pos = position;</div><div class="line"><a name="l00649"></a><span class="lineno"> 649</span>  ROS_INFO(<span class="stringliteral">"Servo #%d configured: position=%d"</span>, servo, position);</div><div class="line"><a name="l00650"></a><span class="lineno"> 650</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00651"></a><span class="lineno"> 651</span> }</div><div class="line"><a name="l00652"></a><span class="lineno"> 652</span> </div><div class="line"><a name="l00653"></a><span class="lineno"> 653</span> </div><div class="line"><a name="l00654"></a><span class="lineno"> 654</span> <span class="keyword">static</span> <span class="keywordtype">int</span> _config_drive_mode (std::string mode, <span class="keywordtype">float</span> rpm, <span class="keywordtype">float</span> radius, <span class="keywordtype">float</span> track, <span class="keywordtype">float</span> scale)</div><div class="line"><a name="l00655"></a><span class="lineno"> 655</span> {</div><div class="line"><a name="l00656"></a><span class="lineno"> 656</span>  <span class="keywordtype">int</span> mode_val = MODE_UNDEFINED;</div><div class="line"><a name="l00657"></a><span class="lineno"> 657</span> </div><div class="line"><a name="l00658"></a><span class="lineno"> 658</span>  <span class="comment">// assumes the parameter was provided in the proper case</span></div><div class="line"><a name="l00659"></a><span class="lineno"> 659</span>  <span class="keywordflow">if</span> (0 == strcmp (mode.c_str(), _CONST(<span class="stringliteral">"ackerman"</span>)))</div><div class="line"><a name="l00660"></a><span class="lineno"> 660</span>  mode_val = MODE_ACKERMAN;</div><div class="line"><a name="l00661"></a><span class="lineno"> 661</span>  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (0 == strcmp (mode.c_str(), _CONST(<span class="stringliteral">"differential"</span>)))</div><div class="line"><a name="l00662"></a><span class="lineno"> 662</span>  mode_val = MODE_DIFFERENTIAL;</div><div class="line"><a name="l00663"></a><span class="lineno"> 663</span>  <span class="keywordflow">else</span> <span class="keywordflow">if</span> (0 == strcmp (mode.c_str(), _CONST(<span class="stringliteral">"mecanum"</span>)))</div><div class="line"><a name="l00664"></a><span class="lineno"> 664</span>  mode_val = MODE_MECANUM;</div><div class="line"><a name="l00665"></a><span class="lineno"> 665</span>  <span class="keywordflow">else</span> {</div><div class="line"><a name="l00666"></a><span class="lineno"> 666</span>  mode_val = MODE_INVALID;</div><div class="line"><a name="l00667"></a><span class="lineno"> 667</span>  ROS_ERROR(<span class="stringliteral">"Invalid drive mode %s :: drive mode must be one of ackerman, differential, or mecanum"</span>, mode.c_str());</div><div class="line"><a name="l00668"></a><span class="lineno"> 668</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00669"></a><span class="lineno"> 669</span>  }</div><div class="line"><a name="l00670"></a><span class="lineno"> 670</span> </div><div class="line"><a name="l00671"></a><span class="lineno"> 671</span>  <span class="keywordflow">if</span> (rpm <= 0.0) {</div><div class="line"><a name="l00672"></a><span class="lineno"> 672</span>  ROS_ERROR(<span class="stringliteral">"Invalid RPM %6.4f :: the motor's output RPM must be greater than 0.0"</span>, rpm);</div><div class="line"><a name="l00673"></a><span class="lineno"> 673</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00674"></a><span class="lineno"> 674</span>  }</div><div class="line"><a name="l00675"></a><span class="lineno"> 675</span> </div><div class="line"><a name="l00676"></a><span class="lineno"> 676</span>  <span class="keywordflow">if</span> (radius <= 0.0) {</div><div class="line"><a name="l00677"></a><span class="lineno"> 677</span>  ROS_ERROR(<span class="stringliteral">"Invalid radius %6.4f :: the wheel radius must be greater than 0.0 meters"</span>, radius);</div><div class="line"><a name="l00678"></a><span class="lineno"> 678</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00679"></a><span class="lineno"> 679</span>  }</div><div class="line"><a name="l00680"></a><span class="lineno"> 680</span> </div><div class="line"><a name="l00681"></a><span class="lineno"> 681</span>  <span class="keywordflow">if</span> (track <= 0.0) {</div><div class="line"><a name="l00682"></a><span class="lineno"> 682</span>  ROS_ERROR(<span class="stringliteral">"Invalid track %6.4f :: the axel track must be greater than 0.0 meters"</span>, track);</div><div class="line"><a name="l00683"></a><span class="lineno"> 683</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00684"></a><span class="lineno"> 684</span>  }</div><div class="line"><a name="l00685"></a><span class="lineno"> 685</span> </div><div class="line"><a name="l00686"></a><span class="lineno"> 686</span>  <span class="keywordflow">if</span> (scale <= 0.0) {</div><div class="line"><a name="l00687"></a><span class="lineno"> 687</span>  ROS_ERROR(<span class="stringliteral">"Invalid scale %6.4f :: the scalar for Twist messages must be greater than 0.0"</span>, scale);</div><div class="line"><a name="l00688"></a><span class="lineno"> 688</span>  <span class="keywordflow">return</span> -1;</div><div class="line"><a name="l00689"></a><span class="lineno"> 689</span>  }</div><div class="line"><a name="l00690"></a><span class="lineno"> 690</span> </div><div class="line"><a name="l00691"></a><span class="lineno"> 691</span>  _active_drive.mode = mode_val;</div><div class="line"><a name="l00692"></a><span class="lineno"> 692</span>  _active_drive.rpm = rpm;</div><div class="line"><a name="l00693"></a><span class="lineno"> 693</span>  _active_drive.radius = radius; <span class="comment">// the service takes the radius in meters</span></div><div class="line"><a name="l00694"></a><span class="lineno"> 694</span>  _active_drive.track = track; <span class="comment">// the service takes the track in meters</span></div><div class="line"><a name="l00695"></a><span class="lineno"> 695</span>  _active_drive.scale = scale;</div><div class="line"><a name="l00696"></a><span class="lineno"> 696</span> </div><div class="line"><a name="l00697"></a><span class="lineno"> 697</span>  ROS_INFO(<span class="stringliteral">"Drive mode configured: mode=%s, rpm=%6.4f, radius=%6.4f, track=%6.4f, scale=%6.4f"</span>, mode.c_str(), rpm, radius, track, scale);</div><div class="line"><a name="l00698"></a><span class="lineno"> 698</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l00699"></a><span class="lineno"> 699</span> }</div><div class="line"><a name="l00700"></a><span class="lineno"> 700</span> </div><div class="line"><a name="l00701"></a><span class="lineno"> 701</span> </div><div class="line"><a name="l00702"></a><span class="lineno"> 702</span> </div><div class="line"><a name="l00710"></a><span class="lineno"> 710</span> <span class="keyword">static</span> <span class="keywordtype">void</span> _init (<span class="keywordtype">char</span>* filename)</div><div class="line"><a name="l00711"></a><span class="lineno"> 711</span> {</div><div class="line"><a name="l00712"></a><span class="lineno"> 712</span>  <span class="keywordtype">int</span> res;</div><div class="line"><a name="l00713"></a><span class="lineno"> 713</span>  <span class="keywordtype">char</span> mode1res;</div><div class="line"><a name="l00714"></a><span class="lineno"> 714</span>  <span class="keywordtype">int</span> i;</div><div class="line"><a name="l00715"></a><span class="lineno"> 715</span> </div><div class="line"><a name="l00716"></a><span class="lineno"> 716</span>  <span class="comment">/* initialize all of the global data objects */</span></div><div class="line"><a name="l00717"></a><span class="lineno"> 717</span>  </div><div class="line"><a name="l00718"></a><span class="lineno"> 718</span>  <span class="keywordflow">for</span> (i=0; i<MAX_BOARDS;i++)</div><div class="line"><a name="l00719"></a><span class="lineno"> 719</span>  _pwm_boards[i] = -1;</div><div class="line"><a name="l00720"></a><span class="lineno"> 720</span>  _active_board = -1;</div><div class="line"><a name="l00721"></a><span class="lineno"> 721</span> </div><div class="line"><a name="l00722"></a><span class="lineno"> 722</span>  <span class="keywordflow">for</span> (i=0; i<(MAX_SERVOS);i++) {</div><div class="line"><a name="l00723"></a><span class="lineno"> 723</span>  <span class="comment">// these values have not useful meaning</span></div><div class="line"><a name="l00724"></a><span class="lineno"> 724</span>  _servo_configs[i].center = -1;</div><div class="line"><a name="l00725"></a><span class="lineno"> 725</span>  _servo_configs[i].range = -1;</div><div class="line"><a name="l00726"></a><span class="lineno"> 726</span>  _servo_configs[i].direction = 1;</div><div class="line"><a name="l00727"></a><span class="lineno"> 727</span>  _servo_configs[i].mode_pos = -1;</div><div class="line"><a name="l00728"></a><span class="lineno"> 728</span>  }</div><div class="line"><a name="l00729"></a><span class="lineno"> 729</span>  _last_servo = -1;</div><div class="line"><a name="l00730"></a><span class="lineno"> 730</span> </div><div class="line"><a name="l00731"></a><span class="lineno"> 731</span>  _active_drive.mode = MODE_UNDEFINED;</div><div class="line"><a name="l00732"></a><span class="lineno"> 732</span>  _active_drive.rpm = -1.0;</div><div class="line"><a name="l00733"></a><span class="lineno"> 733</span>  _active_drive.radius = -1.0;</div><div class="line"><a name="l00734"></a><span class="lineno"> 734</span>  _active_drive.track = -1.0;</div><div class="line"><a name="l00735"></a><span class="lineno"> 735</span>  _active_drive.scale = -1.0;</div><div class="line"><a name="l00736"></a><span class="lineno"> 736</span>  </div><div class="line"><a name="l00737"></a><span class="lineno"> 737</span>  </div><div class="line"><a name="l00738"></a><span class="lineno"> 738</span>  <span class="keywordflow">if</span> ((_controller_io_handle = open (filename, O_RDWR)) < 0) {</div><div class="line"><a name="l00739"></a><span class="lineno"> 739</span>  ROS_FATAL (<span class="stringliteral">"Failed to open I2C bus %s"</span>, filename);</div><div class="line"><a name="l00740"></a><span class="lineno"> 740</span>  <span class="keywordflow">return</span>; <span class="comment">/* exit(1) */</span> <span class="comment">/* additional ERROR HANDLING information is available with 'errno' */</span></div><div class="line"><a name="l00741"></a><span class="lineno"> 741</span>  }</div><div class="line"><a name="l00742"></a><span class="lineno"> 742</span> }</div><div class="line"><a name="l00743"></a><span class="lineno"> 743</span> </div><div class="line"><a name="l00744"></a><span class="lineno"> 744</span> </div><div class="line"><a name="l00745"></a><span class="lineno"> 745</span> </div><div class="line"><a name="l00746"></a><span class="lineno"> 746</span> <span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l00750"></a><span class="lineno"> 750</span> <span class="comment"></span><span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l00751"></a><span class="lineno"> 751</span> </div><div class="line"><a name="l00797"></a><span class="lineno"><a class="line" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9"> 797</a></span> <span class="keywordtype">void</span> <a class="code" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">servos_absolute</a> (<span class="keyword">const</span> i2cpwm_board::ServoArray::ConstPtr& msg)</div><div class="line"><a name="l00798"></a><span class="lineno"> 798</span> {</div><div class="line"><a name="l00799"></a><span class="lineno"> 799</span>  <span class="comment">/* this subscription works on the active_board */</span></div><div class="line"><a name="l00800"></a><span class="lineno"> 800</span>  </div><div class="line"><a name="l00801"></a><span class="lineno"> 801</span>  <span class="keywordflow">for</span>(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {</div><div class="line"><a name="l00802"></a><span class="lineno"> 802</span>  <span class="keywordtype">int</span> servo = sp->servo;</div><div class="line"><a name="l00803"></a><span class="lineno"> 803</span>  <span class="keywordtype">int</span> value = sp->value;</div><div class="line"><a name="l00804"></a><span class="lineno"> 804</span> </div><div class="line"><a name="l00805"></a><span class="lineno"> 805</span>  <span class="keywordflow">if</span> ((value < 0) || (value > 4096)) {</div><div class="line"><a name="l00806"></a><span class="lineno"> 806</span>  ROS_ERROR(<span class="stringliteral">"Invalid PWM value %d :: PWM values must be between 0 and 4096"</span>, value);</div><div class="line"><a name="l00807"></a><span class="lineno"> 807</span>  <span class="keywordflow">continue</span>;</div><div class="line"><a name="l00808"></a><span class="lineno"> 808</span>  }</div><div class="line"><a name="l00809"></a><span class="lineno"> 809</span>  _set_pwm_interval (servo, 0, value);</div><div class="line"><a name="l00810"></a><span class="lineno"> 810</span>  ROS_DEBUG(<span class="stringliteral">"servo[%d] = %d"</span>, servo, value);</div><div class="line"><a name="l00811"></a><span class="lineno"> 811</span>  }</div><div class="line"><a name="l00812"></a><span class="lineno"> 812</span> }</div><div class="line"><a name="l00813"></a><span class="lineno"> 813</span> </div><div class="line"><a name="l00814"></a><span class="lineno"> 814</span> </div><div class="line"><a name="l00867"></a><span class="lineno"><a class="line" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53"> 867</a></span> <span class="keywordtype">void</span> <a class="code" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">servos_proportional</a> (<span class="keyword">const</span> i2cpwm_board::ServoArray::ConstPtr& msg)</div><div class="line"><a name="l00868"></a><span class="lineno"> 868</span> {</div><div class="line"><a name="l00869"></a><span class="lineno"> 869</span>  <span class="comment">/* this subscription works on the active_board */</span></div><div class="line"><a name="l00870"></a><span class="lineno"> 870</span> </div><div class="line"><a name="l00871"></a><span class="lineno"> 871</span>  <span class="keywordflow">for</span>(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {</div><div class="line"><a name="l00872"></a><span class="lineno"> 872</span>  <span class="keywordtype">int</span> servo = sp->servo;</div><div class="line"><a name="l00873"></a><span class="lineno"> 873</span>  <span class="keywordtype">float</span> value = sp->value;</div><div class="line"><a name="l00874"></a><span class="lineno"> 874</span>  _set_pwm_interval_proportional (servo, value);</div><div class="line"><a name="l00875"></a><span class="lineno"> 875</span>  }</div><div class="line"><a name="l00876"></a><span class="lineno"> 876</span> }</div><div class="line"><a name="l00877"></a><span class="lineno"> 877</span> </div><div class="line"><a name="l00878"></a><span class="lineno"> 878</span> </div><div class="line"><a name="l00879"></a><span class="lineno"> 879</span> </div><div class="line"><a name="l00880"></a><span class="lineno"> 880</span> </div><div class="line"><a name="l00976"></a><span class="lineno"><a class="line" href="group___topics.html#gae257665eb64139d2720ed332e61507c6"> 976</a></span> <span class="keywordtype">void</span> <a class="code" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">servos_drive</a> (<span class="keyword">const</span> geometry_msgs::Twist::ConstPtr& msg)</div><div class="line"><a name="l00977"></a><span class="lineno"> 977</span> {</div><div class="line"><a name="l00978"></a><span class="lineno"> 978</span>  <span class="comment">/* this subscription works on the active_board */</span></div><div class="line"><a name="l00979"></a><span class="lineno"> 979</span> </div><div class="line"><a name="l00980"></a><span class="lineno"> 980</span>  <span class="keywordtype">int</span> i;</div><div class="line"><a name="l00981"></a><span class="lineno"> 981</span>  <span class="keywordtype">float</span> delta, range, ratio;</div><div class="line"><a name="l00982"></a><span class="lineno"> 982</span>  <span class="keywordtype">float</span> temp_x, temp_y, temp_r;</div><div class="line"><a name="l00983"></a><span class="lineno"> 983</span>  <span class="keywordtype">float</span> dir_x, dir_y, dir_r;</div><div class="line"><a name="l00984"></a><span class="lineno"> 984</span>  <span class="keywordtype">float</span> speed[4];</div><div class="line"><a name="l00985"></a><span class="lineno"> 985</span>  </div><div class="line"><a name="l00986"></a><span class="lineno"> 986</span>  <span class="comment">/* msg is a pointer to a Twist message: msg->linear and msg->angular each of which have members .x .y .z */</span></div><div class="line"><a name="l00987"></a><span class="lineno"> 987</span>  <span class="comment">/* the subscriber uses the maths from: http://robotsforroboticists.com/drive-kinematics/ */</span> </div><div class="line"><a name="l00988"></a><span class="lineno"> 988</span> </div><div class="line"><a name="l00989"></a><span class="lineno"> 989</span>  ROS_DEBUG(<span class="stringliteral">"servos_drive Twist = [%5.2f %5.2f %5.2f] [%5.2f %5.2f %5.2f]"</span>, </div><div class="line"><a name="l00990"></a><span class="lineno"> 990</span>  msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);</div><div class="line"><a name="l00991"></a><span class="lineno"> 991</span> </div><div class="line"><a name="l00992"></a><span class="lineno"> 992</span>  <span class="keywordflow">if</span> (_active_drive.mode == MODE_UNDEFINED) {</div><div class="line"><a name="l00993"></a><span class="lineno"> 993</span>  ROS_ERROR(<span class="stringliteral">"drive mode not set"</span>);</div><div class="line"><a name="l00994"></a><span class="lineno"> 994</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00995"></a><span class="lineno"> 995</span>  }</div><div class="line"><a name="l00996"></a><span class="lineno"> 996</span>  <span class="keywordflow">if</span> ((_active_drive.mode < MODE_UNDEFINED) || (_active_drive.mode >= MODE_INVALID)) {</div><div class="line"><a name="l00997"></a><span class="lineno"> 997</span>  ROS_ERROR(<span class="stringliteral">"unrecognized drive mode set %d"</span>, _active_drive.mode);</div><div class="line"><a name="l00998"></a><span class="lineno"> 998</span>  <span class="keywordflow">return</span>;</div><div class="line"><a name="l00999"></a><span class="lineno"> 999</span>  }</div><div class="line"><a name="l01000"></a><span class="lineno"> 1000</span> </div><div class="line"><a name="l01001"></a><span class="lineno"> 1001</span>  dir_x = ((msg->linear.x < 0) ? -1 : 1);</div><div class="line"><a name="l01002"></a><span class="lineno"> 1002</span>  dir_y = ((msg->linear.y < 0) ? -1 : 1);</div><div class="line"><a name="l01003"></a><span class="lineno"> 1003</span>  dir_r = ((msg->angular.z < 0) ? -1 : 1);</div><div class="line"><a name="l01004"></a><span class="lineno"> 1004</span> </div><div class="line"><a name="l01005"></a><span class="lineno"> 1005</span>  temp_x = _active_drive.scale * _abs(msg->linear.x);</div><div class="line"><a name="l01006"></a><span class="lineno"> 1006</span>  temp_y = _active_drive.scale * _abs(msg->linear.y);</div><div class="line"><a name="l01007"></a><span class="lineno"> 1007</span>  temp_r = _abs(msg->angular.z);</div><div class="line"><a name="l01008"></a><span class="lineno"> 1008</span>  </div><div class="line"><a name="l01009"></a><span class="lineno"> 1009</span>  <span class="comment">// temp_x = _smoothing (temp_x);</span></div><div class="line"><a name="l01010"></a><span class="lineno"> 1010</span>  <span class="comment">// temp_y = _smoothing (temp_y);</span></div><div class="line"><a name="l01011"></a><span class="lineno"> 1011</span>  <span class="comment">// temp_r = _smoothing (temp_r) / 2;</span></div><div class="line"><a name="l01012"></a><span class="lineno"> 1012</span> </div><div class="line"><a name="l01013"></a><span class="lineno"> 1013</span>  <span class="comment">/* the delta is the angular velocity * half the drive track */</span></div><div class="line"><a name="l01014"></a><span class="lineno"> 1014</span>  delta = (temp_r * (_active_drive.track / 2));</div><div class="line"><a name="l01015"></a><span class="lineno"> 1015</span> </div><div class="line"><a name="l01016"></a><span class="lineno"> 1016</span>  ratio = _convert_mps_to_proportional(temp_y + delta);</div><div class="line"><a name="l01017"></a><span class="lineno"> 1017</span>  <span class="keywordflow">if</span> (ratio > 1.0)</div><div class="line"><a name="l01018"></a><span class="lineno"> 1018</span>  temp_y /= ratio;</div><div class="line"><a name="l01019"></a><span class="lineno"> 1019</span> </div><div class="line"><a name="l01020"></a><span class="lineno"> 1020</span>  </div><div class="line"><a name="l01021"></a><span class="lineno"> 1021</span>  <span class="keywordflow">switch</span> (_active_drive.mode) {</div><div class="line"><a name="l01022"></a><span class="lineno"> 1022</span> </div><div class="line"><a name="l01023"></a><span class="lineno"> 1023</span>  <span class="keywordflow">case</span> MODE_ACKERMAN:</div><div class="line"><a name="l01024"></a><span class="lineno"> 1024</span>  <span class="comment">/*</span></div><div class="line"><a name="l01025"></a><span class="lineno"> 1025</span> <span class="comment"> with ackerman drive, steering is handled by a separate servo</span></div><div class="line"><a name="l01026"></a><span class="lineno"> 1026</span> <span class="comment"> we drive assigned servos exclusively by the linear.x</span></div><div class="line"><a name="l01027"></a><span class="lineno"> 1027</span> <span class="comment"> */</span></div><div class="line"><a name="l01028"></a><span class="lineno"> 1028</span>  speed[0] = temp_x * dir_x;</div><div class="line"><a name="l01029"></a><span class="lineno"> 1029</span>  speed[0] = _convert_mps_to_proportional(speed[0]);</div><div class="line"><a name="l01030"></a><span class="lineno"> 1030</span>  <span class="keywordflow">if</span> (_abs(speed[0]) > 1.0)</div><div class="line"><a name="l01031"></a><span class="lineno"> 1031</span>  speed[0] = 1.0 * dir_x;</div><div class="line"><a name="l01032"></a><span class="lineno"> 1032</span>  </div><div class="line"><a name="l01033"></a><span class="lineno"> 1033</span>  ROS_DEBUG(<span class="stringliteral">"ackerman drive mode speed=%6.4f"</span>, speed[0]);</div><div class="line"><a name="l01034"></a><span class="lineno"> 1034</span>  <span class="keywordflow">break</span>;</div><div class="line"><a name="l01035"></a><span class="lineno"> 1035</span> </div><div class="line"><a name="l01036"></a><span class="lineno"> 1036</span>  <span class="keywordflow">case</span> MODE_DIFFERENTIAL:</div><div class="line"><a name="l01037"></a><span class="lineno"> 1037</span>  <span class="comment">/*</span></div><div class="line"><a name="l01038"></a><span class="lineno"> 1038</span> <span class="comment"> with differential drive, steering is handled by the relative speed of left and right servos</span></div><div class="line"><a name="l01039"></a><span class="lineno"> 1039</span> <span class="comment"> we drive assigned servos by mixing linear.x and angular.z</span></div><div class="line"><a name="l01040"></a><span class="lineno"> 1040</span> <span class="comment"> we compute the delta for left and right components</span></div><div class="line"><a name="l01041"></a><span class="lineno"> 1041</span> <span class="comment"> we use the sign of the angular velocity to determine which is the faster / slower</span></div><div class="line"><a name="l01042"></a><span class="lineno"> 1042</span> <span class="comment"> */</span></div><div class="line"><a name="l01043"></a><span class="lineno"> 1043</span> </div><div class="line"><a name="l01044"></a><span class="lineno"> 1044</span>  <span class="comment">/* the delta is the angular velocity * half the drive track */</span></div><div class="line"><a name="l01045"></a><span class="lineno"> 1045</span>  </div><div class="line"><a name="l01046"></a><span class="lineno"> 1046</span>  <span class="keywordflow">if</span> (dir_r > 0) { <span class="comment">// turning right</span></div><div class="line"><a name="l01047"></a><span class="lineno"> 1047</span>  speed[0] = (temp_y + delta) * dir_y;</div><div class="line"><a name="l01048"></a><span class="lineno"> 1048</span>  speed[1] = (temp_y - delta) * dir_y;</div><div class="line"><a name="l01049"></a><span class="lineno"> 1049</span>  } <span class="keywordflow">else</span> { <span class="comment">// turning left</span></div><div class="line"><a name="l01050"></a><span class="lineno"> 1050</span>  speed[0] = (temp_y - delta) * dir_y;</div><div class="line"><a name="l01051"></a><span class="lineno"> 1051</span>  speed[1] = (temp_y + delta) * dir_y;</div><div class="line"><a name="l01052"></a><span class="lineno"> 1052</span>  }</div><div class="line"><a name="l01053"></a><span class="lineno"> 1053</span> </div><div class="line"><a name="l01054"></a><span class="lineno"> 1054</span>  ROS_DEBUG(<span class="stringliteral">"computed differential drive mode speed left=%6.4f right=%6.4f"</span>, speed[0], speed[1]);</div><div class="line"><a name="l01055"></a><span class="lineno"> 1055</span> </div><div class="line"><a name="l01056"></a><span class="lineno"> 1056</span>  <span class="comment">/* if any of the results are greater that 1.0, we need to scale all the results down */</span></div><div class="line"><a name="l01057"></a><span class="lineno"> 1057</span>  range = _max (_abs(speed[0]), _abs(speed[1]));</div><div class="line"><a name="l01058"></a><span class="lineno"> 1058</span>  </div><div class="line"><a name="l01059"></a><span class="lineno"> 1059</span>  ratio = _convert_mps_to_proportional(range);</div><div class="line"><a name="l01060"></a><span class="lineno"> 1060</span>  <span class="keywordflow">if</span> (ratio > 1.0) {</div><div class="line"><a name="l01061"></a><span class="lineno"> 1061</span>  speed[0] /= ratio;</div><div class="line"><a name="l01062"></a><span class="lineno"> 1062</span>  speed[1] /= ratio;</div><div class="line"><a name="l01063"></a><span class="lineno"> 1063</span>  }</div><div class="line"><a name="l01064"></a><span class="lineno"> 1064</span>  ROS_DEBUG(<span class="stringliteral">"adjusted differential drive mode speed left=%6.4f right=%6.4f"</span>, speed[0], speed[1]);</div><div class="line"><a name="l01065"></a><span class="lineno"> 1065</span> </div><div class="line"><a name="l01066"></a><span class="lineno"> 1066</span>  speed[0] = _convert_mps_to_proportional(speed[0]);</div><div class="line"><a name="l01067"></a><span class="lineno"> 1067</span>  speed[1] = _convert_mps_to_proportional(speed[1]);</div><div class="line"><a name="l01068"></a><span class="lineno"> 1068</span> </div><div class="line"><a name="l01069"></a><span class="lineno"> 1069</span>  ROS_DEBUG(<span class="stringliteral">"differential drive mode speed left=%6.4f right=%6.4f"</span>, speed[0], speed[1]);</div><div class="line"><a name="l01070"></a><span class="lineno"> 1070</span>  <span class="keywordflow">break</span>;</div><div class="line"><a name="l01071"></a><span class="lineno"> 1071</span> </div><div class="line"><a name="l01072"></a><span class="lineno"> 1072</span>  <span class="keywordflow">case</span> MODE_MECANUM:</div><div class="line"><a name="l01073"></a><span class="lineno"> 1073</span>  <span class="comment">/*</span></div><div class="line"><a name="l01074"></a><span class="lineno"> 1074</span> <span class="comment"> with mecanum drive, steering is handled by the relative speed of left and right servos</span></div><div class="line"><a name="l01075"></a><span class="lineno"> 1075</span> <span class="comment"> with mecanum drive, lateral motion is handled by the rotation of front and rear servos</span></div><div class="line"><a name="l01076"></a><span class="lineno"> 1076</span> <span class="comment"> we drive assigned servos by mixing linear.x and angular.z and linear.y</span></div><div class="line"><a name="l01077"></a><span class="lineno"> 1077</span> <span class="comment"> */</span></div><div class="line"><a name="l01078"></a><span class="lineno"> 1078</span> </div><div class="line"><a name="l01079"></a><span class="lineno"> 1079</span>  <span class="keywordflow">if</span> (dir_r > 0) { <span class="comment">// turning right</span></div><div class="line"><a name="l01080"></a><span class="lineno"> 1080</span>  speed[0] = speed[2] = (temp_y + delta) * dir_y;</div><div class="line"><a name="l01081"></a><span class="lineno"> 1081</span>  speed[1] = speed[3] = (temp_y - delta) * dir_y;</div><div class="line"><a name="l01082"></a><span class="lineno"> 1082</span>  } <span class="keywordflow">else</span> { <span class="comment">// turning left</span></div><div class="line"><a name="l01083"></a><span class="lineno"> 1083</span>  speed[0] = speed[2] = (temp_y - delta) * dir_y;</div><div class="line"><a name="l01084"></a><span class="lineno"> 1084</span>  speed[1] = speed[3] = (temp_y + delta) * dir_y;</div><div class="line"><a name="l01085"></a><span class="lineno"> 1085</span>  }</div><div class="line"><a name="l01086"></a><span class="lineno"> 1086</span> </div><div class="line"><a name="l01087"></a><span class="lineno"> 1087</span>  speed[0] += temp_x * dir_x;</div><div class="line"><a name="l01088"></a><span class="lineno"> 1088</span>  speed[3] += temp_x * dir_x;</div><div class="line"><a name="l01089"></a><span class="lineno"> 1089</span>  speed[1] -= temp_x * dir_x;</div><div class="line"><a name="l01090"></a><span class="lineno"> 1090</span>  speed[2] -= temp_x * dir_x;</div><div class="line"><a name="l01091"></a><span class="lineno"> 1091</span>  ROS_DEBUG(<span class="stringliteral">"computed mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f"</span>, speed[0], speed[1], speed[2], speed[3]);</div><div class="line"><a name="l01092"></a><span class="lineno"> 1092</span> </div><div class="line"><a name="l01093"></a><span class="lineno"> 1093</span>  range = _max (_max (_max (_abs(speed[0]), _abs(speed[1])), _abs(speed[2])), _abs(speed[3]));</div><div class="line"><a name="l01094"></a><span class="lineno"> 1094</span>  ratio = _convert_mps_to_proportional(range);</div><div class="line"><a name="l01095"></a><span class="lineno"> 1095</span>  <span class="keywordflow">if</span> (ratio > 1.0) {</div><div class="line"><a name="l01096"></a><span class="lineno"> 1096</span>  speed[0] /= ratio;</div><div class="line"><a name="l01097"></a><span class="lineno"> 1097</span>  speed[1] /= ratio;</div><div class="line"><a name="l01098"></a><span class="lineno"> 1098</span>  speed[2] /= ratio;</div><div class="line"><a name="l01099"></a><span class="lineno"> 1099</span>  speed[3] /= ratio;</div><div class="line"><a name="l01100"></a><span class="lineno"> 1100</span>  }</div><div class="line"><a name="l01101"></a><span class="lineno"> 1101</span>  ROS_DEBUG(<span class="stringliteral">"adjusted mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f"</span>, speed[0], speed[1], speed[2], speed[3]);</div><div class="line"><a name="l01102"></a><span class="lineno"> 1102</span> </div><div class="line"><a name="l01103"></a><span class="lineno"> 1103</span>  speed[0] = _convert_mps_to_proportional(speed[0]);</div><div class="line"><a name="l01104"></a><span class="lineno"> 1104</span>  speed[1] = _convert_mps_to_proportional(speed[1]);</div><div class="line"><a name="l01105"></a><span class="lineno"> 1105</span>  speed[2] = _convert_mps_to_proportional(speed[2]);</div><div class="line"><a name="l01106"></a><span class="lineno"> 1106</span>  speed[3] = _convert_mps_to_proportional(speed[3]);</div><div class="line"><a name="l01107"></a><span class="lineno"> 1107</span> </div><div class="line"><a name="l01108"></a><span class="lineno"> 1108</span>  ROS_DEBUG(<span class="stringliteral">"mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f"</span>, speed[0], speed[1], speed[2], speed[3]);</div><div class="line"><a name="l01109"></a><span class="lineno"> 1109</span>  <span class="keywordflow">break</span>;</div><div class="line"><a name="l01110"></a><span class="lineno"> 1110</span> </div><div class="line"><a name="l01111"></a><span class="lineno"> 1111</span>  <span class="keywordflow">default</span>:</div><div class="line"><a name="l01112"></a><span class="lineno"> 1112</span>  <span class="keywordflow">break</span>;</div><div class="line"><a name="l01113"></a><span class="lineno"> 1113</span> </div><div class="line"><a name="l01114"></a><span class="lineno"> 1114</span>  }</div><div class="line"><a name="l01115"></a><span class="lineno"> 1115</span>  </div><div class="line"><a name="l01116"></a><span class="lineno"> 1116</span>  <span class="comment">/* find all drive servos and set their new speed */</span></div><div class="line"><a name="l01117"></a><span class="lineno"> 1117</span>  <span class="keywordflow">for</span> (i=0; i<(_last_servo); i++) {</div><div class="line"><a name="l01118"></a><span class="lineno"> 1118</span>  <span class="comment">// we use 'fall thru' on the switch statement to allow all necessary servos to be controlled</span></div><div class="line"><a name="l01119"></a><span class="lineno"> 1119</span>  <span class="keywordflow">switch</span> (_active_drive.mode) {</div><div class="line"><a name="l01120"></a><span class="lineno"> 1120</span>  <span class="keywordflow">case</span> MODE_MECANUM:</div><div class="line"><a name="l01121"></a><span class="lineno"> 1121</span>  <span class="keywordflow">if</span> (_servo_configs[i].mode_pos == POSITION_LEFTREAR)</div><div class="line"><a name="l01122"></a><span class="lineno"> 1122</span>  _set_pwm_interval_proportional (i+1, speed[2]);</div><div class="line"><a name="l01123"></a><span class="lineno"> 1123</span>  <span class="keywordflow">if</span> (_servo_configs[i].mode_pos == POSITION_RIGHTREAR)</div><div class="line"><a name="l01124"></a><span class="lineno"> 1124</span>  _set_pwm_interval_proportional (i+1, speed[3]);</div><div class="line"><a name="l01125"></a><span class="lineno"> 1125</span>  <span class="keywordflow">case</span> MODE_DIFFERENTIAL:</div><div class="line"><a name="l01126"></a><span class="lineno"> 1126</span>  <span class="keywordflow">if</span> (_servo_configs[i].mode_pos == POSITION_LEFTFRONT)</div><div class="line"><a name="l01127"></a><span class="lineno"> 1127</span>  _set_pwm_interval_proportional (i+1, speed[0]);</div><div class="line"><a name="l01128"></a><span class="lineno"> 1128</span>  <span class="keywordflow">case</span> MODE_ACKERMAN:</div><div class="line"><a name="l01129"></a><span class="lineno"> 1129</span>  <span class="keywordflow">if</span> (_servo_configs[i].mode_pos == POSITION_RIGHTFRONT)</div><div class="line"><a name="l01130"></a><span class="lineno"> 1130</span>  _set_pwm_interval_proportional (i+1, speed[1]);</div><div class="line"><a name="l01131"></a><span class="lineno"> 1131</span>  }</div><div class="line"><a name="l01132"></a><span class="lineno"> 1132</span>  }</div><div class="line"><a name="l01133"></a><span class="lineno"> 1133</span> }</div><div class="line"><a name="l01134"></a><span class="lineno"> 1134</span> </div><div class="line"><a name="l01135"></a><span class="lineno"> 1135</span> </div><div class="line"><a name="l01136"></a><span class="lineno"> 1136</span> <span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l01141"></a><span class="lineno"> 1141</span> <span class="comment"></span><span class="comment">// services</span></div><div class="line"><a name="l01142"></a><span class="lineno"> 1142</span> <span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l01143"></a><span class="lineno"> 1143</span> </div><div class="line"><a name="l01172"></a><span class="lineno"><a class="line" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85"> 1172</a></span> <span class="keywordtype">bool</span> <a class="code" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">set_pwm_frequency</a> (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)</div><div class="line"><a name="l01173"></a><span class="lineno"> 1173</span> {</div><div class="line"><a name="l01174"></a><span class="lineno"> 1174</span>  <span class="keywordtype">int</span> freq;</div><div class="line"><a name="l01175"></a><span class="lineno"> 1175</span>  freq = req.value;</div><div class="line"><a name="l01176"></a><span class="lineno"> 1176</span>  <span class="keywordflow">if</span> ((freq<12) || (freq>1024)) {</div><div class="line"><a name="l01177"></a><span class="lineno"> 1177</span>  ROS_ERROR(<span class="stringliteral">"Invalid PWM frequency %d :: PWM frequencies should be between 12 and 1024"</span>, freq);</div><div class="line"><a name="l01178"></a><span class="lineno"> 1178</span>  freq = 50; <span class="comment">// most analog RC servos are designed for 20ms pulses.</span></div><div class="line"><a name="l01179"></a><span class="lineno"> 1179</span>  res.error = freq;</div><div class="line"><a name="l01180"></a><span class="lineno"> 1180</span>  }</div><div class="line"><a name="l01181"></a><span class="lineno"> 1181</span>  _set_pwm_frequency (freq); <span class="comment">// I think we must reset frequency when we change boards</span></div><div class="line"><a name="l01182"></a><span class="lineno"> 1182</span>  res.error = freq;</div><div class="line"><a name="l01183"></a><span class="lineno"> 1183</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01184"></a><span class="lineno"> 1184</span> }</div><div class="line"><a name="l01185"></a><span class="lineno"> 1185</span> </div><div class="line"><a name="l01186"></a><span class="lineno"> 1186</span> </div><div class="line"><a name="l01228"></a><span class="lineno"><a class="line" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1"> 1228</a></span> <span class="keywordtype">bool</span> <a class="code" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">config_servos</a> (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)</div><div class="line"><a name="l01229"></a><span class="lineno"> 1229</span> {</div><div class="line"><a name="l01230"></a><span class="lineno"> 1230</span>  <span class="comment">/* this service works on the active_board */</span></div><div class="line"><a name="l01231"></a><span class="lineno"> 1231</span>  <span class="keywordtype">int</span> i;</div><div class="line"><a name="l01232"></a><span class="lineno"> 1232</span>  </div><div class="line"><a name="l01233"></a><span class="lineno"> 1233</span>  res.error = 0;</div><div class="line"><a name="l01234"></a><span class="lineno"> 1234</span> </div><div class="line"><a name="l01235"></a><span class="lineno"> 1235</span>  <span class="keywordflow">if</span> ((_active_board<1) || (_active_board>62)) {</div><div class="line"><a name="l01236"></a><span class="lineno"> 1236</span>  ROS_ERROR(<span class="stringliteral">"Internal error - invalid board number %d :: PWM board numbers must be between 1 and 62"</span>, _active_board);</div><div class="line"><a name="l01237"></a><span class="lineno"> 1237</span>  res.error = -1;</div><div class="line"><a name="l01238"></a><span class="lineno"> 1238</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01239"></a><span class="lineno"> 1239</span>  }</div><div class="line"><a name="l01240"></a><span class="lineno"> 1240</span> </div><div class="line"><a name="l01241"></a><span class="lineno"> 1241</span>  <span class="keywordflow">for</span> (i=0;i<req.servos.size();i++) {</div><div class="line"><a name="l01242"></a><span class="lineno"> 1242</span>  <span class="keywordtype">int</span> servo = req.servos[i].servo;</div><div class="line"><a name="l01243"></a><span class="lineno"> 1243</span>  <span class="keywordtype">int</span> center = req.servos[i].center;</div><div class="line"><a name="l01244"></a><span class="lineno"> 1244</span>  <span class="keywordtype">int</span> range = req.servos[i].range;</div><div class="line"><a name="l01245"></a><span class="lineno"> 1245</span>  <span class="keywordtype">int</span> direction = req.servos[i].direction;</div><div class="line"><a name="l01246"></a><span class="lineno"> 1246</span> </div><div class="line"><a name="l01247"></a><span class="lineno"> 1247</span>  _config_servo (servo, center, range, direction);</div><div class="line"><a name="l01248"></a><span class="lineno"> 1248</span>  }</div><div class="line"><a name="l01249"></a><span class="lineno"> 1249</span>  </div><div class="line"><a name="l01250"></a><span class="lineno"> 1250</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01251"></a><span class="lineno"> 1251</span> }</div><div class="line"><a name="l01252"></a><span class="lineno"> 1252</span> </div><div class="line"><a name="l01253"></a><span class="lineno"> 1253</span> </div><div class="line"><a name="l01327"></a><span class="lineno"><a class="line" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9"> 1327</a></span> <span class="keywordtype">bool</span> <a class="code" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">config_drive_mode</a> (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)</div><div class="line"><a name="l01328"></a><span class="lineno"> 1328</span> {</div><div class="line"><a name="l01329"></a><span class="lineno"> 1329</span>  res.error = 0;</div><div class="line"><a name="l01330"></a><span class="lineno"> 1330</span> </div><div class="line"><a name="l01331"></a><span class="lineno"> 1331</span>  <span class="keywordtype">int</span> i;</div><div class="line"><a name="l01332"></a><span class="lineno"> 1332</span> </div><div class="line"><a name="l01333"></a><span class="lineno"> 1333</span>  <span class="keywordflow">if</span> ((res.error = _config_drive_mode (req.mode, req.rpm, req.radius, req.track, req.scale)))</div><div class="line"><a name="l01334"></a><span class="lineno"> 1334</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01335"></a><span class="lineno"> 1335</span> </div><div class="line"><a name="l01336"></a><span class="lineno"> 1336</span>  <span class="keywordflow">for</span> (i=0;i<req.servos.size();i++) {</div><div class="line"><a name="l01337"></a><span class="lineno"> 1337</span>  <span class="keywordtype">int</span> servo = req.servos[i].servo;</div><div class="line"><a name="l01338"></a><span class="lineno"> 1338</span>  <span class="keywordtype">int</span> position = req.servos[i].position;</div><div class="line"><a name="l01339"></a><span class="lineno"> 1339</span> </div><div class="line"><a name="l01340"></a><span class="lineno"> 1340</span>  <span class="keywordflow">if</span> (_config_servo_position (servo, position) != 0) {</div><div class="line"><a name="l01341"></a><span class="lineno"> 1341</span>  res.error = servo; <span class="comment">/* this needs to be more specific and indicate a bad server ID was provided */</span></div><div class="line"><a name="l01342"></a><span class="lineno"> 1342</span>  <span class="keywordflow">continue</span>;</div><div class="line"><a name="l01343"></a><span class="lineno"> 1343</span>  }</div><div class="line"><a name="l01344"></a><span class="lineno"> 1344</span>  }</div><div class="line"><a name="l01345"></a><span class="lineno"> 1345</span> </div><div class="line"><a name="l01346"></a><span class="lineno"> 1346</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01347"></a><span class="lineno"> 1347</span> }</div><div class="line"><a name="l01348"></a><span class="lineno"> 1348</span> </div><div class="line"><a name="l01349"></a><span class="lineno"> 1349</span> </div><div class="line"><a name="l01350"></a><span class="lineno"> 1350</span> </div><div class="line"><a name="l01370"></a><span class="lineno"><a class="line" href="group___services.html#ga4583b417d172bde68e75db603da3362e"> 1370</a></span> <span class="keywordtype">bool</span> <a class="code" href="group___services.html#ga4583b417d172bde68e75db603da3362e">stop_servos</a> (std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)</div><div class="line"><a name="l01371"></a><span class="lineno"> 1371</span> {</div><div class="line"><a name="l01372"></a><span class="lineno"> 1372</span>  <span class="keywordtype">int</span> save_active = _active_board;</div><div class="line"><a name="l01373"></a><span class="lineno"> 1373</span>  <span class="keywordtype">int</span> i;</div><div class="line"><a name="l01374"></a><span class="lineno"> 1374</span> </div><div class="line"><a name="l01375"></a><span class="lineno"> 1375</span>  <span class="keywordflow">for</span> (i=0; i<MAX_BOARDS; i++) {</div><div class="line"><a name="l01376"></a><span class="lineno"> 1376</span>  <span class="keywordflow">if</span> (_pwm_boards[i] > 0) {</div><div class="line"><a name="l01377"></a><span class="lineno"> 1377</span>  _set_active_board (i+1); <span class="comment">// API is ONE based</span></div><div class="line"><a name="l01378"></a><span class="lineno"> 1378</span>  _set_pwm_interval_all (0, 0);</div><div class="line"><a name="l01379"></a><span class="lineno"> 1379</span>  }</div><div class="line"><a name="l01380"></a><span class="lineno"> 1380</span>  }</div><div class="line"><a name="l01381"></a><span class="lineno"> 1381</span>  _set_active_board (save_active); <span class="comment">// restore last active board</span></div><div class="line"><a name="l01382"></a><span class="lineno"> 1382</span>  <span class="keywordflow">return</span> <span class="keyword">true</span>;</div><div class="line"><a name="l01383"></a><span class="lineno"> 1383</span> }</div><div class="line"><a name="l01384"></a><span class="lineno"> 1384</span> </div><div class="line"><a name="l01385"></a><span class="lineno"> 1385</span> </div><div class="line"><a name="l01386"></a><span class="lineno"> 1386</span> </div><div class="line"><a name="l01387"></a><span class="lineno"> 1387</span> </div><div class="line"><a name="l01388"></a><span class="lineno"> 1388</span> </div><div class="line"><a name="l01389"></a><span class="lineno"> 1389</span> <span class="keyword">static</span> std::string _get_string_param (XmlRpc::XmlRpcValue obj, std::string param_name)</div><div class="line"><a name="l01390"></a><span class="lineno"> 1390</span> {</div><div class="line"><a name="l01391"></a><span class="lineno"> 1391</span>  XmlRpc::XmlRpcValue &item = obj[param_name];</div><div class="line"><a name="l01392"></a><span class="lineno"> 1392</span>  <span class="keywordflow">if</span> (item.getType() == XmlRpc::XmlRpcValue::TypeString)</div><div class="line"><a name="l01393"></a><span class="lineno"> 1393</span>  <span class="keywordflow">return</span> item;</div><div class="line"><a name="l01394"></a><span class="lineno"> 1394</span> </div><div class="line"><a name="l01395"></a><span class="lineno"> 1395</span>  ROS_WARN(<span class="stringliteral">"invalid paramter type for %s - expected TypeString"</span>, param_name.c_str());</div><div class="line"><a name="l01396"></a><span class="lineno"> 1396</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l01397"></a><span class="lineno"> 1397</span> }</div><div class="line"><a name="l01398"></a><span class="lineno"> 1398</span> </div><div class="line"><a name="l01399"></a><span class="lineno"> 1399</span>  </div><div class="line"><a name="l01400"></a><span class="lineno"> 1400</span> <span class="keyword">static</span> <span class="keywordtype">int</span> _get_int_param (XmlRpc::XmlRpcValue obj, std::string param_name)</div><div class="line"><a name="l01401"></a><span class="lineno"> 1401</span> {</div><div class="line"><a name="l01402"></a><span class="lineno"> 1402</span>  XmlRpc::XmlRpcValue &item = obj[param_name];</div><div class="line"><a name="l01403"></a><span class="lineno"> 1403</span>  <span class="keywordflow">if</span> (item.getType() == XmlRpc::XmlRpcValue::TypeInt)</div><div class="line"><a name="l01404"></a><span class="lineno"> 1404</span>  <span class="keywordflow">return</span> item;</div><div class="line"><a name="l01405"></a><span class="lineno"> 1405</span> </div><div class="line"><a name="l01406"></a><span class="lineno"> 1406</span>  ROS_WARN(<span class="stringliteral">"invalid paramter type for %s - expected TypeInt"</span>, param_name.c_str());</div><div class="line"><a name="l01407"></a><span class="lineno"> 1407</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l01408"></a><span class="lineno"> 1408</span> }</div><div class="line"><a name="l01409"></a><span class="lineno"> 1409</span> </div><div class="line"><a name="l01410"></a><span class="lineno"> 1410</span>  </div><div class="line"><a name="l01411"></a><span class="lineno"> 1411</span> <span class="keyword">static</span> <span class="keywordtype">double</span> _get_float_param (XmlRpc::XmlRpcValue obj, std::string param_name)</div><div class="line"><a name="l01412"></a><span class="lineno"> 1412</span> {</div><div class="line"><a name="l01413"></a><span class="lineno"> 1413</span>  XmlRpc::XmlRpcValue &item = obj[param_name];</div><div class="line"><a name="l01414"></a><span class="lineno"> 1414</span>  <span class="keywordflow">if</span> (item.getType() == XmlRpc::XmlRpcValue::TypeDouble)</div><div class="line"><a name="l01415"></a><span class="lineno"> 1415</span>  <span class="keywordflow">return</span> item;</div><div class="line"><a name="l01416"></a><span class="lineno"> 1416</span> </div><div class="line"><a name="l01417"></a><span class="lineno"> 1417</span>  ROS_WARN(<span class="stringliteral">"invalid paramter type for %s - expected TypeDouble"</span>, param_name.c_str());</div><div class="line"><a name="l01418"></a><span class="lineno"> 1418</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l01419"></a><span class="lineno"> 1419</span> }</div><div class="line"><a name="l01420"></a><span class="lineno"> 1420</span> </div><div class="line"><a name="l01421"></a><span class="lineno"> 1421</span>  </div><div class="line"><a name="l01422"></a><span class="lineno"> 1422</span> <span class="keyword">static</span> <span class="keywordtype">int</span> _load_params (<span class="keywordtype">void</span>)</div><div class="line"><a name="l01423"></a><span class="lineno"> 1423</span> { </div><div class="line"><a name="l01424"></a><span class="lineno"> 1424</span>  ros::NodeHandle nhp; <span class="comment">// not currently private namespace</span></div><div class="line"><a name="l01425"></a><span class="lineno"> 1425</span> </div><div class="line"><a name="l01426"></a><span class="lineno"> 1426</span>  <span class="comment">/*</span></div><div class="line"><a name="l01427"></a><span class="lineno"> 1427</span> <span class="comment"> pwm_frequency: 50</span></div><div class="line"><a name="l01428"></a><span class="lineno"> 1428</span> <span class="comment"> */</span></div><div class="line"><a name="l01429"></a><span class="lineno"> 1429</span> </div><div class="line"><a name="l01430"></a><span class="lineno"> 1430</span>  <span class="keywordtype">int</span> pwm;</div><div class="line"><a name="l01431"></a><span class="lineno"> 1431</span>  nhp.param (<span class="stringliteral">"pwm_frequency"</span>, pwm, 50);</div><div class="line"><a name="l01432"></a><span class="lineno"> 1432</span> </div><div class="line"><a name="l01433"></a><span class="lineno"> 1433</span>  </div><div class="line"><a name="l01434"></a><span class="lineno"> 1434</span>  <span class="comment">/*</span></div><div class="line"><a name="l01435"></a><span class="lineno"> 1435</span> <span class="comment"> // note: servos are numbered sequntially with '1' being the first servo on board #1, '17' is the first servo on board #2</span></div><div class="line"><a name="l01436"></a><span class="lineno"> 1436</span> <span class="comment"></span></div><div class="line"><a name="l01437"></a><span class="lineno"> 1437</span> <span class="comment"> servo_config:</span></div><div class="line"><a name="l01438"></a><span class="lineno"> 1438</span> <span class="comment"> - {id: 1, center: 333, direction: -1, range: 100}</span></div><div class="line"><a name="l01439"></a><span class="lineno"> 1439</span> <span class="comment"> - {id: 2, center: 336, direction: 1, range: 108}</span></div><div class="line"><a name="l01440"></a><span class="lineno"> 1440</span> <span class="comment"></span></div><div class="line"><a name="l01441"></a><span class="lineno"> 1441</span> <span class="comment"> */</span></div><div class="line"><a name="l01442"></a><span class="lineno"> 1442</span>  <span class="comment">// attempt to load configuration for servos</span></div><div class="line"><a name="l01443"></a><span class="lineno"> 1443</span>  <span class="keywordflow">if</span>(nhp.hasParam (<span class="stringliteral">"servo_config"</span>)) {</div><div class="line"><a name="l01444"></a><span class="lineno"> 1444</span>  XmlRpc::XmlRpcValue servos;</div><div class="line"><a name="l01445"></a><span class="lineno"> 1445</span>  nhp.getParam (<span class="stringliteral">"servo_config"</span>, servos);</div><div class="line"><a name="l01446"></a><span class="lineno"> 1446</span> </div><div class="line"><a name="l01447"></a><span class="lineno"> 1447</span>  <span class="keywordflow">if</span>(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {</div><div class="line"><a name="l01448"></a><span class="lineno"> 1448</span>  ROS_DEBUG(<span class="stringliteral">"Retrieving members from 'servo_config' in namespace(%s)"</span>, nhp.getNamespace().c_str());</div><div class="line"><a name="l01449"></a><span class="lineno"> 1449</span>  </div><div class="line"><a name="l01450"></a><span class="lineno"> 1450</span>  <span class="keywordflow">for</span>(int32_t i = 0; i < servos.size(); i++) {</div><div class="line"><a name="l01451"></a><span class="lineno"> 1451</span>  XmlRpc::XmlRpcValue servo;</div><div class="line"><a name="l01452"></a><span class="lineno"> 1452</span>  servo = servos[i]; <span class="comment">// get the data from the iterator</span></div><div class="line"><a name="l01453"></a><span class="lineno"> 1453</span>  <span class="keywordflow">if</span>(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {</div><div class="line"><a name="l01454"></a><span class="lineno"> 1454</span>  ROS_DEBUG(<span class="stringliteral">"Retrieving items from 'servo_config' member %d in namespace(%s)"</span>, i, nhp.getNamespace().c_str());</div><div class="line"><a name="l01455"></a><span class="lineno"> 1455</span> </div><div class="line"><a name="l01456"></a><span class="lineno"> 1456</span>  <span class="comment">// get the servo settings</span></div><div class="line"><a name="l01457"></a><span class="lineno"> 1457</span>  <span class="keywordtype">int</span> <span class="keywordtype">id</span> = 0, center = 0, direction = 0, range = 0;</div><div class="line"><a name="l01458"></a><span class="lineno"> 1458</span>  <span class="keywordtype">id</span> = _get_int_param (servo, <span class="stringliteral">"id"</span>);</div><div class="line"><a name="l01459"></a><span class="lineno"> 1459</span>  center = _get_int_param (servo, <span class="stringliteral">"center"</span>);</div><div class="line"><a name="l01460"></a><span class="lineno"> 1460</span>  direction = _get_int_param (servo, <span class="stringliteral">"direction"</span>);</div><div class="line"><a name="l01461"></a><span class="lineno"> 1461</span>  range = _get_int_param (servo, <span class="stringliteral">"range"</span>);</div><div class="line"><a name="l01462"></a><span class="lineno"> 1462</span>  </div><div class="line"><a name="l01463"></a><span class="lineno"> 1463</span>  <span class="keywordflow">if</span> (<span class="keywordtype">id</span> && center && direction && range) {</div><div class="line"><a name="l01464"></a><span class="lineno"> 1464</span>  <span class="keywordflow">if</span> ((<span class="keywordtype">id</span> >= 1) && (<span class="keywordtype">id</span> <= MAX_SERVOS)) {</div><div class="line"><a name="l01465"></a><span class="lineno"> 1465</span>  <span class="keywordtype">int</span> board = ((int)(<span class="keywordtype">id</span> / 16)) + 1;</div><div class="line"><a name="l01466"></a><span class="lineno"> 1466</span>  _set_active_board (board);</div><div class="line"><a name="l01467"></a><span class="lineno"> 1467</span>  _set_pwm_frequency (pwm);</div><div class="line"><a name="l01468"></a><span class="lineno"> 1468</span>  _config_servo (<span class="keywordtype">id</span>, center, range, direction);</div><div class="line"><a name="l01469"></a><span class="lineno"> 1469</span>  }</div><div class="line"><a name="l01470"></a><span class="lineno"> 1470</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01471"></a><span class="lineno"> 1471</span>  ROS_WARN(<span class="stringliteral">"Parameter servo id=%d is out of bounds"</span>, <span class="keywordtype">id</span>);</div><div class="line"><a name="l01472"></a><span class="lineno"> 1472</span>  }</div><div class="line"><a name="l01473"></a><span class="lineno"> 1473</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01474"></a><span class="lineno"> 1474</span>  ROS_WARN(<span class="stringliteral">"Invalid parameters for servo id=%d'"</span>, <span class="keywordtype">id</span>);</div><div class="line"><a name="l01475"></a><span class="lineno"> 1475</span>  }</div><div class="line"><a name="l01476"></a><span class="lineno"> 1476</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01477"></a><span class="lineno"> 1477</span>  ROS_WARN(<span class="stringliteral">"Invalid type %d for member of 'servo_config' - expected TypeStruct(%d)"</span>, servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);</div><div class="line"><a name="l01478"></a><span class="lineno"> 1478</span>  }</div><div class="line"><a name="l01479"></a><span class="lineno"> 1479</span>  }</div><div class="line"><a name="l01480"></a><span class="lineno"> 1480</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01481"></a><span class="lineno"> 1481</span>  ROS_WARN(<span class="stringliteral">"Invalid type %d for 'servo_config' - expected TypeArray(%d)"</span>, servos.getType(), XmlRpc::XmlRpcValue::TypeArray);</div><div class="line"><a name="l01482"></a><span class="lineno"> 1482</span>  }</div><div class="line"><a name="l01483"></a><span class="lineno"> 1483</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01484"></a><span class="lineno"> 1484</span>  ROS_DEBUG(<span class="stringliteral">"Parameter Server namespace[%s] does not contain 'servo_config"</span>, nhp.getNamespace().c_str());</div><div class="line"><a name="l01485"></a><span class="lineno"> 1485</span> </div><div class="line"><a name="l01486"></a><span class="lineno"> 1486</span> </div><div class="line"><a name="l01487"></a><span class="lineno"> 1487</span>  <span class="comment">/*</span></div><div class="line"><a name="l01488"></a><span class="lineno"> 1488</span> <span class="comment"> drive_config:</span></div><div class="line"><a name="l01489"></a><span class="lineno"> 1489</span> <span class="comment"> mode: mecanum</span></div><div class="line"><a name="l01490"></a><span class="lineno"> 1490</span> <span class="comment"> radius: 0.062</span></div><div class="line"><a name="l01491"></a><span class="lineno"> 1491</span> <span class="comment"> rpm: 60.0</span></div><div class="line"><a name="l01492"></a><span class="lineno"> 1492</span> <span class="comment"> scale: 0.3</span></div><div class="line"><a name="l01493"></a><span class="lineno"> 1493</span> <span class="comment"> track: 0.2</span></div><div class="line"><a name="l01494"></a><span class="lineno"> 1494</span> <span class="comment"> servos:</span></div><div class="line"><a name="l01495"></a><span class="lineno"> 1495</span> <span class="comment"> - {id: 1, position: 1}</span></div><div class="line"><a name="l01496"></a><span class="lineno"> 1496</span> <span class="comment"> - {id: 2, position: 2}</span></div><div class="line"><a name="l01497"></a><span class="lineno"> 1497</span> <span class="comment"> - {id: 3, position: 3}</span></div><div class="line"><a name="l01498"></a><span class="lineno"> 1498</span> <span class="comment"> - {id: 4, position: 4}</span></div><div class="line"><a name="l01499"></a><span class="lineno"> 1499</span> <span class="comment"> */</span></div><div class="line"><a name="l01500"></a><span class="lineno"> 1500</span> </div><div class="line"><a name="l01501"></a><span class="lineno"> 1501</span>  <span class="comment">// attempt to load configuration for drive mode</span></div><div class="line"><a name="l01502"></a><span class="lineno"> 1502</span>  <span class="keywordflow">if</span>(nhp.hasParam (<span class="stringliteral">"drive_config"</span>)) {</div><div class="line"><a name="l01503"></a><span class="lineno"> 1503</span>  XmlRpc::XmlRpcValue drive;</div><div class="line"><a name="l01504"></a><span class="lineno"> 1504</span>  nhp.getParam (<span class="stringliteral">"drive_config"</span>, drive);</div><div class="line"><a name="l01505"></a><span class="lineno"> 1505</span> </div><div class="line"><a name="l01506"></a><span class="lineno"> 1506</span>  <span class="keywordflow">if</span>(drive.getType() == XmlRpc::XmlRpcValue::TypeStruct) {</div><div class="line"><a name="l01507"></a><span class="lineno"> 1507</span>  ROS_DEBUG(<span class="stringliteral">"Retrieving members from 'drive_config' in namespace(%s)"</span>, nhp.getNamespace().c_str());</div><div class="line"><a name="l01508"></a><span class="lineno"> 1508</span> </div><div class="line"><a name="l01509"></a><span class="lineno"> 1509</span>  <span class="comment">// get the drive mode settings</span></div><div class="line"><a name="l01510"></a><span class="lineno"> 1510</span>  std::string mode;</div><div class="line"><a name="l01511"></a><span class="lineno"> 1511</span>  <span class="keywordtype">float</span> radius, rpm, scale, track;</div><div class="line"><a name="l01512"></a><span class="lineno"> 1512</span>  <span class="keywordtype">int</span> id, position;</div><div class="line"><a name="l01513"></a><span class="lineno"> 1513</span> </div><div class="line"><a name="l01514"></a><span class="lineno"> 1514</span>  mode = _get_string_param (drive, <span class="stringliteral">"mode"</span>);</div><div class="line"><a name="l01515"></a><span class="lineno"> 1515</span>  rpm = _get_float_param (drive, <span class="stringliteral">"rpm"</span>);</div><div class="line"><a name="l01516"></a><span class="lineno"> 1516</span>  radius = _get_float_param (drive, <span class="stringliteral">"radius"</span>);</div><div class="line"><a name="l01517"></a><span class="lineno"> 1517</span>  track = _get_float_param (drive, <span class="stringliteral">"track"</span>);</div><div class="line"><a name="l01518"></a><span class="lineno"> 1518</span>  scale = _get_float_param (drive, <span class="stringliteral">"scale"</span>);</div><div class="line"><a name="l01519"></a><span class="lineno"> 1519</span> </div><div class="line"><a name="l01520"></a><span class="lineno"> 1520</span>  _config_drive_mode (mode, rpm, radius, track, scale);</div><div class="line"><a name="l01521"></a><span class="lineno"> 1521</span> </div><div class="line"><a name="l01522"></a><span class="lineno"> 1522</span>  XmlRpc::XmlRpcValue &servos = drive[<span class="stringliteral">"servos"</span>];</div><div class="line"><a name="l01523"></a><span class="lineno"> 1523</span>  <span class="keywordflow">if</span>(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {</div><div class="line"><a name="l01524"></a><span class="lineno"> 1524</span>  ROS_DEBUG(<span class="stringliteral">"Retrieving members from 'drive_config/servos' in namespace(%s)"</span>, nhp.getNamespace().c_str());</div><div class="line"><a name="l01525"></a><span class="lineno"> 1525</span>  </div><div class="line"><a name="l01526"></a><span class="lineno"> 1526</span>  <span class="keywordflow">for</span>(int32_t i = 0; i < servos.size(); i++) {</div><div class="line"><a name="l01527"></a><span class="lineno"> 1527</span>  XmlRpc::XmlRpcValue servo;</div><div class="line"><a name="l01528"></a><span class="lineno"> 1528</span>  servo = servos[i]; <span class="comment">// get the data from the iterator</span></div><div class="line"><a name="l01529"></a><span class="lineno"> 1529</span>  <span class="keywordflow">if</span>(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {</div><div class="line"><a name="l01530"></a><span class="lineno"> 1530</span>  ROS_DEBUG(<span class="stringliteral">"Retrieving items from 'drive_config/servos' member %d in namespace(%s)"</span>, i, nhp.getNamespace().c_str());</div><div class="line"><a name="l01531"></a><span class="lineno"> 1531</span> </div><div class="line"><a name="l01532"></a><span class="lineno"> 1532</span>  <span class="comment">// get the servo position settings</span></div><div class="line"><a name="l01533"></a><span class="lineno"> 1533</span>  <span class="keywordtype">int</span> id, position;</div><div class="line"><a name="l01534"></a><span class="lineno"> 1534</span>  <span class="keywordtype">id</span> = _get_int_param (servo, <span class="stringliteral">"id"</span>);</div><div class="line"><a name="l01535"></a><span class="lineno"> 1535</span>  position = _get_int_param (servo, <span class="stringliteral">"position"</span>);</div><div class="line"><a name="l01536"></a><span class="lineno"> 1536</span>  </div><div class="line"><a name="l01537"></a><span class="lineno"> 1537</span>  <span class="keywordflow">if</span> (<span class="keywordtype">id</span> && position)</div><div class="line"><a name="l01538"></a><span class="lineno"> 1538</span>  _config_servo_position (<span class="keywordtype">id</span>, position); <span class="comment">// had its own error reporting</span></div><div class="line"><a name="l01539"></a><span class="lineno"> 1539</span>  }</div><div class="line"><a name="l01540"></a><span class="lineno"> 1540</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01541"></a><span class="lineno"> 1541</span>  ROS_WARN(<span class="stringliteral">"Invalid type %d for member %d of 'drive_config/servos' - expected TypeStruct(%d)"</span>, i, servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);</div><div class="line"><a name="l01542"></a><span class="lineno"> 1542</span>  }</div><div class="line"><a name="l01543"></a><span class="lineno"> 1543</span>  }</div><div class="line"><a name="l01544"></a><span class="lineno"> 1544</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01545"></a><span class="lineno"> 1545</span>  ROS_WARN(<span class="stringliteral">"Invalid type %d for 'drive_config/servos' - expected TypeArray(%d)"</span>, servos.getType(), XmlRpc::XmlRpcValue::TypeArray);</div><div class="line"><a name="l01546"></a><span class="lineno"> 1546</span>  }</div><div class="line"><a name="l01547"></a><span class="lineno"> 1547</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01548"></a><span class="lineno"> 1548</span>  ROS_WARN(<span class="stringliteral">"Invalid type %d for 'drive_config' - expected TypeStruct(%d)"</span>, drive.getType(), XmlRpc::XmlRpcValue::TypeStruct);</div><div class="line"><a name="l01549"></a><span class="lineno"> 1549</span>  }</div><div class="line"><a name="l01550"></a><span class="lineno"> 1550</span>  <span class="keywordflow">else</span></div><div class="line"><a name="l01551"></a><span class="lineno"> 1551</span>  ROS_DEBUG(<span class="stringliteral">"Parameter Server namespace[%s] does not contain 'drive_config"</span>, nhp.getNamespace().c_str());</div><div class="line"><a name="l01552"></a><span class="lineno"> 1552</span> } </div><div class="line"><a name="l01553"></a><span class="lineno"> 1553</span> </div><div class="line"><a name="l01554"></a><span class="lineno"> 1554</span> </div><div class="line"><a name="l01555"></a><span class="lineno"> 1555</span> <span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l01557"></a><span class="lineno"> 1557</span> <span class="comment"></span><span class="comment">// main</span></div><div class="line"><a name="l01558"></a><span class="lineno"> 1558</span> <span class="comment">// ------------------------------------------------------------------------------------------------------------------------------------</span></div><div class="line"><a name="l01559"></a><span class="lineno"> 1559</span> </div><div class="line"><a name="l01560"></a><span class="lineno"> 1560</span> <span class="keywordtype">int</span> main (<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> **argv)</div><div class="line"><a name="l01561"></a><span class="lineno"> 1561</span> {</div><div class="line"><a name="l01562"></a><span class="lineno"> 1562</span> </div><div class="line"><a name="l01563"></a><span class="lineno"> 1563</span>  _controller_io_handle = 0;</div><div class="line"><a name="l01564"></a><span class="lineno"> 1564</span>  _pwm_frequency = 50; <span class="comment">// set the initial pulse frequency to 50 Hz which is standard for RC servos</span></div><div class="line"><a name="l01565"></a><span class="lineno"> 1565</span> </div><div class="line"><a name="l01566"></a><span class="lineno"> 1566</span>  </div><div class="line"><a name="l01567"></a><span class="lineno"> 1567</span>  _init (_CONST(<span class="stringliteral">"/dev/i2c-1"</span>)); <span class="comment">// default I2C device on RPi2 and RPi3 = "/dev/i2c-1";</span></div><div class="line"><a name="l01568"></a><span class="lineno"> 1568</span> </div><div class="line"><a name="l01569"></a><span class="lineno"> 1569</span>  _set_active_board (1);</div><div class="line"><a name="l01570"></a><span class="lineno"> 1570</span> </div><div class="line"><a name="l01571"></a><span class="lineno"> 1571</span>  _set_pwm_frequency (50);</div><div class="line"><a name="l01572"></a><span class="lineno"> 1572</span> </div><div class="line"><a name="l01573"></a><span class="lineno"> 1573</span> </div><div class="line"><a name="l01574"></a><span class="lineno"> 1574</span>  ros::init (argc, argv, <span class="stringliteral">"i2cpwm_controller"</span>);</div><div class="line"><a name="l01575"></a><span class="lineno"> 1575</span> </div><div class="line"><a name="l01576"></a><span class="lineno"> 1576</span>  ros::NodeHandle n;</div><div class="line"><a name="l01577"></a><span class="lineno"> 1577</span> </div><div class="line"><a name="l01578"></a><span class="lineno"> 1578</span>  ros::ServiceServer freq_srv = n.advertiseService (<span class="stringliteral">"set_pwm_frequency"</span>, <a class="code" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">set_pwm_frequency</a>);</div><div class="line"><a name="l01579"></a><span class="lineno"> 1579</span>  ros::ServiceServer config_srv = n.advertiseService (<span class="stringliteral">"config_servos"</span>, <a class="code" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">config_servos</a>); <span class="comment">// 'config' will setup the necessary properties of continuous servos and is helpful for standard servos</span></div><div class="line"><a name="l01580"></a><span class="lineno"> 1580</span>  ros::ServiceServer mode_srv = n.advertiseService (<span class="stringliteral">"config_drive_mode"</span>, <a class="code" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">config_drive_mode</a>); <span class="comment">// 'mode' specifies which servos are used for motion and which behavior will be applied when driving</span></div><div class="line"><a name="l01581"></a><span class="lineno"> 1581</span>  ros::ServiceServer stop_srv = n.advertiseService (<span class="stringliteral">"stop_servos"</span>, <a class="code" href="group___services.html#ga4583b417d172bde68e75db603da3362e">stop_servos</a>); <span class="comment">// the 'stop' service can be used at any time</span></div><div class="line"><a name="l01582"></a><span class="lineno"> 1582</span> </div><div class="line"><a name="l01583"></a><span class="lineno"> 1583</span>  ros::Subscriber abs_sub = n.subscribe (<span class="stringliteral">"servos_absolute"</span>, 500, <a class="code" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">servos_absolute</a>); <span class="comment">// the 'absolute' topic will be used for standard servo motion and testing of continuous servos</span></div><div class="line"><a name="l01584"></a><span class="lineno"> 1584</span>  ros::Subscriber rel_sub = n.subscribe (<span class="stringliteral">"servos_proportional"</span>, 500, <a class="code" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">servos_proportional</a>); <span class="comment">// the 'proportion' topic will be used for standard servos and continuous rotation aka drive servos</span></div><div class="line"><a name="l01585"></a><span class="lineno"> 1585</span>  ros::Subscriber drive_sub = n.subscribe (<span class="stringliteral">"servos_drive"</span>, 500, <a class="code" href="group___topics.html#gae257665eb64139d2720ed332e61507c6">servos_drive</a>); <span class="comment">// the 'drive' topic will be used for continuous rotation aka drive servos controlled by Twist messages</span></div><div class="line"><a name="l01586"></a><span class="lineno"> 1586</span>  </div><div class="line"><a name="l01587"></a><span class="lineno"> 1587</span>  _load_params();</div><div class="line"><a name="l01588"></a><span class="lineno"> 1588</span>  </div><div class="line"><a name="l01589"></a><span class="lineno"> 1589</span>  ros::spin();</div><div class="line"><a name="l01590"></a><span class="lineno"> 1590</span> </div><div class="line"><a name="l01591"></a><span class="lineno"> 1591</span>  close (_controller_io_handle);</div><div class="line"><a name="l01592"></a><span class="lineno"> 1592</span> </div><div class="line"><a name="l01593"></a><span class="lineno"> 1593</span>  <span class="keywordflow">return</span> 0;</div><div class="line"><a name="l01594"></a><span class="lineno"> 1594</span> }</div><div class="line"><a name="l01595"></a><span class="lineno"> 1595</span> </div><div class="ttc" id="group___services_html_ga948cb0dd7ba8c2ac4c7f2d2d93940d85"><div class="ttname"><a href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85">set_pwm_frequency</a></div><div class="ttdeci">bool set_pwm_frequency(i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)</div><div class="ttdoc">service to set set PWM frequency </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l01172">i2cpwm_controller.cpp:1172</a></div></div>
+<div class="ttc" id="group___services_html_gabf512716d4497c6a4cb7bf9c5d94e2f1"><div class="ttname"><a href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1">config_servos</a></div><div class="ttdeci">bool config_servos(i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)</div><div class="ttdoc">store configuration data for servos on the active board </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l01228">i2cpwm_controller.cpp:1228</a></div></div>
+<div class="ttc" id="group___services_html_ga4583b417d172bde68e75db603da3362e"><div class="ttname"><a href="group___services.html#ga4583b417d172bde68e75db603da3362e">stop_servos</a></div><div class="ttdeci">bool stop_servos(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)</div><div class="ttdoc">service to stop all servos on all boards </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l01370">i2cpwm_controller.cpp:1370</a></div></div>
+<div class="ttc" id="group___services_html_gae3222f92a0cd33b0d0a2fcfd6d563ba9"><div class="ttname"><a href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9">config_drive_mode</a></div><div class="ttdeci">bool config_drive_mode(i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)</div><div class="ttdoc">set drive mode and drive servos </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l01327">i2cpwm_controller.cpp:1327</a></div></div>
+<div class="ttc" id="group___topics_html_gae257665eb64139d2720ed332e61507c6"><div class="ttname"><a href="group___topics.html#gae257665eb64139d2720ed332e61507c6">servos_drive</a></div><div class="ttdeci">void servos_drive(const geometry_msgs::Twist::ConstPtr &msg)</div><div class="ttdoc">subscriber topic to move servos based on a drive mode </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l00976">i2cpwm_controller.cpp:976</a></div></div>
+<div class="ttc" id="group___topics_html_gaa3fe8c6f5a98243c302c0d27cea219d9"><div class="ttname"><a href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9">servos_absolute</a></div><div class="ttdeci">void servos_absolute(const i2cpwm_board::ServoArray::ConstPtr &msg)</div><div class="ttdoc">subscriber topic to move servos in a physical position </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l00797">i2cpwm_controller.cpp:797</a></div></div>
+<div class="ttc" id="group___topics_html_ga1262b2e51072c237c51b6de4cb199e53"><div class="ttname"><a href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53">servos_proportional</a></div><div class="ttdeci">void servos_proportional(const i2cpwm_board::ServoArray::ConstPtr &msg)</div><div class="ttdoc">subscriber topic to move servos in the range of ±1.0 </div><div class="ttdef"><b>Definition:</b> <a href="i2cpwm__controller_8cpp_source.html#l00867">i2cpwm_controller.cpp:867</a></div></div>
+</div><!-- fragment --></div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/index.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/index.html
new file mode 100644
index 0000000000000000000000000000000000000000..77496517724623ac3d2d59fce55f8adf9090eb88
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/index.html
@@ -0,0 +1,124 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Main Page</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="header">
+ <div class="headertitle">
+<div class="title">i2cpwm_board Documentation</div> </div>
+</div><!--header-->
+<div class="contents">
+<div class="textblock"><p>Controller for I2C interfaced 16 channel PWM boards with PCA9685 chip<br />
+ Bradan Lane Studio <a href="#" onclick="location.href='mai'+'lto:'+'inf'+'o@'+'bra'+'da'+'nla'+'ne'+'.co'+'m'; return false;">info@<span style="display: none;">.nosp@m.</span>brad<span style="display: none;">.nosp@m.</span>anlan<span style="display: none;">.nosp@m.</span>e.co<span style="display: none;">.nosp@m.</span>m</a><br />
+ Copyright (c) 2016, Bradan Lane Studio<br />
+ Licensed under GPLv3<br />
+ </p><hr/>
+<h1><a class="anchor" id="overview"></a>
+FILE STRUCTURE AND CODE</h1>
+<p>The file is broken into sections:</p><ul>
+<li>private properties (all private properties have a leading underscore)</li>
+<li>private methods (all private methods have a leading underscore)</li>
+<li>public topic subscribers:<ul>
+<li><a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9" title="subscriber topic to move servos in a physical position ">servos_absolute()</a></li>
+<li><a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53" title="subscriber topic to move servos in the range of ±1.0 ">servos_proportional()</a></li>
+<li><a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6" title="subscriber topic to move servos based on a drive mode ">servos_drive()</a></li>
+</ul>
+</li>
+<li>public services:<ul>
+<li><a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85" title="service to set set PWM frequency ">set_pwm_frequency()</a></li>
+<li>set_active_board()</li>
+<li><a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a></li>
+<li><a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9" title="set drive mode and drive servos ">config_drive_mode()</a></li>
+<li><a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e" title="service to stop all servos on all boards ">stop_servos()</a></li>
+</ul>
+</li>
+</ul>
+<p>The code is currently authored in C and should be rewritten as proper C++.</p>
+<p>This documentation refers to 'servo' and 'RC servo' but is equally applicable to any PWM or PPM controlled DC motor.</p>
+<p>All published services and topics use a one-based count syntax. For example, the first servo is '1' and the default board is '1'. The hardware uses zero-based values. For example the first channel on the 16 channel 12-bit PWM board is '0' and the first I2C board is '0' with address 0x40. The switch from one-based to zero-based is done at the lowest level of this code. All public interactions should assume one-based values.</p>
+<p><em>WARNING</em>: The code has only been tested with a single board using the default I2C address of 0x40. Once testing has been done with additional configurations, this warning will be removed or amended.</p>
+<h1><a class="anchor" id="pwmservos"></a>
+USING PWM WITH SERVOS</h1>
+<p>While the PCA9685 chip and related boards are called "PWM" for pulse width modulation, there use with servos is more accurately PPM for pulse position modulation.</p>
+<p>For standard 180 degree servos with a motion arc of ±90 degrees the pulse moves the servo arm to specifc position and holds it. For continuous motion servos, the pulse moves the servo at a specific speed.</p>
+<p>The documentation will refer to positon, speed or position/speed. These are interchangeable as the two terms are dependent on whether the servo is a fixed rotation servo or a continuous rotation servo and independent of the board itself.</p>
+<p>Analog RC servos are most often designed for 20ms pulses. This is achieved with a frequency of 50Hz. This software defaults to 50Hz. Use the set_pwm_frequncy() to change this frequency value. It may be necessary or convenient to change the PWM frequency if using DC motors connected to PWM controllers. It may also be convenient if using PWM to control LEDs.</p>
+<p>A commonly available board is the Adafruit 16 channel 12-bit PWM board or the similarly named HAT. There are similar boards as well as clones. All of these boards use the PCA9685 chip. Not all boards have been tested.</p>
+<p>The PWM boards drive LED and servos using pulse width modulation. The 12 bit interface means values are in the range of 0..4096. The pulse is defined as a start value and end value. When driving servos, the start point is typically 0 and the end point is the duration.</p>
+<p>FYI: If using more than one PWM board or when using a board with an I2C address other than the default 0x40, the set_active_board() service must be used before using other services or topics from this package.</p>
+<h1><a class="anchor" id="configuration"></a>
+CONFIGURING SERVOS</h1>
+<p>The tolerance of the resistors in RC servos means each servo may have a slightly different center point.</p>
+<p>The <a class="el" href="group___topics.html#gaa3fe8c6f5a98243c302c0d27cea219d9" title="subscriber topic to move servos in a physical position ">servos_absolute()</a> topic controls servos with absolute pulse start and stop values. This topic subscriber is not generally useful in robot operations, however it is convenient for testing and for determining configuration values. Use this topic to determine the center position for a standard servo or the stop position for a continuous servo.</p>
+<p>Also use this topic to determine the range of each servo - either the ±90 postion for a standard servo or the max forward and reverse speed for a continuous rotation servo.</p>
+<p><b>note:</b> The centering value and range of servos is dependent on the pulse frequency. If you use <a class="el" href="group___services.html#ga948cb0dd7ba8c2ac4c7f2d2d93940d85" title="service to set set PWM frequency ">set_pwm_frequency()</a> to change the system value, you will need to determine new center and range values.</p>
+<p>The <a class="el" href="group___topics.html#ga1262b2e51072c237c51b6de4cb199e53" title="subscriber topic to move servos in the range of ±1.0 ">servos_proportional()</a> topic controls servos through their range of motion using values between -1.0 and 1.0. Use the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> service to set the center values, range values, and directions of rotation for servos. This enables servo motion to be generalized to a standard proportion of ±1.0. Use of the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> service is required before proportional control is available.</p>
+<h1><a class="anchor" id="drivemode"></a>
+ROBOT DRIVE MODE</h1>
+<p>In addition to absolute and proportional topics for controling servos, this package provides support for the geometry 'Twist' message. The <a class="el" href="group___topics.html#gae257665eb64139d2720ed332e61507c6" title="subscriber topic to move servos based on a drive mode ">servos_drive()</a> topic handles the calculations to convert linear and angular data to servo drive data.</p>
+<p>Drive mode requires details for each servo. Use of the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> before using drive mode.</p>
+<p>The geometry_msgs::Twist providesf linear and angular velocity measured in m/s (meters per second) and r/s (radians per second) respectively. To perform the necessary calculations, data about the drive system is required. Use <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9" title="set drive mode and drive servos ">config_drive_mode()</a> to provide the RPM of the drive wheels, their radius, and the track distance between left and right. These values determine speed and turn rates.</p>
+<p>Specifiy the desired drive mode with the <code>mode</code> property to cofig_drive_mode(). This package supports three drive modes:</p>
+<ol type="1">
+<li><b>ackerman</b> - A single velocity drive using one or more servos with a non-drive servo controlling steering.</li>
+<li><b>differential</b> - Skid steer or track steer using one or more servos for each of the left and right sides. The result is full speed forward or reverse, min/max radius turns, and the ability to perform zero-radius turns</li>
+<li><b>mecanum</b> - Independent drive on all four wheel capable of holonomic drive - moving in any combination of forward, reverse, turning, and sideways. The servos are assigned positons for left-front, right-front, left-rear, and right-rear wheels. This mode supports similar drive characteristics to differential drive with the additional of lateral motion.</li>
+</ol>
+<p>The servos on the PWM board are assigned to their respective drive positons using the <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9" title="set drive mode and drive servos ">config_drive_mode()</a> service. The applicable servos are assigned positions as follows:</p>
+<table class="doxtable">
+<tr>
+<th>positon </th><th>ackerman </th><th>differential </th><th>mecanum </th></tr>
+<tr>
+<td>position 1 corresponds to </td><td>drive </td><td>left </td><td>left-front </td></tr>
+<tr>
+<td>position 2 corresponds to </td><td></td><td>right </td><td>right-front </td></tr>
+<tr>
+<td>position 3 corresponds to </td><td></td><td></td><td>left-rear </td></tr>
+<tr>
+<td>position 4 corresponds to </td><td></td><td></td><td>right-rear </td></tr>
+</table>
+<p>The drive topic requires that use of both the <a class="el" href="group___services.html#gabf512716d4497c6a4cb7bf9c5d94e2f1" title="store configuration data for servos on the active board ">config_servos()</a> service and <a class="el" href="group___services.html#gae3222f92a0cd33b0d0a2fcfd6d563ba9" title="set drive mode and drive servos ">config_drive_mode()</a> service before drive mode will result in expected behavior.</p>
+<p>All non-drive servos may still be operated with the proportion or absolute topics.</p>
+<p><b>Note:</b> <em>This controller does not implement encoders for PWM motors. There is no guarantee that the drive velocity will exactly match the Twist message input. While this may not be acceptable for a commercial or scientific application, it may not be of convern for education and amatuer competitions. Encoders could be added and feedback applied to the PWM values. Additionally, when drive control is a product of positional feedback - such as line following and navigation via camera vision - drive encoders are usually not required.</em></p>
+<p>The <a class="el" href="group___services.html#ga4583b417d172bde68e75db603da3362e" title="service to stop all servos on all boards ">stop_servos()</a> service is provided as convenience to stop all servos and place then is a powered off state. This is different from setting each servo to its center value. The stop service is useful as a safety operation.</p>
+<h1><a class="anchor" id="testing"></a>
+TESTING</h1>
+<p>Basic testing is available from the command line. Start the I2C PWM node with <code>roslaunch i2cpwm_board i2cpwm_node.launch</code> (or <code>roscore</code> and <code>rosrun i2cpwm_board i2cpwm_board</code>) and then proceed with example commands contained within the documentation for each service and topic subscriber. </p>
+</div></div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/jquery.js b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/jquery.js
new file mode 100644
index 0000000000000000000000000000000000000000..d52a1c77576c892c82d021f68d54d55e34d0efbd
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/jquery.js
@@ -0,0 +1,68 @@
+/*
+ * jQuery JavaScript Library v1.7.1
+ * http://jquery.com/
+ *
+ * Copyright 2011, John Resig
+ * Dual licensed under the MIT or GPL Version 2 licenses.
+ * http://jquery.org/license
+ *
+ * Includes Sizzle.js
+ * http://sizzlejs.com/
+ * Copyright 2011, The Dojo Foundation
+ * Released under the MIT, BSD, and GPL Licenses.
+ *
+ * Date: Mon Nov 21 21:11:03 2011 -0500
+ */
+(function(bb,L){var av=bb.document,bu=bb.navigator,bl=bb.location;var b=(function(){var bF=function(b0,b1){return new bF.fn.init(b0,b1,bD)},bU=bb.jQuery,bH=bb.$,bD,bY=/^(?:[^#<]*(<[\w\W]+>)[^>]*$|#([\w\-]*)$)/,bM=/\S/,bI=/^\s+/,bE=/\s+$/,bA=/^<(\w+)\s*\/?>(?:<\/\1>)?$/,bN=/^[\],:{}\s]*$/,bW=/\\(?:["\\\/bfnrt]|u[0-9a-fA-F]{4})/g,bP=/"[^"\\\n\r]*"|true|false|null|-?\d+(?:\.\d*)?(?:[eE][+\-]?\d+)?/g,bJ=/(?:^|:|,)(?:\s*\[)+/g,by=/(webkit)[ \/]([\w.]+)/,bR=/(opera)(?:.*version)?[ \/]([\w.]+)/,bQ=/(msie) ([\w.]+)/,bS=/(mozilla)(?:.*? rv:([\w.]+))?/,bB=/-([a-z]|[0-9])/ig,bZ=/^-ms-/,bT=function(b0,b1){return(b1+"").toUpperCase()},bX=bu.userAgent,bV,bC,e,bL=Object.prototype.toString,bG=Object.prototype.hasOwnProperty,bz=Array.prototype.push,bK=Array.prototype.slice,bO=String.prototype.trim,bv=Array.prototype.indexOf,bx={};bF.fn=bF.prototype={constructor:bF,init:function(b0,b4,b3){var b2,b5,b1,b6;if(!b0){return this}if(b0.nodeType){this.context=this[0]=b0;this.length=1;return this}if(b0==="body"&&!b4&&av.body){this.context=av;this[0]=av.body;this.selector=b0;this.length=1;return this}if(typeof b0==="string"){if(b0.charAt(0)==="<"&&b0.charAt(b0.length-1)===">"&&b0.length>=3){b2=[null,b0,null]}else{b2=bY.exec(b0)}if(b2&&(b2[1]||!b4)){if(b2[1]){b4=b4 instanceof bF?b4[0]:b4;b6=(b4?b4.ownerDocument||b4:av);b1=bA.exec(b0);if(b1){if(bF.isPlainObject(b4)){b0=[av.createElement(b1[1])];bF.fn.attr.call(b0,b4,true)}else{b0=[b6.createElement(b1[1])]}}else{b1=bF.buildFragment([b2[1]],[b6]);b0=(b1.cacheable?bF.clone(b1.fragment):b1.fragment).childNodes}return bF.merge(this,b0)}else{b5=av.getElementById(b2[2]);if(b5&&b5.parentNode){if(b5.id!==b2[2]){return b3.find(b0)}this.length=1;this[0]=b5}this.context=av;this.selector=b0;return this}}else{if(!b4||b4.jquery){return(b4||b3).find(b0)}else{return this.constructor(b4).find(b0)}}}else{if(bF.isFunction(b0)){return b3.ready(b0)}}if(b0.selector!==L){this.selector=b0.selector;this.context=b0.context}return bF.makeArray(b0,this)},selector:"",jquery:"1.7.1",length:0,size:function(){return this.length},toArray:function(){return bK.call(this,0)},get:function(b0){return b0==null?this.toArray():(b0<0?this[this.length+b0]:this[b0])},pushStack:function(b1,b3,b0){var b2=this.constructor();if(bF.isArray(b1)){bz.apply(b2,b1)}else{bF.merge(b2,b1)}b2.prevObject=this;b2.context=this.context;if(b3==="find"){b2.selector=this.selector+(this.selector?" ":"")+b0}else{if(b3){b2.selector=this.selector+"."+b3+"("+b0+")"}}return b2},each:function(b1,b0){return bF.each(this,b1,b0)},ready:function(b0){bF.bindReady();bC.add(b0);return this},eq:function(b0){b0=+b0;return b0===-1?this.slice(b0):this.slice(b0,b0+1)},first:function(){return this.eq(0)},last:function(){return this.eq(-1)},slice:function(){return this.pushStack(bK.apply(this,arguments),"slice",bK.call(arguments).join(","))},map:function(b0){return this.pushStack(bF.map(this,function(b2,b1){return b0.call(b2,b1,b2)}))},end:function(){return this.prevObject||this.constructor(null)},push:bz,sort:[].sort,splice:[].splice};bF.fn.init.prototype=bF.fn;bF.extend=bF.fn.extend=function(){var b9,b2,b0,b1,b6,b7,b5=arguments[0]||{},b4=1,b3=arguments.length,b8=false;if(typeof b5==="boolean"){b8=b5;b5=arguments[1]||{};b4=2}if(typeof b5!=="object"&&!bF.isFunction(b5)){b5={}}if(b3===b4){b5=this;--b4}for(;b4<b3;b4++){if((b9=arguments[b4])!=null){for(b2 in b9){b0=b5[b2];b1=b9[b2];if(b5===b1){continue}if(b8&&b1&&(bF.isPlainObject(b1)||(b6=bF.isArray(b1)))){if(b6){b6=false;b7=b0&&bF.isArray(b0)?b0:[]}else{b7=b0&&bF.isPlainObject(b0)?b0:{}}b5[b2]=bF.extend(b8,b7,b1)}else{if(b1!==L){b5[b2]=b1}}}}}return b5};bF.extend({noConflict:function(b0){if(bb.$===bF){bb.$=bH}if(b0&&bb.jQuery===bF){bb.jQuery=bU}return bF},isReady:false,readyWait:1,holdReady:function(b0){if(b0){bF.readyWait++}else{bF.ready(true)}},ready:function(b0){if((b0===true&&!--bF.readyWait)||(b0!==true&&!bF.isReady)){if(!av.body){return setTimeout(bF.ready,1)}bF.isReady=true;if(b0!==true&&--bF.readyWait>0){return}bC.fireWith(av,[bF]);if(bF.fn.trigger){bF(av).trigger("ready").off("ready")}}},bindReady:function(){if(bC){return}bC=bF.Callbacks("once memory");if(av.readyState==="complete"){return setTimeout(bF.ready,1)}if(av.addEventListener){av.addEventListener("DOMContentLoaded",e,false);bb.addEventListener("load",bF.ready,false)}else{if(av.attachEvent){av.attachEvent("onreadystatechange",e);bb.attachEvent("onload",bF.ready);var b0=false;try{b0=bb.frameElement==null}catch(b1){}if(av.documentElement.doScroll&&b0){bw()}}}},isFunction:function(b0){return bF.type(b0)==="function"},isArray:Array.isArray||function(b0){return bF.type(b0)==="array"},isWindow:function(b0){return b0&&typeof b0==="object"&&"setInterval" in b0},isNumeric:function(b0){return !isNaN(parseFloat(b0))&&isFinite(b0)},type:function(b0){return b0==null?String(b0):bx[bL.call(b0)]||"object"},isPlainObject:function(b2){if(!b2||bF.type(b2)!=="object"||b2.nodeType||bF.isWindow(b2)){return false}try{if(b2.constructor&&!bG.call(b2,"constructor")&&!bG.call(b2.constructor.prototype,"isPrototypeOf")){return false}}catch(b1){return false}var b0;for(b0 in b2){}return b0===L||bG.call(b2,b0)},isEmptyObject:function(b1){for(var b0 in b1){return false}return true},error:function(b0){throw new Error(b0)},parseJSON:function(b0){if(typeof b0!=="string"||!b0){return null}b0=bF.trim(b0);if(bb.JSON&&bb.JSON.parse){return bb.JSON.parse(b0)}if(bN.test(b0.replace(bW,"@").replace(bP,"]").replace(bJ,""))){return(new Function("return "+b0))()}bF.error("Invalid JSON: "+b0)},parseXML:function(b2){var b0,b1;try{if(bb.DOMParser){b1=new DOMParser();b0=b1.parseFromString(b2,"text/xml")}else{b0=new ActiveXObject("Microsoft.XMLDOM");b0.async="false";b0.loadXML(b2)}}catch(b3){b0=L}if(!b0||!b0.documentElement||b0.getElementsByTagName("parsererror").length){bF.error("Invalid XML: "+b2)}return b0},noop:function(){},globalEval:function(b0){if(b0&&bM.test(b0)){(bb.execScript||function(b1){bb["eval"].call(bb,b1)})(b0)}},camelCase:function(b0){return b0.replace(bZ,"ms-").replace(bB,bT)},nodeName:function(b1,b0){return b1.nodeName&&b1.nodeName.toUpperCase()===b0.toUpperCase()},each:function(b3,b6,b2){var b1,b4=0,b5=b3.length,b0=b5===L||bF.isFunction(b3);if(b2){if(b0){for(b1 in b3){if(b6.apply(b3[b1],b2)===false){break}}}else{for(;b4<b5;){if(b6.apply(b3[b4++],b2)===false){break}}}}else{if(b0){for(b1 in b3){if(b6.call(b3[b1],b1,b3[b1])===false){break}}}else{for(;b4<b5;){if(b6.call(b3[b4],b4,b3[b4++])===false){break}}}}return b3},trim:bO?function(b0){return b0==null?"":bO.call(b0)}:function(b0){return b0==null?"":b0.toString().replace(bI,"").replace(bE,"")},makeArray:function(b3,b1){var b0=b1||[];if(b3!=null){var b2=bF.type(b3);if(b3.length==null||b2==="string"||b2==="function"||b2==="regexp"||bF.isWindow(b3)){bz.call(b0,b3)}else{bF.merge(b0,b3)}}return b0},inArray:function(b2,b3,b1){var b0;if(b3){if(bv){return bv.call(b3,b2,b1)}b0=b3.length;b1=b1?b1<0?Math.max(0,b0+b1):b1:0;for(;b1<b0;b1++){if(b1 in b3&&b3[b1]===b2){return b1}}}return -1},merge:function(b4,b2){var b3=b4.length,b1=0;if(typeof b2.length==="number"){for(var b0=b2.length;b1<b0;b1++){b4[b3++]=b2[b1]}}else{while(b2[b1]!==L){b4[b3++]=b2[b1++]}}b4.length=b3;return b4},grep:function(b1,b6,b0){var b2=[],b5;b0=!!b0;for(var b3=0,b4=b1.length;b3<b4;b3++){b5=!!b6(b1[b3],b3);if(b0!==b5){b2.push(b1[b3])}}return b2},map:function(b0,b7,b8){var b5,b6,b4=[],b2=0,b1=b0.length,b3=b0 instanceof bF||b1!==L&&typeof b1==="number"&&((b1>0&&b0[0]&&b0[b1-1])||b1===0||bF.isArray(b0));if(b3){for(;b2<b1;b2++){b5=b7(b0[b2],b2,b8);if(b5!=null){b4[b4.length]=b5}}}else{for(b6 in b0){b5=b7(b0[b6],b6,b8);if(b5!=null){b4[b4.length]=b5}}}return b4.concat.apply([],b4)},guid:1,proxy:function(b4,b3){if(typeof b3==="string"){var b2=b4[b3];b3=b4;b4=b2}if(!bF.isFunction(b4)){return L}var b0=bK.call(arguments,2),b1=function(){return b4.apply(b3,b0.concat(bK.call(arguments)))};b1.guid=b4.guid=b4.guid||b1.guid||bF.guid++;return b1},access:function(b0,b8,b6,b2,b5,b7){var b1=b0.length;if(typeof b8==="object"){for(var b3 in b8){bF.access(b0,b3,b8[b3],b2,b5,b6)}return b0}if(b6!==L){b2=!b7&&b2&&bF.isFunction(b6);for(var b4=0;b4<b1;b4++){b5(b0[b4],b8,b2?b6.call(b0[b4],b4,b5(b0[b4],b8)):b6,b7)}return b0}return b1?b5(b0[0],b8):L},now:function(){return(new Date()).getTime()},uaMatch:function(b1){b1=b1.toLowerCase();var b0=by.exec(b1)||bR.exec(b1)||bQ.exec(b1)||b1.indexOf("compatible")<0&&bS.exec(b1)||[];return{browser:b0[1]||"",version:b0[2]||"0"}},sub:function(){function b0(b3,b4){return new b0.fn.init(b3,b4)}bF.extend(true,b0,this);b0.superclass=this;b0.fn=b0.prototype=this();b0.fn.constructor=b0;b0.sub=this.sub;b0.fn.init=function b2(b3,b4){if(b4&&b4 instanceof bF&&!(b4 instanceof b0)){b4=b0(b4)}return bF.fn.init.call(this,b3,b4,b1)};b0.fn.init.prototype=b0.fn;var b1=b0(av);return b0},browser:{}});bF.each("Boolean Number String Function Array Date RegExp Object".split(" "),function(b1,b0){bx["[object "+b0+"]"]=b0.toLowerCase()});bV=bF.uaMatch(bX);if(bV.browser){bF.browser[bV.browser]=true;bF.browser.version=bV.version}if(bF.browser.webkit){bF.browser.safari=true}if(bM.test("\xA0")){bI=/^[\s\xA0]+/;bE=/[\s\xA0]+$/}bD=bF(av);if(av.addEventListener){e=function(){av.removeEventListener("DOMContentLoaded",e,false);bF.ready()}}else{if(av.attachEvent){e=function(){if(av.readyState==="complete"){av.detachEvent("onreadystatechange",e);bF.ready()}}}}function bw(){if(bF.isReady){return}try{av.documentElement.doScroll("left")}catch(b0){setTimeout(bw,1);return}bF.ready()}return bF})();var a2={};function X(e){var bv=a2[e]={},bw,bx;e=e.split(/\s+/);for(bw=0,bx=e.length;bw<bx;bw++){bv[e[bw]]=true}return bv}b.Callbacks=function(bw){bw=bw?(a2[bw]||X(bw)):{};var bB=[],bC=[],bx,by,bv,bz,bA,bE=function(bF){var bG,bJ,bI,bH,bK;for(bG=0,bJ=bF.length;bG<bJ;bG++){bI=bF[bG];bH=b.type(bI);if(bH==="array"){bE(bI)}else{if(bH==="function"){if(!bw.unique||!bD.has(bI)){bB.push(bI)}}}}},e=function(bG,bF){bF=bF||[];bx=!bw.memory||[bG,bF];by=true;bA=bv||0;bv=0;bz=bB.length;for(;bB&&bA<bz;bA++){if(bB[bA].apply(bG,bF)===false&&bw.stopOnFalse){bx=true;break}}by=false;if(bB){if(!bw.once){if(bC&&bC.length){bx=bC.shift();bD.fireWith(bx[0],bx[1])}}else{if(bx===true){bD.disable()}else{bB=[]}}}},bD={add:function(){if(bB){var bF=bB.length;bE(arguments);if(by){bz=bB.length}else{if(bx&&bx!==true){bv=bF;e(bx[0],bx[1])}}}return this},remove:function(){if(bB){var bF=arguments,bH=0,bI=bF.length;for(;bH<bI;bH++){for(var bG=0;bG<bB.length;bG++){if(bF[bH]===bB[bG]){if(by){if(bG<=bz){bz--;if(bG<=bA){bA--}}}bB.splice(bG--,1);if(bw.unique){break}}}}}return this},has:function(bG){if(bB){var bF=0,bH=bB.length;for(;bF<bH;bF++){if(bG===bB[bF]){return true}}}return false},empty:function(){bB=[];return this},disable:function(){bB=bC=bx=L;return this},disabled:function(){return !bB},lock:function(){bC=L;if(!bx||bx===true){bD.disable()}return this},locked:function(){return !bC},fireWith:function(bG,bF){if(bC){if(by){if(!bw.once){bC.push([bG,bF])}}else{if(!(bw.once&&bx)){e(bG,bF)}}}return this},fire:function(){bD.fireWith(this,arguments);return this},fired:function(){return !!bx}};return bD};var aJ=[].slice;b.extend({Deferred:function(by){var bx=b.Callbacks("once memory"),bw=b.Callbacks("once memory"),bv=b.Callbacks("memory"),e="pending",bA={resolve:bx,reject:bw,notify:bv},bC={done:bx.add,fail:bw.add,progress:bv.add,state:function(){return e},isResolved:bx.fired,isRejected:bw.fired,then:function(bE,bD,bF){bB.done(bE).fail(bD).progress(bF);return this},always:function(){bB.done.apply(bB,arguments).fail.apply(bB,arguments);return this},pipe:function(bF,bE,bD){return b.Deferred(function(bG){b.each({done:[bF,"resolve"],fail:[bE,"reject"],progress:[bD,"notify"]},function(bI,bL){var bH=bL[0],bK=bL[1],bJ;if(b.isFunction(bH)){bB[bI](function(){bJ=bH.apply(this,arguments);if(bJ&&b.isFunction(bJ.promise)){bJ.promise().then(bG.resolve,bG.reject,bG.notify)}else{bG[bK+"With"](this===bB?bG:this,[bJ])}})}else{bB[bI](bG[bK])}})}).promise()},promise:function(bE){if(bE==null){bE=bC}else{for(var bD in bC){bE[bD]=bC[bD]}}return bE}},bB=bC.promise({}),bz;for(bz in bA){bB[bz]=bA[bz].fire;bB[bz+"With"]=bA[bz].fireWith}bB.done(function(){e="resolved"},bw.disable,bv.lock).fail(function(){e="rejected"},bx.disable,bv.lock);if(by){by.call(bB,bB)}return bB},when:function(bA){var bx=aJ.call(arguments,0),bv=0,e=bx.length,bB=new Array(e),bw=e,by=e,bC=e<=1&&bA&&b.isFunction(bA.promise)?bA:b.Deferred(),bE=bC.promise();function bD(bF){return function(bG){bx[bF]=arguments.length>1?aJ.call(arguments,0):bG;if(!(--bw)){bC.resolveWith(bC,bx)}}}function bz(bF){return function(bG){bB[bF]=arguments.length>1?aJ.call(arguments,0):bG;bC.notifyWith(bE,bB)}}if(e>1){for(;bv<e;bv++){if(bx[bv]&&bx[bv].promise&&b.isFunction(bx[bv].promise)){bx[bv].promise().then(bD(bv),bC.reject,bz(bv))}else{--bw}}if(!bw){bC.resolveWith(bC,bx)}}else{if(bC!==bA){bC.resolveWith(bC,e?[bA]:[])}}return bE}});b.support=(function(){var bJ,bI,bF,bG,bx,bE,bA,bD,bz,bK,bB,by,bw,bv=av.createElement("div"),bH=av.documentElement;bv.setAttribute("className","t");bv.innerHTML=" <link/><table></table><a href='/a' style='top:1px;float:left;opacity:.55;'>a</a><input type='checkbox'/>";bI=bv.getElementsByTagName("*");bF=bv.getElementsByTagName("a")[0];if(!bI||!bI.length||!bF){return{}}bG=av.createElement("select");bx=bG.appendChild(av.createElement("option"));bE=bv.getElementsByTagName("input")[0];bJ={leadingWhitespace:(bv.firstChild.nodeType===3),tbody:!bv.getElementsByTagName("tbody").length,htmlSerialize:!!bv.getElementsByTagName("link").length,style:/top/.test(bF.getAttribute("style")),hrefNormalized:(bF.getAttribute("href")==="/a"),opacity:/^0.55/.test(bF.style.opacity),cssFloat:!!bF.style.cssFloat,checkOn:(bE.value==="on"),optSelected:bx.selected,getSetAttribute:bv.className!=="t",enctype:!!av.createElement("form").enctype,html5Clone:av.createElement("nav").cloneNode(true).outerHTML!=="<:nav></:nav>",submitBubbles:true,changeBubbles:true,focusinBubbles:false,deleteExpando:true,noCloneEvent:true,inlineBlockNeedsLayout:false,shrinkWrapBlocks:false,reliableMarginRight:true};bE.checked=true;bJ.noCloneChecked=bE.cloneNode(true).checked;bG.disabled=true;bJ.optDisabled=!bx.disabled;try{delete bv.test}catch(bC){bJ.deleteExpando=false}if(!bv.addEventListener&&bv.attachEvent&&bv.fireEvent){bv.attachEvent("onclick",function(){bJ.noCloneEvent=false});bv.cloneNode(true).fireEvent("onclick")}bE=av.createElement("input");bE.value="t";bE.setAttribute("type","radio");bJ.radioValue=bE.value==="t";bE.setAttribute("checked","checked");bv.appendChild(bE);bD=av.createDocumentFragment();bD.appendChild(bv.lastChild);bJ.checkClone=bD.cloneNode(true).cloneNode(true).lastChild.checked;bJ.appendChecked=bE.checked;bD.removeChild(bE);bD.appendChild(bv);bv.innerHTML="";if(bb.getComputedStyle){bA=av.createElement("div");bA.style.width="0";bA.style.marginRight="0";bv.style.width="2px";bv.appendChild(bA);bJ.reliableMarginRight=(parseInt((bb.getComputedStyle(bA,null)||{marginRight:0}).marginRight,10)||0)===0}if(bv.attachEvent){for(by in {submit:1,change:1,focusin:1}){bB="on"+by;bw=(bB in bv);if(!bw){bv.setAttribute(bB,"return;");bw=(typeof bv[bB]==="function")}bJ[by+"Bubbles"]=bw}}bD.removeChild(bv);bD=bG=bx=bA=bv=bE=null;b(function(){var bM,bU,bV,bT,bN,bO,bL,bS,bR,e,bP,bQ=av.getElementsByTagName("body")[0];if(!bQ){return}bL=1;bS="position:absolute;top:0;left:0;width:1px;height:1px;margin:0;";bR="visibility:hidden;border:0;";e="style='"+bS+"border:5px solid #000;padding:0;'";bP="<div "+e+"><div></div></div><table "+e+" cellpadding='0' cellspacing='0'><tr><td></td></tr></table>";bM=av.createElement("div");bM.style.cssText=bR+"width:0;height:0;position:static;top:0;margin-top:"+bL+"px";bQ.insertBefore(bM,bQ.firstChild);bv=av.createElement("div");bM.appendChild(bv);bv.innerHTML="<table><tr><td style='padding:0;border:0;display:none'></td><td>t</td></tr></table>";bz=bv.getElementsByTagName("td");bw=(bz[0].offsetHeight===0);bz[0].style.display="";bz[1].style.display="none";bJ.reliableHiddenOffsets=bw&&(bz[0].offsetHeight===0);bv.innerHTML="";bv.style.width=bv.style.paddingLeft="1px";b.boxModel=bJ.boxModel=bv.offsetWidth===2;if(typeof bv.style.zoom!=="undefined"){bv.style.display="inline";bv.style.zoom=1;bJ.inlineBlockNeedsLayout=(bv.offsetWidth===2);bv.style.display="";bv.innerHTML="<div style='width:4px;'></div>";bJ.shrinkWrapBlocks=(bv.offsetWidth!==2)}bv.style.cssText=bS+bR;bv.innerHTML=bP;bU=bv.firstChild;bV=bU.firstChild;bN=bU.nextSibling.firstChild.firstChild;bO={doesNotAddBorder:(bV.offsetTop!==5),doesAddBorderForTableAndCells:(bN.offsetTop===5)};bV.style.position="fixed";bV.style.top="20px";bO.fixedPosition=(bV.offsetTop===20||bV.offsetTop===15);bV.style.position=bV.style.top="";bU.style.overflow="hidden";bU.style.position="relative";bO.subtractsBorderForOverflowNotVisible=(bV.offsetTop===-5);bO.doesNotIncludeMarginInBodyOffset=(bQ.offsetTop!==bL);bQ.removeChild(bM);bv=bM=null;b.extend(bJ,bO)});return bJ})();var aS=/^(?:\{.*\}|\[.*\])$/,aA=/([A-Z])/g;b.extend({cache:{},uuid:0,expando:"jQuery"+(b.fn.jquery+Math.random()).replace(/\D/g,""),noData:{embed:true,object:"clsid:D27CDB6E-AE6D-11cf-96B8-444553540000",applet:true},hasData:function(e){e=e.nodeType?b.cache[e[b.expando]]:e[b.expando];return !!e&&!S(e)},data:function(bx,bv,bz,by){if(!b.acceptData(bx)){return}var bG,bA,bD,bE=b.expando,bC=typeof bv==="string",bF=bx.nodeType,e=bF?b.cache:bx,bw=bF?bx[bE]:bx[bE]&&bE,bB=bv==="events";if((!bw||!e[bw]||(!bB&&!by&&!e[bw].data))&&bC&&bz===L){return}if(!bw){if(bF){bx[bE]=bw=++b.uuid}else{bw=bE}}if(!e[bw]){e[bw]={};if(!bF){e[bw].toJSON=b.noop}}if(typeof bv==="object"||typeof bv==="function"){if(by){e[bw]=b.extend(e[bw],bv)}else{e[bw].data=b.extend(e[bw].data,bv)}}bG=bA=e[bw];if(!by){if(!bA.data){bA.data={}}bA=bA.data}if(bz!==L){bA[b.camelCase(bv)]=bz}if(bB&&!bA[bv]){return bG.events}if(bC){bD=bA[bv];if(bD==null){bD=bA[b.camelCase(bv)]}}else{bD=bA}return bD},removeData:function(bx,bv,by){if(!b.acceptData(bx)){return}var bB,bA,bz,bC=b.expando,bD=bx.nodeType,e=bD?b.cache:bx,bw=bD?bx[bC]:bC;if(!e[bw]){return}if(bv){bB=by?e[bw]:e[bw].data;if(bB){if(!b.isArray(bv)){if(bv in bB){bv=[bv]}else{bv=b.camelCase(bv);if(bv in bB){bv=[bv]}else{bv=bv.split(" ")}}}for(bA=0,bz=bv.length;bA<bz;bA++){delete bB[bv[bA]]}if(!(by?S:b.isEmptyObject)(bB)){return}}}if(!by){delete e[bw].data;if(!S(e[bw])){return}}if(b.support.deleteExpando||!e.setInterval){delete e[bw]}else{e[bw]=null}if(bD){if(b.support.deleteExpando){delete bx[bC]}else{if(bx.removeAttribute){bx.removeAttribute(bC)}else{bx[bC]=null}}}},_data:function(bv,e,bw){return b.data(bv,e,bw,true)},acceptData:function(bv){if(bv.nodeName){var e=b.noData[bv.nodeName.toLowerCase()];if(e){return !(e===true||bv.getAttribute("classid")!==e)}}return true}});b.fn.extend({data:function(by,bA){var bB,e,bw,bz=null;if(typeof by==="undefined"){if(this.length){bz=b.data(this[0]);if(this[0].nodeType===1&&!b._data(this[0],"parsedAttrs")){e=this[0].attributes;for(var bx=0,bv=e.length;bx<bv;bx++){bw=e[bx].name;if(bw.indexOf("data-")===0){bw=b.camelCase(bw.substring(5));a5(this[0],bw,bz[bw])}}b._data(this[0],"parsedAttrs",true)}}return bz}else{if(typeof by==="object"){return this.each(function(){b.data(this,by)})}}bB=by.split(".");bB[1]=bB[1]?"."+bB[1]:"";if(bA===L){bz=this.triggerHandler("getData"+bB[1]+"!",[bB[0]]);if(bz===L&&this.length){bz=b.data(this[0],by);bz=a5(this[0],by,bz)}return bz===L&&bB[1]?this.data(bB[0]):bz}else{return this.each(function(){var bC=b(this),bD=[bB[0],bA];bC.triggerHandler("setData"+bB[1]+"!",bD);b.data(this,by,bA);bC.triggerHandler("changeData"+bB[1]+"!",bD)})}},removeData:function(e){return this.each(function(){b.removeData(this,e)})}});function a5(bx,bw,by){if(by===L&&bx.nodeType===1){var bv="data-"+bw.replace(aA,"-$1").toLowerCase();by=bx.getAttribute(bv);if(typeof by==="string"){try{by=by==="true"?true:by==="false"?false:by==="null"?null:b.isNumeric(by)?parseFloat(by):aS.test(by)?b.parseJSON(by):by}catch(bz){}b.data(bx,bw,by)}else{by=L}}return by}function S(bv){for(var e in bv){if(e==="data"&&b.isEmptyObject(bv[e])){continue}if(e!=="toJSON"){return false}}return true}function bi(by,bx,bA){var bw=bx+"defer",bv=bx+"queue",e=bx+"mark",bz=b._data(by,bw);if(bz&&(bA==="queue"||!b._data(by,bv))&&(bA==="mark"||!b._data(by,e))){setTimeout(function(){if(!b._data(by,bv)&&!b._data(by,e)){b.removeData(by,bw,true);bz.fire()}},0)}}b.extend({_mark:function(bv,e){if(bv){e=(e||"fx")+"mark";b._data(bv,e,(b._data(bv,e)||0)+1)}},_unmark:function(by,bx,bv){if(by!==true){bv=bx;bx=by;by=false}if(bx){bv=bv||"fx";var e=bv+"mark",bw=by?0:((b._data(bx,e)||1)-1);if(bw){b._data(bx,e,bw)}else{b.removeData(bx,e,true);bi(bx,bv,"mark")}}},queue:function(bv,e,bx){var bw;if(bv){e=(e||"fx")+"queue";bw=b._data(bv,e);if(bx){if(!bw||b.isArray(bx)){bw=b._data(bv,e,b.makeArray(bx))}else{bw.push(bx)}}return bw||[]}},dequeue:function(by,bx){bx=bx||"fx";var bv=b.queue(by,bx),bw=bv.shift(),e={};if(bw==="inprogress"){bw=bv.shift()}if(bw){if(bx==="fx"){bv.unshift("inprogress")}b._data(by,bx+".run",e);bw.call(by,function(){b.dequeue(by,bx)},e)}if(!bv.length){b.removeData(by,bx+"queue "+bx+".run",true);bi(by,bx,"queue")}}});b.fn.extend({queue:function(e,bv){if(typeof e!=="string"){bv=e;e="fx"}if(bv===L){return b.queue(this[0],e)}return this.each(function(){var bw=b.queue(this,e,bv);if(e==="fx"&&bw[0]!=="inprogress"){b.dequeue(this,e)}})},dequeue:function(e){return this.each(function(){b.dequeue(this,e)})},delay:function(bv,e){bv=b.fx?b.fx.speeds[bv]||bv:bv;e=e||"fx";return this.queue(e,function(bx,bw){var by=setTimeout(bx,bv);bw.stop=function(){clearTimeout(by)}})},clearQueue:function(e){return this.queue(e||"fx",[])},promise:function(bD,bw){if(typeof bD!=="string"){bw=bD;bD=L}bD=bD||"fx";var e=b.Deferred(),bv=this,by=bv.length,bB=1,bz=bD+"defer",bA=bD+"queue",bC=bD+"mark",bx;function bE(){if(!(--bB)){e.resolveWith(bv,[bv])}}while(by--){if((bx=b.data(bv[by],bz,L,true)||(b.data(bv[by],bA,L,true)||b.data(bv[by],bC,L,true))&&b.data(bv[by],bz,b.Callbacks("once memory"),true))){bB++;bx.add(bE)}}bE();return e.promise()}});var aP=/[\n\t\r]/g,af=/\s+/,aU=/\r/g,g=/^(?:button|input)$/i,D=/^(?:button|input|object|select|textarea)$/i,l=/^a(?:rea)?$/i,ao=/^(?:autofocus|autoplay|async|checked|controls|defer|disabled|hidden|loop|multiple|open|readonly|required|scoped|selected)$/i,F=b.support.getSetAttribute,be,aY,aF;b.fn.extend({attr:function(e,bv){return b.access(this,e,bv,true,b.attr)},removeAttr:function(e){return this.each(function(){b.removeAttr(this,e)})},prop:function(e,bv){return b.access(this,e,bv,true,b.prop)},removeProp:function(e){e=b.propFix[e]||e;return this.each(function(){try{this[e]=L;delete this[e]}catch(bv){}})},addClass:function(by){var bA,bw,bv,bx,bz,bB,e;if(b.isFunction(by)){return this.each(function(bC){b(this).addClass(by.call(this,bC,this.className))})}if(by&&typeof by==="string"){bA=by.split(af);for(bw=0,bv=this.length;bw<bv;bw++){bx=this[bw];if(bx.nodeType===1){if(!bx.className&&bA.length===1){bx.className=by}else{bz=" "+bx.className+" ";for(bB=0,e=bA.length;bB<e;bB++){if(!~bz.indexOf(" "+bA[bB]+" ")){bz+=bA[bB]+" "}}bx.className=b.trim(bz)}}}}return this},removeClass:function(bz){var bA,bw,bv,by,bx,bB,e;if(b.isFunction(bz)){return this.each(function(bC){b(this).removeClass(bz.call(this,bC,this.className))})}if((bz&&typeof bz==="string")||bz===L){bA=(bz||"").split(af);for(bw=0,bv=this.length;bw<bv;bw++){by=this[bw];if(by.nodeType===1&&by.className){if(bz){bx=(" "+by.className+" ").replace(aP," ");for(bB=0,e=bA.length;bB<e;bB++){bx=bx.replace(" "+bA[bB]+" "," ")}by.className=b.trim(bx)}else{by.className=""}}}}return this},toggleClass:function(bx,bv){var bw=typeof bx,e=typeof bv==="boolean";if(b.isFunction(bx)){return this.each(function(by){b(this).toggleClass(bx.call(this,by,this.className,bv),bv)})}return this.each(function(){if(bw==="string"){var bA,bz=0,by=b(this),bB=bv,bC=bx.split(af);while((bA=bC[bz++])){bB=e?bB:!by.hasClass(bA);by[bB?"addClass":"removeClass"](bA)}}else{if(bw==="undefined"||bw==="boolean"){if(this.className){b._data(this,"__className__",this.className)}this.className=this.className||bx===false?"":b._data(this,"__className__")||""}}})},hasClass:function(e){var bx=" "+e+" ",bw=0,bv=this.length;for(;bw<bv;bw++){if(this[bw].nodeType===1&&(" "+this[bw].className+" ").replace(aP," ").indexOf(bx)>-1){return true}}return false},val:function(bx){var e,bv,by,bw=this[0];if(!arguments.length){if(bw){e=b.valHooks[bw.nodeName.toLowerCase()]||b.valHooks[bw.type];if(e&&"get" in e&&(bv=e.get(bw,"value"))!==L){return bv}bv=bw.value;return typeof bv==="string"?bv.replace(aU,""):bv==null?"":bv}return}by=b.isFunction(bx);return this.each(function(bA){var bz=b(this),bB;if(this.nodeType!==1){return}if(by){bB=bx.call(this,bA,bz.val())}else{bB=bx}if(bB==null){bB=""}else{if(typeof bB==="number"){bB+=""}else{if(b.isArray(bB)){bB=b.map(bB,function(bC){return bC==null?"":bC+""})}}}e=b.valHooks[this.nodeName.toLowerCase()]||b.valHooks[this.type];if(!e||!("set" in e)||e.set(this,bB,"value")===L){this.value=bB}})}});b.extend({valHooks:{option:{get:function(e){var bv=e.attributes.value;return !bv||bv.specified?e.value:e.text}},select:{get:function(e){var bA,bv,bz,bx,by=e.selectedIndex,bB=[],bC=e.options,bw=e.type==="select-one";if(by<0){return null}bv=bw?by:0;bz=bw?by+1:bC.length;for(;bv<bz;bv++){bx=bC[bv];if(bx.selected&&(b.support.optDisabled?!bx.disabled:bx.getAttribute("disabled")===null)&&(!bx.parentNode.disabled||!b.nodeName(bx.parentNode,"optgroup"))){bA=b(bx).val();if(bw){return bA}bB.push(bA)}}if(bw&&!bB.length&&bC.length){return b(bC[by]).val()}return bB},set:function(bv,bw){var e=b.makeArray(bw);b(bv).find("option").each(function(){this.selected=b.inArray(b(this).val(),e)>=0});if(!e.length){bv.selectedIndex=-1}return e}}},attrFn:{val:true,css:true,html:true,text:true,data:true,width:true,height:true,offset:true},attr:function(bA,bx,bB,bz){var bw,e,by,bv=bA.nodeType;if(!bA||bv===3||bv===8||bv===2){return}if(bz&&bx in b.attrFn){return b(bA)[bx](bB)}if(typeof bA.getAttribute==="undefined"){return b.prop(bA,bx,bB)}by=bv!==1||!b.isXMLDoc(bA);if(by){bx=bx.toLowerCase();e=b.attrHooks[bx]||(ao.test(bx)?aY:be)}if(bB!==L){if(bB===null){b.removeAttr(bA,bx);return}else{if(e&&"set" in e&&by&&(bw=e.set(bA,bB,bx))!==L){return bw}else{bA.setAttribute(bx,""+bB);return bB}}}else{if(e&&"get" in e&&by&&(bw=e.get(bA,bx))!==null){return bw}else{bw=bA.getAttribute(bx);return bw===null?L:bw}}},removeAttr:function(bx,bz){var by,bA,bv,e,bw=0;if(bz&&bx.nodeType===1){bA=bz.toLowerCase().split(af);e=bA.length;for(;bw<e;bw++){bv=bA[bw];if(bv){by=b.propFix[bv]||bv;b.attr(bx,bv,"");bx.removeAttribute(F?bv:by);if(ao.test(bv)&&by in bx){bx[by]=false}}}}},attrHooks:{type:{set:function(e,bv){if(g.test(e.nodeName)&&e.parentNode){b.error("type property can't be changed")}else{if(!b.support.radioValue&&bv==="radio"&&b.nodeName(e,"input")){var bw=e.value;e.setAttribute("type",bv);if(bw){e.value=bw}return bv}}}},value:{get:function(bv,e){if(be&&b.nodeName(bv,"button")){return be.get(bv,e)}return e in bv?bv.value:null},set:function(bv,bw,e){if(be&&b.nodeName(bv,"button")){return be.set(bv,bw,e)}bv.value=bw}}},propFix:{tabindex:"tabIndex",readonly:"readOnly","for":"htmlFor","class":"className",maxlength:"maxLength",cellspacing:"cellSpacing",cellpadding:"cellPadding",rowspan:"rowSpan",colspan:"colSpan",usemap:"useMap",frameborder:"frameBorder",contenteditable:"contentEditable"},prop:function(bz,bx,bA){var bw,e,by,bv=bz.nodeType;if(!bz||bv===3||bv===8||bv===2){return}by=bv!==1||!b.isXMLDoc(bz);if(by){bx=b.propFix[bx]||bx;e=b.propHooks[bx]}if(bA!==L){if(e&&"set" in e&&(bw=e.set(bz,bA,bx))!==L){return bw}else{return(bz[bx]=bA)}}else{if(e&&"get" in e&&(bw=e.get(bz,bx))!==null){return bw}else{return bz[bx]}}},propHooks:{tabIndex:{get:function(bv){var e=bv.getAttributeNode("tabindex");return e&&e.specified?parseInt(e.value,10):D.test(bv.nodeName)||l.test(bv.nodeName)&&bv.href?0:L}}}});b.attrHooks.tabindex=b.propHooks.tabIndex;aY={get:function(bv,e){var bx,bw=b.prop(bv,e);return bw===true||typeof bw!=="boolean"&&(bx=bv.getAttributeNode(e))&&bx.nodeValue!==false?e.toLowerCase():L},set:function(bv,bx,e){var bw;if(bx===false){b.removeAttr(bv,e)}else{bw=b.propFix[e]||e;if(bw in bv){bv[bw]=true}bv.setAttribute(e,e.toLowerCase())}return e}};if(!F){aF={name:true,id:true};be=b.valHooks.button={get:function(bw,bv){var e;e=bw.getAttributeNode(bv);return e&&(aF[bv]?e.nodeValue!=="":e.specified)?e.nodeValue:L},set:function(bw,bx,bv){var e=bw.getAttributeNode(bv);if(!e){e=av.createAttribute(bv);bw.setAttributeNode(e)}return(e.nodeValue=bx+"")}};b.attrHooks.tabindex.set=be.set;b.each(["width","height"],function(bv,e){b.attrHooks[e]=b.extend(b.attrHooks[e],{set:function(bw,bx){if(bx===""){bw.setAttribute(e,"auto");return bx}}})});b.attrHooks.contenteditable={get:be.get,set:function(bv,bw,e){if(bw===""){bw="false"}be.set(bv,bw,e)}}}if(!b.support.hrefNormalized){b.each(["href","src","width","height"],function(bv,e){b.attrHooks[e]=b.extend(b.attrHooks[e],{get:function(bx){var bw=bx.getAttribute(e,2);return bw===null?L:bw}})})}if(!b.support.style){b.attrHooks.style={get:function(e){return e.style.cssText.toLowerCase()||L},set:function(e,bv){return(e.style.cssText=""+bv)}}}if(!b.support.optSelected){b.propHooks.selected=b.extend(b.propHooks.selected,{get:function(bv){var e=bv.parentNode;if(e){e.selectedIndex;if(e.parentNode){e.parentNode.selectedIndex}}return null}})}if(!b.support.enctype){b.propFix.enctype="encoding"}if(!b.support.checkOn){b.each(["radio","checkbox"],function(){b.valHooks[this]={get:function(e){return e.getAttribute("value")===null?"on":e.value}}})}b.each(["radio","checkbox"],function(){b.valHooks[this]=b.extend(b.valHooks[this],{set:function(e,bv){if(b.isArray(bv)){return(e.checked=b.inArray(b(e).val(),bv)>=0)}}})});var bd=/^(?:textarea|input|select)$/i,n=/^([^\.]*)?(?:\.(.+))?$/,J=/\bhover(\.\S+)?\b/,aO=/^key/,bf=/^(?:mouse|contextmenu)|click/,T=/^(?:focusinfocus|focusoutblur)$/,U=/^(\w*)(?:#([\w\-]+))?(?:\.([\w\-]+))?$/,Y=function(e){var bv=U.exec(e);if(bv){bv[1]=(bv[1]||"").toLowerCase();bv[3]=bv[3]&&new RegExp("(?:^|\\s)"+bv[3]+"(?:\\s|$)")}return bv},j=function(bw,e){var bv=bw.attributes||{};return((!e[1]||bw.nodeName.toLowerCase()===e[1])&&(!e[2]||(bv.id||{}).value===e[2])&&(!e[3]||e[3].test((bv["class"]||{}).value)))},bt=function(e){return b.event.special.hover?e:e.replace(J,"mouseenter$1 mouseleave$1")};b.event={add:function(bx,bC,bJ,bA,by){var bD,bB,bK,bI,bH,bF,e,bG,bv,bz,bw,bE;if(bx.nodeType===3||bx.nodeType===8||!bC||!bJ||!(bD=b._data(bx))){return}if(bJ.handler){bv=bJ;bJ=bv.handler}if(!bJ.guid){bJ.guid=b.guid++}bK=bD.events;if(!bK){bD.events=bK={}}bB=bD.handle;if(!bB){bD.handle=bB=function(bL){return typeof b!=="undefined"&&(!bL||b.event.triggered!==bL.type)?b.event.dispatch.apply(bB.elem,arguments):L};bB.elem=bx}bC=b.trim(bt(bC)).split(" ");for(bI=0;bI<bC.length;bI++){bH=n.exec(bC[bI])||[];bF=bH[1];e=(bH[2]||"").split(".").sort();bE=b.event.special[bF]||{};bF=(by?bE.delegateType:bE.bindType)||bF;bE=b.event.special[bF]||{};bG=b.extend({type:bF,origType:bH[1],data:bA,handler:bJ,guid:bJ.guid,selector:by,quick:Y(by),namespace:e.join(".")},bv);bw=bK[bF];if(!bw){bw=bK[bF]=[];bw.delegateCount=0;if(!bE.setup||bE.setup.call(bx,bA,e,bB)===false){if(bx.addEventListener){bx.addEventListener(bF,bB,false)}else{if(bx.attachEvent){bx.attachEvent("on"+bF,bB)}}}}if(bE.add){bE.add.call(bx,bG);if(!bG.handler.guid){bG.handler.guid=bJ.guid}}if(by){bw.splice(bw.delegateCount++,0,bG)}else{bw.push(bG)}b.event.global[bF]=true}bx=null},global:{},remove:function(bJ,bE,bv,bH,bB){var bI=b.hasData(bJ)&&b._data(bJ),bF,bx,bz,bL,bC,bA,bG,bw,by,bK,bD,e;if(!bI||!(bw=bI.events)){return}bE=b.trim(bt(bE||"")).split(" ");for(bF=0;bF<bE.length;bF++){bx=n.exec(bE[bF])||[];bz=bL=bx[1];bC=bx[2];if(!bz){for(bz in bw){b.event.remove(bJ,bz+bE[bF],bv,bH,true)}continue}by=b.event.special[bz]||{};bz=(bH?by.delegateType:by.bindType)||bz;bD=bw[bz]||[];bA=bD.length;bC=bC?new RegExp("(^|\\.)"+bC.split(".").sort().join("\\.(?:.*\\.)?")+"(\\.|$)"):null;for(bG=0;bG<bD.length;bG++){e=bD[bG];if((bB||bL===e.origType)&&(!bv||bv.guid===e.guid)&&(!bC||bC.test(e.namespace))&&(!bH||bH===e.selector||bH==="**"&&e.selector)){bD.splice(bG--,1);if(e.selector){bD.delegateCount--}if(by.remove){by.remove.call(bJ,e)}}}if(bD.length===0&&bA!==bD.length){if(!by.teardown||by.teardown.call(bJ,bC)===false){b.removeEvent(bJ,bz,bI.handle)}delete bw[bz]}}if(b.isEmptyObject(bw)){bK=bI.handle;if(bK){bK.elem=null}b.removeData(bJ,["events","handle"],true)}},customEvent:{getData:true,setData:true,changeData:true},trigger:function(bv,bD,bA,bJ){if(bA&&(bA.nodeType===3||bA.nodeType===8)){return}var bG=bv.type||bv,bx=[],e,bw,bC,bH,bz,by,bF,bE,bB,bI;if(T.test(bG+b.event.triggered)){return}if(bG.indexOf("!")>=0){bG=bG.slice(0,-1);bw=true}if(bG.indexOf(".")>=0){bx=bG.split(".");bG=bx.shift();bx.sort()}if((!bA||b.event.customEvent[bG])&&!b.event.global[bG]){return}bv=typeof bv==="object"?bv[b.expando]?bv:new b.Event(bG,bv):new b.Event(bG);bv.type=bG;bv.isTrigger=true;bv.exclusive=bw;bv.namespace=bx.join(".");bv.namespace_re=bv.namespace?new RegExp("(^|\\.)"+bx.join("\\.(?:.*\\.)?")+"(\\.|$)"):null;by=bG.indexOf(":")<0?"on"+bG:"";if(!bA){e=b.cache;for(bC in e){if(e[bC].events&&e[bC].events[bG]){b.event.trigger(bv,bD,e[bC].handle.elem,true)}}return}bv.result=L;if(!bv.target){bv.target=bA}bD=bD!=null?b.makeArray(bD):[];bD.unshift(bv);bF=b.event.special[bG]||{};if(bF.trigger&&bF.trigger.apply(bA,bD)===false){return}bB=[[bA,bF.bindType||bG]];if(!bJ&&!bF.noBubble&&!b.isWindow(bA)){bI=bF.delegateType||bG;bH=T.test(bI+bG)?bA:bA.parentNode;bz=null;for(;bH;bH=bH.parentNode){bB.push([bH,bI]);bz=bH}if(bz&&bz===bA.ownerDocument){bB.push([bz.defaultView||bz.parentWindow||bb,bI])}}for(bC=0;bC<bB.length&&!bv.isPropagationStopped();bC++){bH=bB[bC][0];bv.type=bB[bC][1];bE=(b._data(bH,"events")||{})[bv.type]&&b._data(bH,"handle");if(bE){bE.apply(bH,bD)}bE=by&&bH[by];if(bE&&b.acceptData(bH)&&bE.apply(bH,bD)===false){bv.preventDefault()}}bv.type=bG;if(!bJ&&!bv.isDefaultPrevented()){if((!bF._default||bF._default.apply(bA.ownerDocument,bD)===false)&&!(bG==="click"&&b.nodeName(bA,"a"))&&b.acceptData(bA)){if(by&&bA[bG]&&((bG!=="focus"&&bG!=="blur")||bv.target.offsetWidth!==0)&&!b.isWindow(bA)){bz=bA[by];if(bz){bA[by]=null}b.event.triggered=bG;bA[bG]();b.event.triggered=L;if(bz){bA[by]=bz}}}}return bv.result},dispatch:function(e){e=b.event.fix(e||bb.event);var bz=((b._data(this,"events")||{})[e.type]||[]),bA=bz.delegateCount,bG=[].slice.call(arguments,0),by=!e.exclusive&&!e.namespace,bH=[],bC,bB,bK,bx,bF,bE,bv,bD,bI,bw,bJ;bG[0]=e;e.delegateTarget=this;if(bA&&!e.target.disabled&&!(e.button&&e.type==="click")){bx=b(this);bx.context=this.ownerDocument||this;for(bK=e.target;bK!=this;bK=bK.parentNode||this){bE={};bD=[];bx[0]=bK;for(bC=0;bC<bA;bC++){bI=bz[bC];bw=bI.selector;if(bE[bw]===L){bE[bw]=(bI.quick?j(bK,bI.quick):bx.is(bw))}if(bE[bw]){bD.push(bI)}}if(bD.length){bH.push({elem:bK,matches:bD})}}}if(bz.length>bA){bH.push({elem:this,matches:bz.slice(bA)})}for(bC=0;bC<bH.length&&!e.isPropagationStopped();bC++){bv=bH[bC];e.currentTarget=bv.elem;for(bB=0;bB<bv.matches.length&&!e.isImmediatePropagationStopped();bB++){bI=bv.matches[bB];if(by||(!e.namespace&&!bI.namespace)||e.namespace_re&&e.namespace_re.test(bI.namespace)){e.data=bI.data;e.handleObj=bI;bF=((b.event.special[bI.origType]||{}).handle||bI.handler).apply(bv.elem,bG);if(bF!==L){e.result=bF;if(bF===false){e.preventDefault();e.stopPropagation()}}}}}return e.result},props:"attrChange attrName relatedNode srcElement altKey bubbles cancelable ctrlKey currentTarget eventPhase metaKey relatedTarget shiftKey target timeStamp view which".split(" "),fixHooks:{},keyHooks:{props:"char charCode key keyCode".split(" "),filter:function(bv,e){if(bv.which==null){bv.which=e.charCode!=null?e.charCode:e.keyCode}return bv}},mouseHooks:{props:"button buttons clientX clientY fromElement offsetX offsetY pageX pageY screenX screenY toElement".split(" "),filter:function(bx,bw){var by,bz,e,bv=bw.button,bA=bw.fromElement;if(bx.pageX==null&&bw.clientX!=null){by=bx.target.ownerDocument||av;bz=by.documentElement;e=by.body;bx.pageX=bw.clientX+(bz&&bz.scrollLeft||e&&e.scrollLeft||0)-(bz&&bz.clientLeft||e&&e.clientLeft||0);bx.pageY=bw.clientY+(bz&&bz.scrollTop||e&&e.scrollTop||0)-(bz&&bz.clientTop||e&&e.clientTop||0)}if(!bx.relatedTarget&&bA){bx.relatedTarget=bA===bx.target?bw.toElement:bA}if(!bx.which&&bv!==L){bx.which=(bv&1?1:(bv&2?3:(bv&4?2:0)))}return bx}},fix:function(bw){if(bw[b.expando]){return bw}var bv,bz,e=bw,bx=b.event.fixHooks[bw.type]||{},by=bx.props?this.props.concat(bx.props):this.props;bw=b.Event(e);for(bv=by.length;bv;){bz=by[--bv];bw[bz]=e[bz]}if(!bw.target){bw.target=e.srcElement||av}if(bw.target.nodeType===3){bw.target=bw.target.parentNode}if(bw.metaKey===L){bw.metaKey=bw.ctrlKey}return bx.filter?bx.filter(bw,e):bw},special:{ready:{setup:b.bindReady},load:{noBubble:true},focus:{delegateType:"focusin"},blur:{delegateType:"focusout"},beforeunload:{setup:function(bw,bv,e){if(b.isWindow(this)){this.onbeforeunload=e}},teardown:function(bv,e){if(this.onbeforeunload===e){this.onbeforeunload=null}}}},simulate:function(bw,by,bx,bv){var bz=b.extend(new b.Event(),bx,{type:bw,isSimulated:true,originalEvent:{}});if(bv){b.event.trigger(bz,null,by)}else{b.event.dispatch.call(by,bz)}if(bz.isDefaultPrevented()){bx.preventDefault()}}};b.event.handle=b.event.dispatch;b.removeEvent=av.removeEventListener?function(bv,e,bw){if(bv.removeEventListener){bv.removeEventListener(e,bw,false)}}:function(bv,e,bw){if(bv.detachEvent){bv.detachEvent("on"+e,bw)}};b.Event=function(bv,e){if(!(this instanceof b.Event)){return new b.Event(bv,e)}if(bv&&bv.type){this.originalEvent=bv;this.type=bv.type;this.isDefaultPrevented=(bv.defaultPrevented||bv.returnValue===false||bv.getPreventDefault&&bv.getPreventDefault())?i:bk}else{this.type=bv}if(e){b.extend(this,e)}this.timeStamp=bv&&bv.timeStamp||b.now();this[b.expando]=true};function bk(){return false}function i(){return true}b.Event.prototype={preventDefault:function(){this.isDefaultPrevented=i;var bv=this.originalEvent;if(!bv){return}if(bv.preventDefault){bv.preventDefault()}else{bv.returnValue=false}},stopPropagation:function(){this.isPropagationStopped=i;var bv=this.originalEvent;if(!bv){return}if(bv.stopPropagation){bv.stopPropagation()}bv.cancelBubble=true},stopImmediatePropagation:function(){this.isImmediatePropagationStopped=i;this.stopPropagation()},isDefaultPrevented:bk,isPropagationStopped:bk,isImmediatePropagationStopped:bk};b.each({mouseenter:"mouseover",mouseleave:"mouseout"},function(bv,e){b.event.special[bv]={delegateType:e,bindType:e,handle:function(bz){var bB=this,bA=bz.relatedTarget,by=bz.handleObj,bw=by.selector,bx;if(!bA||(bA!==bB&&!b.contains(bB,bA))){bz.type=by.origType;bx=by.handler.apply(this,arguments);bz.type=e}return bx}}});if(!b.support.submitBubbles){b.event.special.submit={setup:function(){if(b.nodeName(this,"form")){return false}b.event.add(this,"click._submit keypress._submit",function(bx){var bw=bx.target,bv=b.nodeName(bw,"input")||b.nodeName(bw,"button")?bw.form:L;if(bv&&!bv._submit_attached){b.event.add(bv,"submit._submit",function(e){if(this.parentNode&&!e.isTrigger){b.event.simulate("submit",this.parentNode,e,true)}});bv._submit_attached=true}})},teardown:function(){if(b.nodeName(this,"form")){return false}b.event.remove(this,"._submit")}}}if(!b.support.changeBubbles){b.event.special.change={setup:function(){if(bd.test(this.nodeName)){if(this.type==="checkbox"||this.type==="radio"){b.event.add(this,"propertychange._change",function(e){if(e.originalEvent.propertyName==="checked"){this._just_changed=true}});b.event.add(this,"click._change",function(e){if(this._just_changed&&!e.isTrigger){this._just_changed=false;b.event.simulate("change",this,e,true)}})}return false}b.event.add(this,"beforeactivate._change",function(bw){var bv=bw.target;if(bd.test(bv.nodeName)&&!bv._change_attached){b.event.add(bv,"change._change",function(e){if(this.parentNode&&!e.isSimulated&&!e.isTrigger){b.event.simulate("change",this.parentNode,e,true)}});bv._change_attached=true}})},handle:function(bv){var e=bv.target;if(this!==e||bv.isSimulated||bv.isTrigger||(e.type!=="radio"&&e.type!=="checkbox")){return bv.handleObj.handler.apply(this,arguments)}},teardown:function(){b.event.remove(this,"._change");return bd.test(this.nodeName)}}}if(!b.support.focusinBubbles){b.each({focus:"focusin",blur:"focusout"},function(bx,e){var bv=0,bw=function(by){b.event.simulate(e,by.target,b.event.fix(by),true)};b.event.special[e]={setup:function(){if(bv++===0){av.addEventListener(bx,bw,true)}},teardown:function(){if(--bv===0){av.removeEventListener(bx,bw,true)}}}})}b.fn.extend({on:function(bw,e,bz,by,bv){var bA,bx;if(typeof bw==="object"){if(typeof e!=="string"){bz=e;e=L}for(bx in bw){this.on(bx,e,bz,bw[bx],bv)}return this}if(bz==null&&by==null){by=e;bz=e=L}else{if(by==null){if(typeof e==="string"){by=bz;bz=L}else{by=bz;bz=e;e=L}}}if(by===false){by=bk}else{if(!by){return this}}if(bv===1){bA=by;by=function(bB){b().off(bB);return bA.apply(this,arguments)};by.guid=bA.guid||(bA.guid=b.guid++)}return this.each(function(){b.event.add(this,bw,by,bz,e)})},one:function(bv,e,bx,bw){return this.on.call(this,bv,e,bx,bw,1)},off:function(bw,e,by){if(bw&&bw.preventDefault&&bw.handleObj){var bv=bw.handleObj;b(bw.delegateTarget).off(bv.namespace?bv.type+"."+bv.namespace:bv.type,bv.selector,bv.handler);return this}if(typeof bw==="object"){for(var bx in bw){this.off(bx,e,bw[bx])}return this}if(e===false||typeof e==="function"){by=e;e=L}if(by===false){by=bk}return this.each(function(){b.event.remove(this,bw,by,e)})},bind:function(e,bw,bv){return this.on(e,null,bw,bv)},unbind:function(e,bv){return this.off(e,null,bv)},live:function(e,bw,bv){b(this.context).on(e,this.selector,bw,bv);return this},die:function(e,bv){b(this.context).off(e,this.selector||"**",bv);return this},delegate:function(e,bv,bx,bw){return this.on(bv,e,bx,bw)},undelegate:function(e,bv,bw){return arguments.length==1?this.off(e,"**"):this.off(bv,e,bw)},trigger:function(e,bv){return this.each(function(){b.event.trigger(e,bv,this)})},triggerHandler:function(e,bv){if(this[0]){return b.event.trigger(e,bv,this[0],true)}},toggle:function(bx){var bv=arguments,e=bx.guid||b.guid++,bw=0,by=function(bz){var bA=(b._data(this,"lastToggle"+bx.guid)||0)%bw;b._data(this,"lastToggle"+bx.guid,bA+1);bz.preventDefault();return bv[bA].apply(this,arguments)||false};by.guid=e;while(bw<bv.length){bv[bw++].guid=e}return this.click(by)},hover:function(e,bv){return this.mouseenter(e).mouseleave(bv||e)}});b.each(("blur focus focusin focusout load resize scroll unload click dblclick mousedown mouseup mousemove mouseover mouseout mouseenter mouseleave change select submit keydown keypress keyup error contextmenu").split(" "),function(bv,e){b.fn[e]=function(bx,bw){if(bw==null){bw=bx;bx=null}return arguments.length>0?this.on(e,null,bx,bw):this.trigger(e)};if(b.attrFn){b.attrFn[e]=true}if(aO.test(e)){b.event.fixHooks[e]=b.event.keyHooks}if(bf.test(e)){b.event.fixHooks[e]=b.event.mouseHooks}});
+/*
+ * Sizzle CSS Selector Engine
+ * Copyright 2011, The Dojo Foundation
+ * Released under the MIT, BSD, and GPL Licenses.
+ * More information: http://sizzlejs.com/
+ */
+(function(){var bH=/((?:\((?:\([^()]+\)|[^()]+)+\)|\[(?:\[[^\[\]]*\]|['"][^'"]*['"]|[^\[\]'"]+)+\]|\\.|[^ >+~,(\[\\]+)+|[>+~])(\s*,\s*)?((?:.|\r|\n)*)/g,bC="sizcache"+(Math.random()+"").replace(".",""),bI=0,bL=Object.prototype.toString,bB=false,bA=true,bK=/\\/g,bO=/\r\n/g,bQ=/\W/;[0,0].sort(function(){bA=false;return 0});var by=function(bV,e,bY,bZ){bY=bY||[];e=e||av;var b1=e;if(e.nodeType!==1&&e.nodeType!==9){return[]}if(!bV||typeof bV!=="string"){return bY}var bS,b3,b6,bR,b2,b5,b4,bX,bU=true,bT=by.isXML(e),bW=[],b0=bV;do{bH.exec("");bS=bH.exec(b0);if(bS){b0=bS[3];bW.push(bS[1]);if(bS[2]){bR=bS[3];break}}}while(bS);if(bW.length>1&&bD.exec(bV)){if(bW.length===2&&bE.relative[bW[0]]){b3=bM(bW[0]+bW[1],e,bZ)}else{b3=bE.relative[bW[0]]?[e]:by(bW.shift(),e);while(bW.length){bV=bW.shift();if(bE.relative[bV]){bV+=bW.shift()}b3=bM(bV,b3,bZ)}}}else{if(!bZ&&bW.length>1&&e.nodeType===9&&!bT&&bE.match.ID.test(bW[0])&&!bE.match.ID.test(bW[bW.length-1])){b2=by.find(bW.shift(),e,bT);e=b2.expr?by.filter(b2.expr,b2.set)[0]:b2.set[0]}if(e){b2=bZ?{expr:bW.pop(),set:bF(bZ)}:by.find(bW.pop(),bW.length===1&&(bW[0]==="~"||bW[0]==="+")&&e.parentNode?e.parentNode:e,bT);b3=b2.expr?by.filter(b2.expr,b2.set):b2.set;if(bW.length>0){b6=bF(b3)}else{bU=false}while(bW.length){b5=bW.pop();b4=b5;if(!bE.relative[b5]){b5=""}else{b4=bW.pop()}if(b4==null){b4=e}bE.relative[b5](b6,b4,bT)}}else{b6=bW=[]}}if(!b6){b6=b3}if(!b6){by.error(b5||bV)}if(bL.call(b6)==="[object Array]"){if(!bU){bY.push.apply(bY,b6)}else{if(e&&e.nodeType===1){for(bX=0;b6[bX]!=null;bX++){if(b6[bX]&&(b6[bX]===true||b6[bX].nodeType===1&&by.contains(e,b6[bX]))){bY.push(b3[bX])}}}else{for(bX=0;b6[bX]!=null;bX++){if(b6[bX]&&b6[bX].nodeType===1){bY.push(b3[bX])}}}}}else{bF(b6,bY)}if(bR){by(bR,b1,bY,bZ);by.uniqueSort(bY)}return bY};by.uniqueSort=function(bR){if(bJ){bB=bA;bR.sort(bJ);if(bB){for(var e=1;e<bR.length;e++){if(bR[e]===bR[e-1]){bR.splice(e--,1)}}}}return bR};by.matches=function(e,bR){return by(e,null,null,bR)};by.matchesSelector=function(e,bR){return by(bR,null,null,[e]).length>0};by.find=function(bX,e,bY){var bW,bS,bU,bT,bV,bR;if(!bX){return[]}for(bS=0,bU=bE.order.length;bS<bU;bS++){bV=bE.order[bS];if((bT=bE.leftMatch[bV].exec(bX))){bR=bT[1];bT.splice(1,1);if(bR.substr(bR.length-1)!=="\\"){bT[1]=(bT[1]||"").replace(bK,"");bW=bE.find[bV](bT,e,bY);if(bW!=null){bX=bX.replace(bE.match[bV],"");break}}}}if(!bW){bW=typeof e.getElementsByTagName!=="undefined"?e.getElementsByTagName("*"):[]}return{set:bW,expr:bX}};by.filter=function(b1,b0,b4,bU){var bW,e,bZ,b6,b3,bR,bT,bV,b2,bS=b1,b5=[],bY=b0,bX=b0&&b0[0]&&by.isXML(b0[0]);while(b1&&b0.length){for(bZ in bE.filter){if((bW=bE.leftMatch[bZ].exec(b1))!=null&&bW[2]){bR=bE.filter[bZ];bT=bW[1];e=false;bW.splice(1,1);if(bT.substr(bT.length-1)==="\\"){continue}if(bY===b5){b5=[]}if(bE.preFilter[bZ]){bW=bE.preFilter[bZ](bW,bY,b4,b5,bU,bX);if(!bW){e=b6=true}else{if(bW===true){continue}}}if(bW){for(bV=0;(b3=bY[bV])!=null;bV++){if(b3){b6=bR(b3,bW,bV,bY);b2=bU^b6;if(b4&&b6!=null){if(b2){e=true}else{bY[bV]=false}}else{if(b2){b5.push(b3);e=true}}}}}if(b6!==L){if(!b4){bY=b5}b1=b1.replace(bE.match[bZ],"");if(!e){return[]}break}}}if(b1===bS){if(e==null){by.error(b1)}else{break}}bS=b1}return bY};by.error=function(e){throw new Error("Syntax error, unrecognized expression: "+e)};var bw=by.getText=function(bU){var bS,bT,e=bU.nodeType,bR="";if(e){if(e===1||e===9){if(typeof bU.textContent==="string"){return bU.textContent}else{if(typeof bU.innerText==="string"){return bU.innerText.replace(bO,"")}else{for(bU=bU.firstChild;bU;bU=bU.nextSibling){bR+=bw(bU)}}}}else{if(e===3||e===4){return bU.nodeValue}}}else{for(bS=0;(bT=bU[bS]);bS++){if(bT.nodeType!==8){bR+=bw(bT)}}}return bR};var bE=by.selectors={order:["ID","NAME","TAG"],match:{ID:/#((?:[\w\u00c0-\uFFFF\-]|\\.)+)/,CLASS:/\.((?:[\w\u00c0-\uFFFF\-]|\\.)+)/,NAME:/\[name=['"]*((?:[\w\u00c0-\uFFFF\-]|\\.)+)['"]*\]/,ATTR:/\[\s*((?:[\w\u00c0-\uFFFF\-]|\\.)+)\s*(?:(\S?=)\s*(?:(['"])(.*?)\3|(#?(?:[\w\u00c0-\uFFFF\-]|\\.)*)|)|)\s*\]/,TAG:/^((?:[\w\u00c0-\uFFFF\*\-]|\\.)+)/,CHILD:/:(only|nth|last|first)-child(?:\(\s*(even|odd|(?:[+\-]?\d+|(?:[+\-]?\d*)?n\s*(?:[+\-]\s*\d+)?))\s*\))?/,POS:/:(nth|eq|gt|lt|first|last|even|odd)(?:\((\d*)\))?(?=[^\-]|$)/,PSEUDO:/:((?:[\w\u00c0-\uFFFF\-]|\\.)+)(?:\((['"]?)((?:\([^\)]+\)|[^\(\)]*)+)\2\))?/},leftMatch:{},attrMap:{"class":"className","for":"htmlFor"},attrHandle:{href:function(e){return e.getAttribute("href")},type:function(e){return e.getAttribute("type")}},relative:{"+":function(bW,bR){var bT=typeof bR==="string",bV=bT&&!bQ.test(bR),bX=bT&&!bV;if(bV){bR=bR.toLowerCase()}for(var bS=0,e=bW.length,bU;bS<e;bS++){if((bU=bW[bS])){while((bU=bU.previousSibling)&&bU.nodeType!==1){}bW[bS]=bX||bU&&bU.nodeName.toLowerCase()===bR?bU||false:bU===bR}}if(bX){by.filter(bR,bW,true)}},">":function(bW,bR){var bV,bU=typeof bR==="string",bS=0,e=bW.length;if(bU&&!bQ.test(bR)){bR=bR.toLowerCase();for(;bS<e;bS++){bV=bW[bS];if(bV){var bT=bV.parentNode;bW[bS]=bT.nodeName.toLowerCase()===bR?bT:false}}}else{for(;bS<e;bS++){bV=bW[bS];if(bV){bW[bS]=bU?bV.parentNode:bV.parentNode===bR}}if(bU){by.filter(bR,bW,true)}}},"":function(bT,bR,bV){var bU,bS=bI++,e=bN;if(typeof bR==="string"&&!bQ.test(bR)){bR=bR.toLowerCase();bU=bR;e=bv}e("parentNode",bR,bS,bT,bU,bV)},"~":function(bT,bR,bV){var bU,bS=bI++,e=bN;if(typeof bR==="string"&&!bQ.test(bR)){bR=bR.toLowerCase();bU=bR;e=bv}e("previousSibling",bR,bS,bT,bU,bV)}},find:{ID:function(bR,bS,bT){if(typeof bS.getElementById!=="undefined"&&!bT){var e=bS.getElementById(bR[1]);return e&&e.parentNode?[e]:[]}},NAME:function(bS,bV){if(typeof bV.getElementsByName!=="undefined"){var bR=[],bU=bV.getElementsByName(bS[1]);for(var bT=0,e=bU.length;bT<e;bT++){if(bU[bT].getAttribute("name")===bS[1]){bR.push(bU[bT])}}return bR.length===0?null:bR}},TAG:function(e,bR){if(typeof bR.getElementsByTagName!=="undefined"){return bR.getElementsByTagName(e[1])}}},preFilter:{CLASS:function(bT,bR,bS,e,bW,bX){bT=" "+bT[1].replace(bK,"")+" ";if(bX){return bT}for(var bU=0,bV;(bV=bR[bU])!=null;bU++){if(bV){if(bW^(bV.className&&(" "+bV.className+" ").replace(/[\t\n\r]/g," ").indexOf(bT)>=0)){if(!bS){e.push(bV)}}else{if(bS){bR[bU]=false}}}}return false},ID:function(e){return e[1].replace(bK,"")},TAG:function(bR,e){return bR[1].replace(bK,"").toLowerCase()},CHILD:function(e){if(e[1]==="nth"){if(!e[2]){by.error(e[0])}e[2]=e[2].replace(/^\+|\s*/g,"");var bR=/(-?)(\d*)(?:n([+\-]?\d*))?/.exec(e[2]==="even"&&"2n"||e[2]==="odd"&&"2n+1"||!/\D/.test(e[2])&&"0n+"+e[2]||e[2]);e[2]=(bR[1]+(bR[2]||1))-0;e[3]=bR[3]-0}else{if(e[2]){by.error(e[0])}}e[0]=bI++;return e},ATTR:function(bU,bR,bS,e,bV,bW){var bT=bU[1]=bU[1].replace(bK,"");if(!bW&&bE.attrMap[bT]){bU[1]=bE.attrMap[bT]}bU[4]=(bU[4]||bU[5]||"").replace(bK,"");if(bU[2]==="~="){bU[4]=" "+bU[4]+" "}return bU},PSEUDO:function(bU,bR,bS,e,bV){if(bU[1]==="not"){if((bH.exec(bU[3])||"").length>1||/^\w/.test(bU[3])){bU[3]=by(bU[3],null,null,bR)}else{var bT=by.filter(bU[3],bR,bS,true^bV);if(!bS){e.push.apply(e,bT)}return false}}else{if(bE.match.POS.test(bU[0])||bE.match.CHILD.test(bU[0])){return true}}return bU},POS:function(e){e.unshift(true);return e}},filters:{enabled:function(e){return e.disabled===false&&e.type!=="hidden"},disabled:function(e){return e.disabled===true},checked:function(e){return e.checked===true},selected:function(e){if(e.parentNode){e.parentNode.selectedIndex}return e.selected===true},parent:function(e){return !!e.firstChild},empty:function(e){return !e.firstChild},has:function(bS,bR,e){return !!by(e[3],bS).length},header:function(e){return(/h\d/i).test(e.nodeName)},text:function(bS){var e=bS.getAttribute("type"),bR=bS.type;return bS.nodeName.toLowerCase()==="input"&&"text"===bR&&(e===bR||e===null)},radio:function(e){return e.nodeName.toLowerCase()==="input"&&"radio"===e.type},checkbox:function(e){return e.nodeName.toLowerCase()==="input"&&"checkbox"===e.type},file:function(e){return e.nodeName.toLowerCase()==="input"&&"file"===e.type},password:function(e){return e.nodeName.toLowerCase()==="input"&&"password"===e.type},submit:function(bR){var e=bR.nodeName.toLowerCase();return(e==="input"||e==="button")&&"submit"===bR.type},image:function(e){return e.nodeName.toLowerCase()==="input"&&"image"===e.type},reset:function(bR){var e=bR.nodeName.toLowerCase();return(e==="input"||e==="button")&&"reset"===bR.type},button:function(bR){var e=bR.nodeName.toLowerCase();return e==="input"&&"button"===bR.type||e==="button"},input:function(e){return(/input|select|textarea|button/i).test(e.nodeName)},focus:function(e){return e===e.ownerDocument.activeElement}},setFilters:{first:function(bR,e){return e===0},last:function(bS,bR,e,bT){return bR===bT.length-1},even:function(bR,e){return e%2===0},odd:function(bR,e){return e%2===1},lt:function(bS,bR,e){return bR<e[3]-0},gt:function(bS,bR,e){return bR>e[3]-0},nth:function(bS,bR,e){return e[3]-0===bR},eq:function(bS,bR,e){return e[3]-0===bR}},filter:{PSEUDO:function(bS,bX,bW,bY){var e=bX[1],bR=bE.filters[e];if(bR){return bR(bS,bW,bX,bY)}else{if(e==="contains"){return(bS.textContent||bS.innerText||bw([bS])||"").indexOf(bX[3])>=0}else{if(e==="not"){var bT=bX[3];for(var bV=0,bU=bT.length;bV<bU;bV++){if(bT[bV]===bS){return false}}return true}else{by.error(e)}}}},CHILD:function(bS,bU){var bT,b0,bW,bZ,e,bV,bY,bX=bU[1],bR=bS;switch(bX){case"only":case"first":while((bR=bR.previousSibling)){if(bR.nodeType===1){return false}}if(bX==="first"){return true}bR=bS;case"last":while((bR=bR.nextSibling)){if(bR.nodeType===1){return false}}return true;case"nth":bT=bU[2];b0=bU[3];if(bT===1&&b0===0){return true}bW=bU[0];bZ=bS.parentNode;if(bZ&&(bZ[bC]!==bW||!bS.nodeIndex)){bV=0;for(bR=bZ.firstChild;bR;bR=bR.nextSibling){if(bR.nodeType===1){bR.nodeIndex=++bV}}bZ[bC]=bW}bY=bS.nodeIndex-b0;if(bT===0){return bY===0}else{return(bY%bT===0&&bY/bT>=0)}}},ID:function(bR,e){return bR.nodeType===1&&bR.getAttribute("id")===e},TAG:function(bR,e){return(e==="*"&&bR.nodeType===1)||!!bR.nodeName&&bR.nodeName.toLowerCase()===e},CLASS:function(bR,e){return(" "+(bR.className||bR.getAttribute("class"))+" ").indexOf(e)>-1},ATTR:function(bV,bT){var bS=bT[1],e=by.attr?by.attr(bV,bS):bE.attrHandle[bS]?bE.attrHandle[bS](bV):bV[bS]!=null?bV[bS]:bV.getAttribute(bS),bW=e+"",bU=bT[2],bR=bT[4];return e==null?bU==="!=":!bU&&by.attr?e!=null:bU==="="?bW===bR:bU==="*="?bW.indexOf(bR)>=0:bU==="~="?(" "+bW+" ").indexOf(bR)>=0:!bR?bW&&e!==false:bU==="!="?bW!==bR:bU==="^="?bW.indexOf(bR)===0:bU==="$="?bW.substr(bW.length-bR.length)===bR:bU==="|="?bW===bR||bW.substr(0,bR.length+1)===bR+"-":false},POS:function(bU,bR,bS,bV){var e=bR[2],bT=bE.setFilters[e];if(bT){return bT(bU,bS,bR,bV)}}}};var bD=bE.match.POS,bx=function(bR,e){return"\\"+(e-0+1)};for(var bz in bE.match){bE.match[bz]=new RegExp(bE.match[bz].source+(/(?![^\[]*\])(?![^\(]*\))/.source));bE.leftMatch[bz]=new RegExp(/(^(?:.|\r|\n)*?)/.source+bE.match[bz].source.replace(/\\(\d+)/g,bx))}var bF=function(bR,e){bR=Array.prototype.slice.call(bR,0);if(e){e.push.apply(e,bR);return e}return bR};try{Array.prototype.slice.call(av.documentElement.childNodes,0)[0].nodeType}catch(bP){bF=function(bU,bT){var bS=0,bR=bT||[];if(bL.call(bU)==="[object Array]"){Array.prototype.push.apply(bR,bU)}else{if(typeof bU.length==="number"){for(var e=bU.length;bS<e;bS++){bR.push(bU[bS])}}else{for(;bU[bS];bS++){bR.push(bU[bS])}}}return bR}}var bJ,bG;if(av.documentElement.compareDocumentPosition){bJ=function(bR,e){if(bR===e){bB=true;return 0}if(!bR.compareDocumentPosition||!e.compareDocumentPosition){return bR.compareDocumentPosition?-1:1}return bR.compareDocumentPosition(e)&4?-1:1}}else{bJ=function(bY,bX){if(bY===bX){bB=true;return 0}else{if(bY.sourceIndex&&bX.sourceIndex){return bY.sourceIndex-bX.sourceIndex}}var bV,bR,bS=[],e=[],bU=bY.parentNode,bW=bX.parentNode,bZ=bU;if(bU===bW){return bG(bY,bX)}else{if(!bU){return -1}else{if(!bW){return 1}}}while(bZ){bS.unshift(bZ);bZ=bZ.parentNode}bZ=bW;while(bZ){e.unshift(bZ);bZ=bZ.parentNode}bV=bS.length;bR=e.length;for(var bT=0;bT<bV&&bT<bR;bT++){if(bS[bT]!==e[bT]){return bG(bS[bT],e[bT])}}return bT===bV?bG(bY,e[bT],-1):bG(bS[bT],bX,1)};bG=function(bR,e,bS){if(bR===e){return bS}var bT=bR.nextSibling;while(bT){if(bT===e){return -1}bT=bT.nextSibling}return 1}}(function(){var bR=av.createElement("div"),bS="script"+(new Date()).getTime(),e=av.documentElement;bR.innerHTML="<a name='"+bS+"'/>";e.insertBefore(bR,e.firstChild);if(av.getElementById(bS)){bE.find.ID=function(bU,bV,bW){if(typeof bV.getElementById!=="undefined"&&!bW){var bT=bV.getElementById(bU[1]);return bT?bT.id===bU[1]||typeof bT.getAttributeNode!=="undefined"&&bT.getAttributeNode("id").nodeValue===bU[1]?[bT]:L:[]}};bE.filter.ID=function(bV,bT){var bU=typeof bV.getAttributeNode!=="undefined"&&bV.getAttributeNode("id");return bV.nodeType===1&&bU&&bU.nodeValue===bT}}e.removeChild(bR);e=bR=null})();(function(){var e=av.createElement("div");e.appendChild(av.createComment(""));if(e.getElementsByTagName("*").length>0){bE.find.TAG=function(bR,bV){var bU=bV.getElementsByTagName(bR[1]);if(bR[1]==="*"){var bT=[];for(var bS=0;bU[bS];bS++){if(bU[bS].nodeType===1){bT.push(bU[bS])}}bU=bT}return bU}}e.innerHTML="<a href='#'></a>";if(e.firstChild&&typeof e.firstChild.getAttribute!=="undefined"&&e.firstChild.getAttribute("href")!=="#"){bE.attrHandle.href=function(bR){return bR.getAttribute("href",2)}}e=null})();if(av.querySelectorAll){(function(){var e=by,bT=av.createElement("div"),bS="__sizzle__";bT.innerHTML="<p class='TEST'></p>";if(bT.querySelectorAll&&bT.querySelectorAll(".TEST").length===0){return}by=function(b4,bV,bZ,b3){bV=bV||av;if(!b3&&!by.isXML(bV)){var b2=/^(\w+$)|^\.([\w\-]+$)|^#([\w\-]+$)/.exec(b4);if(b2&&(bV.nodeType===1||bV.nodeType===9)){if(b2[1]){return bF(bV.getElementsByTagName(b4),bZ)}else{if(b2[2]&&bE.find.CLASS&&bV.getElementsByClassName){return bF(bV.getElementsByClassName(b2[2]),bZ)}}}if(bV.nodeType===9){if(b4==="body"&&bV.body){return bF([bV.body],bZ)}else{if(b2&&b2[3]){var bY=bV.getElementById(b2[3]);if(bY&&bY.parentNode){if(bY.id===b2[3]){return bF([bY],bZ)}}else{return bF([],bZ)}}}try{return bF(bV.querySelectorAll(b4),bZ)}catch(b0){}}else{if(bV.nodeType===1&&bV.nodeName.toLowerCase()!=="object"){var bW=bV,bX=bV.getAttribute("id"),bU=bX||bS,b6=bV.parentNode,b5=/^\s*[+~]/.test(b4);if(!bX){bV.setAttribute("id",bU)}else{bU=bU.replace(/'/g,"\\$&")}if(b5&&b6){bV=bV.parentNode}try{if(!b5||b6){return bF(bV.querySelectorAll("[id='"+bU+"'] "+b4),bZ)}}catch(b1){}finally{if(!bX){bW.removeAttribute("id")}}}}}return e(b4,bV,bZ,b3)};for(var bR in e){by[bR]=e[bR]}bT=null})()}(function(){var e=av.documentElement,bS=e.matchesSelector||e.mozMatchesSelector||e.webkitMatchesSelector||e.msMatchesSelector;if(bS){var bU=!bS.call(av.createElement("div"),"div"),bR=false;try{bS.call(av.documentElement,"[test!='']:sizzle")}catch(bT){bR=true}by.matchesSelector=function(bW,bY){bY=bY.replace(/\=\s*([^'"\]]*)\s*\]/g,"='$1']");if(!by.isXML(bW)){try{if(bR||!bE.match.PSEUDO.test(bY)&&!/!=/.test(bY)){var bV=bS.call(bW,bY);if(bV||!bU||bW.document&&bW.document.nodeType!==11){return bV}}}catch(bX){}}return by(bY,null,null,[bW]).length>0}}})();(function(){var e=av.createElement("div");e.innerHTML="<div class='test e'></div><div class='test'></div>";if(!e.getElementsByClassName||e.getElementsByClassName("e").length===0){return}e.lastChild.className="e";if(e.getElementsByClassName("e").length===1){return}bE.order.splice(1,0,"CLASS");bE.find.CLASS=function(bR,bS,bT){if(typeof bS.getElementsByClassName!=="undefined"&&!bT){return bS.getElementsByClassName(bR[1])}};e=null})();function bv(bR,bW,bV,bZ,bX,bY){for(var bT=0,bS=bZ.length;bT<bS;bT++){var e=bZ[bT];if(e){var bU=false;e=e[bR];while(e){if(e[bC]===bV){bU=bZ[e.sizset];break}if(e.nodeType===1&&!bY){e[bC]=bV;e.sizset=bT}if(e.nodeName.toLowerCase()===bW){bU=e;break}e=e[bR]}bZ[bT]=bU}}}function bN(bR,bW,bV,bZ,bX,bY){for(var bT=0,bS=bZ.length;bT<bS;bT++){var e=bZ[bT];if(e){var bU=false;e=e[bR];while(e){if(e[bC]===bV){bU=bZ[e.sizset];break}if(e.nodeType===1){if(!bY){e[bC]=bV;e.sizset=bT}if(typeof bW!=="string"){if(e===bW){bU=true;break}}else{if(by.filter(bW,[e]).length>0){bU=e;break}}}e=e[bR]}bZ[bT]=bU}}}if(av.documentElement.contains){by.contains=function(bR,e){return bR!==e&&(bR.contains?bR.contains(e):true)}}else{if(av.documentElement.compareDocumentPosition){by.contains=function(bR,e){return !!(bR.compareDocumentPosition(e)&16)}}else{by.contains=function(){return false}}}by.isXML=function(e){var bR=(e?e.ownerDocument||e:0).documentElement;return bR?bR.nodeName!=="HTML":false};var bM=function(bS,e,bW){var bV,bX=[],bU="",bY=e.nodeType?[e]:e;while((bV=bE.match.PSEUDO.exec(bS))){bU+=bV[0];bS=bS.replace(bE.match.PSEUDO,"")}bS=bE.relative[bS]?bS+"*":bS;for(var bT=0,bR=bY.length;bT<bR;bT++){by(bS,bY[bT],bX,bW)}return by.filter(bU,bX)};by.attr=b.attr;by.selectors.attrMap={};b.find=by;b.expr=by.selectors;b.expr[":"]=b.expr.filters;b.unique=by.uniqueSort;b.text=by.getText;b.isXMLDoc=by.isXML;b.contains=by.contains})();var ab=/Until$/,aq=/^(?:parents|prevUntil|prevAll)/,a9=/,/,bp=/^.[^:#\[\.,]*$/,P=Array.prototype.slice,H=b.expr.match.POS,ay={children:true,contents:true,next:true,prev:true};b.fn.extend({find:function(e){var bw=this,by,bv;if(typeof e!=="string"){return b(e).filter(function(){for(by=0,bv=bw.length;by<bv;by++){if(b.contains(bw[by],this)){return true}}})}var bx=this.pushStack("","find",e),bA,bB,bz;for(by=0,bv=this.length;by<bv;by++){bA=bx.length;b.find(e,this[by],bx);if(by>0){for(bB=bA;bB<bx.length;bB++){for(bz=0;bz<bA;bz++){if(bx[bz]===bx[bB]){bx.splice(bB--,1);break}}}}}return bx},has:function(bv){var e=b(bv);return this.filter(function(){for(var bx=0,bw=e.length;bx<bw;bx++){if(b.contains(this,e[bx])){return true}}})},not:function(e){return this.pushStack(aG(this,e,false),"not",e)},filter:function(e){return this.pushStack(aG(this,e,true),"filter",e)},is:function(e){return !!e&&(typeof e==="string"?H.test(e)?b(e,this.context).index(this[0])>=0:b.filter(e,this).length>0:this.filter(e).length>0)},closest:function(by,bx){var bv=[],bw,e,bz=this[0];if(b.isArray(by)){var bB=1;while(bz&&bz.ownerDocument&&bz!==bx){for(bw=0;bw<by.length;bw++){if(b(bz).is(by[bw])){bv.push({selector:by[bw],elem:bz,level:bB})}}bz=bz.parentNode;bB++}return bv}var bA=H.test(by)||typeof by!=="string"?b(by,bx||this.context):0;for(bw=0,e=this.length;bw<e;bw++){bz=this[bw];while(bz){if(bA?bA.index(bz)>-1:b.find.matchesSelector(bz,by)){bv.push(bz);break}else{bz=bz.parentNode;if(!bz||!bz.ownerDocument||bz===bx||bz.nodeType===11){break}}}}bv=bv.length>1?b.unique(bv):bv;return this.pushStack(bv,"closest",by)},index:function(e){if(!e){return(this[0]&&this[0].parentNode)?this.prevAll().length:-1}if(typeof e==="string"){return b.inArray(this[0],b(e))}return b.inArray(e.jquery?e[0]:e,this)},add:function(e,bv){var bx=typeof e==="string"?b(e,bv):b.makeArray(e&&e.nodeType?[e]:e),bw=b.merge(this.get(),bx);return this.pushStack(C(bx[0])||C(bw[0])?bw:b.unique(bw))},andSelf:function(){return this.add(this.prevObject)}});function C(e){return !e||!e.parentNode||e.parentNode.nodeType===11}b.each({parent:function(bv){var e=bv.parentNode;return e&&e.nodeType!==11?e:null},parents:function(e){return b.dir(e,"parentNode")},parentsUntil:function(bv,e,bw){return b.dir(bv,"parentNode",bw)},next:function(e){return b.nth(e,2,"nextSibling")},prev:function(e){return b.nth(e,2,"previousSibling")},nextAll:function(e){return b.dir(e,"nextSibling")},prevAll:function(e){return b.dir(e,"previousSibling")},nextUntil:function(bv,e,bw){return b.dir(bv,"nextSibling",bw)},prevUntil:function(bv,e,bw){return b.dir(bv,"previousSibling",bw)},siblings:function(e){return b.sibling(e.parentNode.firstChild,e)},children:function(e){return b.sibling(e.firstChild)},contents:function(e){return b.nodeName(e,"iframe")?e.contentDocument||e.contentWindow.document:b.makeArray(e.childNodes)}},function(e,bv){b.fn[e]=function(by,bw){var bx=b.map(this,bv,by);if(!ab.test(e)){bw=by}if(bw&&typeof bw==="string"){bx=b.filter(bw,bx)}bx=this.length>1&&!ay[e]?b.unique(bx):bx;if((this.length>1||a9.test(bw))&&aq.test(e)){bx=bx.reverse()}return this.pushStack(bx,e,P.call(arguments).join(","))}});b.extend({filter:function(bw,e,bv){if(bv){bw=":not("+bw+")"}return e.length===1?b.find.matchesSelector(e[0],bw)?[e[0]]:[]:b.find.matches(bw,e)},dir:function(bw,bv,by){var e=[],bx=bw[bv];while(bx&&bx.nodeType!==9&&(by===L||bx.nodeType!==1||!b(bx).is(by))){if(bx.nodeType===1){e.push(bx)}bx=bx[bv]}return e},nth:function(by,e,bw,bx){e=e||1;var bv=0;for(;by;by=by[bw]){if(by.nodeType===1&&++bv===e){break}}return by},sibling:function(bw,bv){var e=[];for(;bw;bw=bw.nextSibling){if(bw.nodeType===1&&bw!==bv){e.push(bw)}}return e}});function aG(bx,bw,e){bw=bw||0;if(b.isFunction(bw)){return b.grep(bx,function(bz,by){var bA=!!bw.call(bz,by,bz);return bA===e})}else{if(bw.nodeType){return b.grep(bx,function(bz,by){return(bz===bw)===e})}else{if(typeof bw==="string"){var bv=b.grep(bx,function(by){return by.nodeType===1});if(bp.test(bw)){return b.filter(bw,bv,!e)}else{bw=b.filter(bw,bv)}}}}return b.grep(bx,function(bz,by){return(b.inArray(bz,bw)>=0)===e})}function a(e){var bw=aR.split("|"),bv=e.createDocumentFragment();if(bv.createElement){while(bw.length){bv.createElement(bw.pop())}}return bv}var aR="abbr|article|aside|audio|canvas|datalist|details|figcaption|figure|footer|header|hgroup|mark|meter|nav|output|progress|section|summary|time|video",ag=/ jQuery\d+="(?:\d+|null)"/g,ar=/^\s+/,R=/<(?!area|br|col|embed|hr|img|input|link|meta|param)(([\w:]+)[^>]*)\/>/ig,d=/<([\w:]+)/,w=/<tbody/i,W=/<|&#?\w+;/,ae=/<(?:script|style)/i,O=/<(?:script|object|embed|option|style)/i,ah=new RegExp("<(?:"+aR+")","i"),o=/checked\s*(?:[^=]|=\s*.checked.)/i,bm=/\/(java|ecma)script/i,aN=/^\s*<!(?:\[CDATA\[|\-\-)/,ax={option:[1,"<select multiple='multiple'>","</select>"],legend:[1,"<fieldset>","</fieldset>"],thead:[1,"<table>","</table>"],tr:[2,"<table><tbody>","</tbody></table>"],td:[3,"<table><tbody><tr>","</tr></tbody></table>"],col:[2,"<table><tbody></tbody><colgroup>","</colgroup></table>"],area:[1,"<map>","</map>"],_default:[0,"",""]},ac=a(av);ax.optgroup=ax.option;ax.tbody=ax.tfoot=ax.colgroup=ax.caption=ax.thead;ax.th=ax.td;if(!b.support.htmlSerialize){ax._default=[1,"div<div>","</div>"]}b.fn.extend({text:function(e){if(b.isFunction(e)){return this.each(function(bw){var bv=b(this);bv.text(e.call(this,bw,bv.text()))})}if(typeof e!=="object"&&e!==L){return this.empty().append((this[0]&&this[0].ownerDocument||av).createTextNode(e))}return b.text(this)},wrapAll:function(e){if(b.isFunction(e)){return this.each(function(bw){b(this).wrapAll(e.call(this,bw))})}if(this[0]){var bv=b(e,this[0].ownerDocument).eq(0).clone(true);if(this[0].parentNode){bv.insertBefore(this[0])}bv.map(function(){var bw=this;while(bw.firstChild&&bw.firstChild.nodeType===1){bw=bw.firstChild}return bw}).append(this)}return this},wrapInner:function(e){if(b.isFunction(e)){return this.each(function(bv){b(this).wrapInner(e.call(this,bv))})}return this.each(function(){var bv=b(this),bw=bv.contents();if(bw.length){bw.wrapAll(e)}else{bv.append(e)}})},wrap:function(e){var bv=b.isFunction(e);return this.each(function(bw){b(this).wrapAll(bv?e.call(this,bw):e)})},unwrap:function(){return this.parent().each(function(){if(!b.nodeName(this,"body")){b(this).replaceWith(this.childNodes)}}).end()},append:function(){return this.domManip(arguments,true,function(e){if(this.nodeType===1){this.appendChild(e)}})},prepend:function(){return this.domManip(arguments,true,function(e){if(this.nodeType===1){this.insertBefore(e,this.firstChild)}})},before:function(){if(this[0]&&this[0].parentNode){return this.domManip(arguments,false,function(bv){this.parentNode.insertBefore(bv,this)})}else{if(arguments.length){var e=b.clean(arguments);e.push.apply(e,this.toArray());return this.pushStack(e,"before",arguments)}}},after:function(){if(this[0]&&this[0].parentNode){return this.domManip(arguments,false,function(bv){this.parentNode.insertBefore(bv,this.nextSibling)})}else{if(arguments.length){var e=this.pushStack(this,"after",arguments);e.push.apply(e,b.clean(arguments));return e}}},remove:function(e,bx){for(var bv=0,bw;(bw=this[bv])!=null;bv++){if(!e||b.filter(e,[bw]).length){if(!bx&&bw.nodeType===1){b.cleanData(bw.getElementsByTagName("*"));b.cleanData([bw])}if(bw.parentNode){bw.parentNode.removeChild(bw)}}}return this},empty:function(){for(var e=0,bv;(bv=this[e])!=null;e++){if(bv.nodeType===1){b.cleanData(bv.getElementsByTagName("*"))}while(bv.firstChild){bv.removeChild(bv.firstChild)}}return this},clone:function(bv,e){bv=bv==null?false:bv;e=e==null?bv:e;return this.map(function(){return b.clone(this,bv,e)})},html:function(bx){if(bx===L){return this[0]&&this[0].nodeType===1?this[0].innerHTML.replace(ag,""):null}else{if(typeof bx==="string"&&!ae.test(bx)&&(b.support.leadingWhitespace||!ar.test(bx))&&!ax[(d.exec(bx)||["",""])[1].toLowerCase()]){bx=bx.replace(R,"<$1></$2>");try{for(var bw=0,bv=this.length;bw<bv;bw++){if(this[bw].nodeType===1){b.cleanData(this[bw].getElementsByTagName("*"));this[bw].innerHTML=bx}}}catch(by){this.empty().append(bx)}}else{if(b.isFunction(bx)){this.each(function(bz){var e=b(this);e.html(bx.call(this,bz,e.html()))})}else{this.empty().append(bx)}}}return this},replaceWith:function(e){if(this[0]&&this[0].parentNode){if(b.isFunction(e)){return this.each(function(bx){var bw=b(this),bv=bw.html();bw.replaceWith(e.call(this,bx,bv))})}if(typeof e!=="string"){e=b(e).detach()}return this.each(function(){var bw=this.nextSibling,bv=this.parentNode;b(this).remove();if(bw){b(bw).before(e)}else{b(bv).append(e)}})}else{return this.length?this.pushStack(b(b.isFunction(e)?e():e),"replaceWith",e):this}},detach:function(e){return this.remove(e,true)},domManip:function(bB,bF,bE){var bx,by,bA,bD,bC=bB[0],bv=[];if(!b.support.checkClone&&arguments.length===3&&typeof bC==="string"&&o.test(bC)){return this.each(function(){b(this).domManip(bB,bF,bE,true)})}if(b.isFunction(bC)){return this.each(function(bH){var bG=b(this);bB[0]=bC.call(this,bH,bF?bG.html():L);bG.domManip(bB,bF,bE)})}if(this[0]){bD=bC&&bC.parentNode;if(b.support.parentNode&&bD&&bD.nodeType===11&&bD.childNodes.length===this.length){bx={fragment:bD}}else{bx=b.buildFragment(bB,this,bv)}bA=bx.fragment;if(bA.childNodes.length===1){by=bA=bA.firstChild}else{by=bA.firstChild}if(by){bF=bF&&b.nodeName(by,"tr");for(var bw=0,e=this.length,bz=e-1;bw<e;bw++){bE.call(bF?ba(this[bw],by):this[bw],bx.cacheable||(e>1&&bw<bz)?b.clone(bA,true,true):bA)}}if(bv.length){b.each(bv,bo)}}return this}});function ba(e,bv){return b.nodeName(e,"table")?(e.getElementsByTagName("tbody")[0]||e.appendChild(e.ownerDocument.createElement("tbody"))):e}function t(bB,bv){if(bv.nodeType!==1||!b.hasData(bB)){return}var by,bx,e,bA=b._data(bB),bz=b._data(bv,bA),bw=bA.events;if(bw){delete bz.handle;bz.events={};for(by in bw){for(bx=0,e=bw[by].length;bx<e;bx++){b.event.add(bv,by+(bw[by][bx].namespace?".":"")+bw[by][bx].namespace,bw[by][bx],bw[by][bx].data)}}}if(bz.data){bz.data=b.extend({},bz.data)}}function ai(bv,e){var bw;if(e.nodeType!==1){return}if(e.clearAttributes){e.clearAttributes()}if(e.mergeAttributes){e.mergeAttributes(bv)}bw=e.nodeName.toLowerCase();if(bw==="object"){e.outerHTML=bv.outerHTML}else{if(bw==="input"&&(bv.type==="checkbox"||bv.type==="radio")){if(bv.checked){e.defaultChecked=e.checked=bv.checked}if(e.value!==bv.value){e.value=bv.value}}else{if(bw==="option"){e.selected=bv.defaultSelected}else{if(bw==="input"||bw==="textarea"){e.defaultValue=bv.defaultValue}}}}e.removeAttribute(b.expando)}b.buildFragment=function(bz,bx,bv){var by,e,bw,bA,bB=bz[0];if(bx&&bx[0]){bA=bx[0].ownerDocument||bx[0]}if(!bA.createDocumentFragment){bA=av}if(bz.length===1&&typeof bB==="string"&&bB.length<512&&bA===av&&bB.charAt(0)==="<"&&!O.test(bB)&&(b.support.checkClone||!o.test(bB))&&(b.support.html5Clone||!ah.test(bB))){e=true;bw=b.fragments[bB];if(bw&&bw!==1){by=bw}}if(!by){by=bA.createDocumentFragment();b.clean(bz,bA,by,bv)}if(e){b.fragments[bB]=bw?by:1}return{fragment:by,cacheable:e}};b.fragments={};b.each({appendTo:"append",prependTo:"prepend",insertBefore:"before",insertAfter:"after",replaceAll:"replaceWith"},function(e,bv){b.fn[e]=function(bw){var bz=[],bC=b(bw),bB=this.length===1&&this[0].parentNode;if(bB&&bB.nodeType===11&&bB.childNodes.length===1&&bC.length===1){bC[bv](this[0]);return this}else{for(var bA=0,bx=bC.length;bA<bx;bA++){var by=(bA>0?this.clone(true):this).get();b(bC[bA])[bv](by);bz=bz.concat(by)}return this.pushStack(bz,e,bC.selector)}}});function bg(e){if(typeof e.getElementsByTagName!=="undefined"){return e.getElementsByTagName("*")}else{if(typeof e.querySelectorAll!=="undefined"){return e.querySelectorAll("*")}else{return[]}}}function az(e){if(e.type==="checkbox"||e.type==="radio"){e.defaultChecked=e.checked}}function E(e){var bv=(e.nodeName||"").toLowerCase();if(bv==="input"){az(e)}else{if(bv!=="script"&&typeof e.getElementsByTagName!=="undefined"){b.grep(e.getElementsByTagName("input"),az)}}}function al(e){var bv=av.createElement("div");ac.appendChild(bv);bv.innerHTML=e.outerHTML;return bv.firstChild}b.extend({clone:function(by,bA,bw){var e,bv,bx,bz=b.support.html5Clone||!ah.test("<"+by.nodeName)?by.cloneNode(true):al(by);if((!b.support.noCloneEvent||!b.support.noCloneChecked)&&(by.nodeType===1||by.nodeType===11)&&!b.isXMLDoc(by)){ai(by,bz);e=bg(by);bv=bg(bz);for(bx=0;e[bx];++bx){if(bv[bx]){ai(e[bx],bv[bx])}}}if(bA){t(by,bz);if(bw){e=bg(by);bv=bg(bz);for(bx=0;e[bx];++bx){t(e[bx],bv[bx])}}}e=bv=null;return bz},clean:function(bw,by,bH,bA){var bF;by=by||av;if(typeof by.createElement==="undefined"){by=by.ownerDocument||by[0]&&by[0].ownerDocument||av}var bI=[],bB;for(var bE=0,bz;(bz=bw[bE])!=null;bE++){if(typeof bz==="number"){bz+=""}if(!bz){continue}if(typeof bz==="string"){if(!W.test(bz)){bz=by.createTextNode(bz)}else{bz=bz.replace(R,"<$1></$2>");var bK=(d.exec(bz)||["",""])[1].toLowerCase(),bx=ax[bK]||ax._default,bD=bx[0],bv=by.createElement("div");if(by===av){ac.appendChild(bv)}else{a(by).appendChild(bv)}bv.innerHTML=bx[1]+bz+bx[2];while(bD--){bv=bv.lastChild}if(!b.support.tbody){var e=w.test(bz),bC=bK==="table"&&!e?bv.firstChild&&bv.firstChild.childNodes:bx[1]==="<table>"&&!e?bv.childNodes:[];for(bB=bC.length-1;bB>=0;--bB){if(b.nodeName(bC[bB],"tbody")&&!bC[bB].childNodes.length){bC[bB].parentNode.removeChild(bC[bB])}}}if(!b.support.leadingWhitespace&&ar.test(bz)){bv.insertBefore(by.createTextNode(ar.exec(bz)[0]),bv.firstChild)}bz=bv.childNodes}}var bG;if(!b.support.appendChecked){if(bz[0]&&typeof(bG=bz.length)==="number"){for(bB=0;bB<bG;bB++){E(bz[bB])}}else{E(bz)}}if(bz.nodeType){bI.push(bz)}else{bI=b.merge(bI,bz)}}if(bH){bF=function(bL){return !bL.type||bm.test(bL.type)};for(bE=0;bI[bE];bE++){if(bA&&b.nodeName(bI[bE],"script")&&(!bI[bE].type||bI[bE].type.toLowerCase()==="text/javascript")){bA.push(bI[bE].parentNode?bI[bE].parentNode.removeChild(bI[bE]):bI[bE])}else{if(bI[bE].nodeType===1){var bJ=b.grep(bI[bE].getElementsByTagName("script"),bF);bI.splice.apply(bI,[bE+1,0].concat(bJ))}bH.appendChild(bI[bE])}}}return bI},cleanData:function(bv){var by,bw,e=b.cache,bB=b.event.special,bA=b.support.deleteExpando;for(var bz=0,bx;(bx=bv[bz])!=null;bz++){if(bx.nodeName&&b.noData[bx.nodeName.toLowerCase()]){continue}bw=bx[b.expando];if(bw){by=e[bw];if(by&&by.events){for(var bC in by.events){if(bB[bC]){b.event.remove(bx,bC)}else{b.removeEvent(bx,bC,by.handle)}}if(by.handle){by.handle.elem=null}}if(bA){delete bx[b.expando]}else{if(bx.removeAttribute){bx.removeAttribute(b.expando)}}delete e[bw]}}}});function bo(e,bv){if(bv.src){b.ajax({url:bv.src,async:false,dataType:"script"})}else{b.globalEval((bv.text||bv.textContent||bv.innerHTML||"").replace(aN,"/*$0*/"))}if(bv.parentNode){bv.parentNode.removeChild(bv)}}var ak=/alpha\([^)]*\)/i,au=/opacity=([^)]*)/,z=/([A-Z]|^ms)/g,bc=/^-?\d+(?:px)?$/i,bn=/^-?\d/,I=/^([\-+])=([\-+.\de]+)/,a7={position:"absolute",visibility:"hidden",display:"block"},an=["Left","Right"],a1=["Top","Bottom"],Z,aI,aX;b.fn.css=function(e,bv){if(arguments.length===2&&bv===L){return this}return b.access(this,e,bv,true,function(bx,bw,by){return by!==L?b.style(bx,bw,by):b.css(bx,bw)})};b.extend({cssHooks:{opacity:{get:function(bw,bv){if(bv){var e=Z(bw,"opacity","opacity");return e===""?"1":e}else{return bw.style.opacity}}}},cssNumber:{fillOpacity:true,fontWeight:true,lineHeight:true,opacity:true,orphans:true,widows:true,zIndex:true,zoom:true},cssProps:{"float":b.support.cssFloat?"cssFloat":"styleFloat"},style:function(bx,bw,bD,by){if(!bx||bx.nodeType===3||bx.nodeType===8||!bx.style){return}var bB,bC,bz=b.camelCase(bw),bv=bx.style,bE=b.cssHooks[bz];bw=b.cssProps[bz]||bz;if(bD!==L){bC=typeof bD;if(bC==="string"&&(bB=I.exec(bD))){bD=(+(bB[1]+1)*+bB[2])+parseFloat(b.css(bx,bw));bC="number"}if(bD==null||bC==="number"&&isNaN(bD)){return}if(bC==="number"&&!b.cssNumber[bz]){bD+="px"}if(!bE||!("set" in bE)||(bD=bE.set(bx,bD))!==L){try{bv[bw]=bD}catch(bA){}}}else{if(bE&&"get" in bE&&(bB=bE.get(bx,false,by))!==L){return bB}return bv[bw]}},css:function(by,bx,bv){var bw,e;bx=b.camelCase(bx);e=b.cssHooks[bx];bx=b.cssProps[bx]||bx;if(bx==="cssFloat"){bx="float"}if(e&&"get" in e&&(bw=e.get(by,true,bv))!==L){return bw}else{if(Z){return Z(by,bx)}}},swap:function(bx,bw,by){var e={};for(var bv in bw){e[bv]=bx.style[bv];bx.style[bv]=bw[bv]}by.call(bx);for(bv in bw){bx.style[bv]=e[bv]}}});b.curCSS=b.css;b.each(["height","width"],function(bv,e){b.cssHooks[e]={get:function(by,bx,bw){var bz;if(bx){if(by.offsetWidth!==0){return p(by,e,bw)}else{b.swap(by,a7,function(){bz=p(by,e,bw)})}return bz}},set:function(bw,bx){if(bc.test(bx)){bx=parseFloat(bx);if(bx>=0){return bx+"px"}}else{return bx}}}});if(!b.support.opacity){b.cssHooks.opacity={get:function(bv,e){return au.test((e&&bv.currentStyle?bv.currentStyle.filter:bv.style.filter)||"")?(parseFloat(RegExp.$1)/100)+"":e?"1":""},set:function(by,bz){var bx=by.style,bv=by.currentStyle,e=b.isNumeric(bz)?"alpha(opacity="+bz*100+")":"",bw=bv&&bv.filter||bx.filter||"";bx.zoom=1;if(bz>=1&&b.trim(bw.replace(ak,""))===""){bx.removeAttribute("filter");if(bv&&!bv.filter){return}}bx.filter=ak.test(bw)?bw.replace(ak,e):bw+" "+e}}}b(function(){if(!b.support.reliableMarginRight){b.cssHooks.marginRight={get:function(bw,bv){var e;b.swap(bw,{display:"inline-block"},function(){if(bv){e=Z(bw,"margin-right","marginRight")}else{e=bw.style.marginRight}});return e}}}});if(av.defaultView&&av.defaultView.getComputedStyle){aI=function(by,bw){var bv,bx,e;bw=bw.replace(z,"-$1").toLowerCase();if((bx=by.ownerDocument.defaultView)&&(e=bx.getComputedStyle(by,null))){bv=e.getPropertyValue(bw);if(bv===""&&!b.contains(by.ownerDocument.documentElement,by)){bv=b.style(by,bw)}}return bv}}if(av.documentElement.currentStyle){aX=function(bz,bw){var bA,e,by,bv=bz.currentStyle&&bz.currentStyle[bw],bx=bz.style;if(bv===null&&bx&&(by=bx[bw])){bv=by}if(!bc.test(bv)&&bn.test(bv)){bA=bx.left;e=bz.runtimeStyle&&bz.runtimeStyle.left;if(e){bz.runtimeStyle.left=bz.currentStyle.left}bx.left=bw==="fontSize"?"1em":(bv||0);bv=bx.pixelLeft+"px";bx.left=bA;if(e){bz.runtimeStyle.left=e}}return bv===""?"auto":bv}}Z=aI||aX;function p(by,bw,bv){var bA=bw==="width"?by.offsetWidth:by.offsetHeight,bz=bw==="width"?an:a1,bx=0,e=bz.length;if(bA>0){if(bv!=="border"){for(;bx<e;bx++){if(!bv){bA-=parseFloat(b.css(by,"padding"+bz[bx]))||0}if(bv==="margin"){bA+=parseFloat(b.css(by,bv+bz[bx]))||0}else{bA-=parseFloat(b.css(by,"border"+bz[bx]+"Width"))||0}}}return bA+"px"}bA=Z(by,bw,bw);if(bA<0||bA==null){bA=by.style[bw]||0}bA=parseFloat(bA)||0;if(bv){for(;bx<e;bx++){bA+=parseFloat(b.css(by,"padding"+bz[bx]))||0;if(bv!=="padding"){bA+=parseFloat(b.css(by,"border"+bz[bx]+"Width"))||0}if(bv==="margin"){bA+=parseFloat(b.css(by,bv+bz[bx]))||0}}}return bA+"px"}if(b.expr&&b.expr.filters){b.expr.filters.hidden=function(bw){var bv=bw.offsetWidth,e=bw.offsetHeight;return(bv===0&&e===0)||(!b.support.reliableHiddenOffsets&&((bw.style&&bw.style.display)||b.css(bw,"display"))==="none")};b.expr.filters.visible=function(e){return !b.expr.filters.hidden(e)}}var k=/%20/g,ap=/\[\]$/,bs=/\r?\n/g,bq=/#.*$/,aD=/^(.*?):[ \t]*([^\r\n]*)\r?$/mg,aZ=/^(?:color|date|datetime|datetime-local|email|hidden|month|number|password|range|search|tel|text|time|url|week)$/i,aM=/^(?:about|app|app\-storage|.+\-extension|file|res|widget):$/,aQ=/^(?:GET|HEAD)$/,c=/^\/\//,M=/\?/,a6=/<script\b[^<]*(?:(?!<\/script>)<[^<]*)*<\/script>/gi,q=/^(?:select|textarea)/i,h=/\s+/,br=/([?&])_=[^&]*/,K=/^([\w\+\.\-]+:)(?:\/\/([^\/?#:]*)(?::(\d+))?)?/,A=b.fn.load,aa={},r={},aE,s,aV=["*/"]+["*"];try{aE=bl.href}catch(aw){aE=av.createElement("a");aE.href="";aE=aE.href}s=K.exec(aE.toLowerCase())||[];function f(e){return function(by,bA){if(typeof by!=="string"){bA=by;by="*"}if(b.isFunction(bA)){var bx=by.toLowerCase().split(h),bw=0,bz=bx.length,bv,bB,bC;for(;bw<bz;bw++){bv=bx[bw];bC=/^\+/.test(bv);if(bC){bv=bv.substr(1)||"*"}bB=e[bv]=e[bv]||[];bB[bC?"unshift":"push"](bA)}}}}function aW(bv,bE,bz,bD,bB,bx){bB=bB||bE.dataTypes[0];bx=bx||{};bx[bB]=true;var bA=bv[bB],bw=0,e=bA?bA.length:0,by=(bv===aa),bC;for(;bw<e&&(by||!bC);bw++){bC=bA[bw](bE,bz,bD);if(typeof bC==="string"){if(!by||bx[bC]){bC=L}else{bE.dataTypes.unshift(bC);bC=aW(bv,bE,bz,bD,bC,bx)}}}if((by||!bC)&&!bx["*"]){bC=aW(bv,bE,bz,bD,"*",bx)}return bC}function am(bw,bx){var bv,e,by=b.ajaxSettings.flatOptions||{};for(bv in bx){if(bx[bv]!==L){(by[bv]?bw:(e||(e={})))[bv]=bx[bv]}}if(e){b.extend(true,bw,e)}}b.fn.extend({load:function(bw,bz,bA){if(typeof bw!=="string"&&A){return A.apply(this,arguments)}else{if(!this.length){return this}}var by=bw.indexOf(" ");if(by>=0){var e=bw.slice(by,bw.length);bw=bw.slice(0,by)}var bx="GET";if(bz){if(b.isFunction(bz)){bA=bz;bz=L}else{if(typeof bz==="object"){bz=b.param(bz,b.ajaxSettings.traditional);bx="POST"}}}var bv=this;b.ajax({url:bw,type:bx,dataType:"html",data:bz,complete:function(bC,bB,bD){bD=bC.responseText;if(bC.isResolved()){bC.done(function(bE){bD=bE});bv.html(e?b("<div>").append(bD.replace(a6,"")).find(e):bD)}if(bA){bv.each(bA,[bD,bB,bC])}}});return this},serialize:function(){return b.param(this.serializeArray())},serializeArray:function(){return this.map(function(){return this.elements?b.makeArray(this.elements):this}).filter(function(){return this.name&&!this.disabled&&(this.checked||q.test(this.nodeName)||aZ.test(this.type))}).map(function(e,bv){var bw=b(this).val();return bw==null?null:b.isArray(bw)?b.map(bw,function(by,bx){return{name:bv.name,value:by.replace(bs,"\r\n")}}):{name:bv.name,value:bw.replace(bs,"\r\n")}}).get()}});b.each("ajaxStart ajaxStop ajaxComplete ajaxError ajaxSuccess ajaxSend".split(" "),function(e,bv){b.fn[bv]=function(bw){return this.on(bv,bw)}});b.each(["get","post"],function(e,bv){b[bv]=function(bw,by,bz,bx){if(b.isFunction(by)){bx=bx||bz;bz=by;by=L}return b.ajax({type:bv,url:bw,data:by,success:bz,dataType:bx})}});b.extend({getScript:function(e,bv){return b.get(e,L,bv,"script")},getJSON:function(e,bv,bw){return b.get(e,bv,bw,"json")},ajaxSetup:function(bv,e){if(e){am(bv,b.ajaxSettings)}else{e=bv;bv=b.ajaxSettings}am(bv,e);return bv},ajaxSettings:{url:aE,isLocal:aM.test(s[1]),global:true,type:"GET",contentType:"application/x-www-form-urlencoded",processData:true,async:true,accepts:{xml:"application/xml, text/xml",html:"text/html",text:"text/plain",json:"application/json, text/javascript","*":aV},contents:{xml:/xml/,html:/html/,json:/json/},responseFields:{xml:"responseXML",text:"responseText"},converters:{"* text":bb.String,"text html":true,"text json":b.parseJSON,"text xml":b.parseXML},flatOptions:{context:true,url:true}},ajaxPrefilter:f(aa),ajaxTransport:f(r),ajax:function(bz,bx){if(typeof bz==="object"){bx=bz;bz=L}bx=bx||{};var bD=b.ajaxSetup({},bx),bS=bD.context||bD,bG=bS!==bD&&(bS.nodeType||bS instanceof b)?b(bS):b.event,bR=b.Deferred(),bN=b.Callbacks("once memory"),bB=bD.statusCode||{},bC,bH={},bO={},bQ,by,bL,bE,bI,bA=0,bw,bK,bJ={readyState:0,setRequestHeader:function(bT,bU){if(!bA){var e=bT.toLowerCase();bT=bO[e]=bO[e]||bT;bH[bT]=bU}return this},getAllResponseHeaders:function(){return bA===2?bQ:null},getResponseHeader:function(bT){var e;if(bA===2){if(!by){by={};while((e=aD.exec(bQ))){by[e[1].toLowerCase()]=e[2]}}e=by[bT.toLowerCase()]}return e===L?null:e},overrideMimeType:function(e){if(!bA){bD.mimeType=e}return this},abort:function(e){e=e||"abort";if(bL){bL.abort(e)}bF(0,e);return this}};function bF(bZ,bU,b0,bW){if(bA===2){return}bA=2;if(bE){clearTimeout(bE)}bL=L;bQ=bW||"";bJ.readyState=bZ>0?4:0;var bT,b4,b3,bX=bU,bY=b0?bj(bD,bJ,b0):L,bV,b2;if(bZ>=200&&bZ<300||bZ===304){if(bD.ifModified){if((bV=bJ.getResponseHeader("Last-Modified"))){b.lastModified[bC]=bV}if((b2=bJ.getResponseHeader("Etag"))){b.etag[bC]=b2}}if(bZ===304){bX="notmodified";bT=true}else{try{b4=G(bD,bY);bX="success";bT=true}catch(b1){bX="parsererror";b3=b1}}}else{b3=bX;if(!bX||bZ){bX="error";if(bZ<0){bZ=0}}}bJ.status=bZ;bJ.statusText=""+(bU||bX);if(bT){bR.resolveWith(bS,[b4,bX,bJ])}else{bR.rejectWith(bS,[bJ,bX,b3])}bJ.statusCode(bB);bB=L;if(bw){bG.trigger("ajax"+(bT?"Success":"Error"),[bJ,bD,bT?b4:b3])}bN.fireWith(bS,[bJ,bX]);if(bw){bG.trigger("ajaxComplete",[bJ,bD]);if(!(--b.active)){b.event.trigger("ajaxStop")}}}bR.promise(bJ);bJ.success=bJ.done;bJ.error=bJ.fail;bJ.complete=bN.add;bJ.statusCode=function(bT){if(bT){var e;if(bA<2){for(e in bT){bB[e]=[bB[e],bT[e]]}}else{e=bT[bJ.status];bJ.then(e,e)}}return this};bD.url=((bz||bD.url)+"").replace(bq,"").replace(c,s[1]+"//");bD.dataTypes=b.trim(bD.dataType||"*").toLowerCase().split(h);if(bD.crossDomain==null){bI=K.exec(bD.url.toLowerCase());bD.crossDomain=!!(bI&&(bI[1]!=s[1]||bI[2]!=s[2]||(bI[3]||(bI[1]==="http:"?80:443))!=(s[3]||(s[1]==="http:"?80:443))))}if(bD.data&&bD.processData&&typeof bD.data!=="string"){bD.data=b.param(bD.data,bD.traditional)}aW(aa,bD,bx,bJ);if(bA===2){return false}bw=bD.global;bD.type=bD.type.toUpperCase();bD.hasContent=!aQ.test(bD.type);if(bw&&b.active++===0){b.event.trigger("ajaxStart")}if(!bD.hasContent){if(bD.data){bD.url+=(M.test(bD.url)?"&":"?")+bD.data;delete bD.data}bC=bD.url;if(bD.cache===false){var bv=b.now(),bP=bD.url.replace(br,"$1_="+bv);bD.url=bP+((bP===bD.url)?(M.test(bD.url)?"&":"?")+"_="+bv:"")}}if(bD.data&&bD.hasContent&&bD.contentType!==false||bx.contentType){bJ.setRequestHeader("Content-Type",bD.contentType)}if(bD.ifModified){bC=bC||bD.url;if(b.lastModified[bC]){bJ.setRequestHeader("If-Modified-Since",b.lastModified[bC])}if(b.etag[bC]){bJ.setRequestHeader("If-None-Match",b.etag[bC])}}bJ.setRequestHeader("Accept",bD.dataTypes[0]&&bD.accepts[bD.dataTypes[0]]?bD.accepts[bD.dataTypes[0]]+(bD.dataTypes[0]!=="*"?", "+aV+"; q=0.01":""):bD.accepts["*"]);for(bK in bD.headers){bJ.setRequestHeader(bK,bD.headers[bK])}if(bD.beforeSend&&(bD.beforeSend.call(bS,bJ,bD)===false||bA===2)){bJ.abort();return false}for(bK in {success:1,error:1,complete:1}){bJ[bK](bD[bK])}bL=aW(r,bD,bx,bJ);if(!bL){bF(-1,"No Transport")}else{bJ.readyState=1;if(bw){bG.trigger("ajaxSend",[bJ,bD])}if(bD.async&&bD.timeout>0){bE=setTimeout(function(){bJ.abort("timeout")},bD.timeout)}try{bA=1;bL.send(bH,bF)}catch(bM){if(bA<2){bF(-1,bM)}else{throw bM}}}return bJ},param:function(e,bw){var bv=[],by=function(bz,bA){bA=b.isFunction(bA)?bA():bA;bv[bv.length]=encodeURIComponent(bz)+"="+encodeURIComponent(bA)};if(bw===L){bw=b.ajaxSettings.traditional}if(b.isArray(e)||(e.jquery&&!b.isPlainObject(e))){b.each(e,function(){by(this.name,this.value)})}else{for(var bx in e){v(bx,e[bx],bw,by)}}return bv.join("&").replace(k,"+")}});function v(bw,by,bv,bx){if(b.isArray(by)){b.each(by,function(bA,bz){if(bv||ap.test(bw)){bx(bw,bz)}else{v(bw+"["+(typeof bz==="object"||b.isArray(bz)?bA:"")+"]",bz,bv,bx)}})}else{if(!bv&&by!=null&&typeof by==="object"){for(var e in by){v(bw+"["+e+"]",by[e],bv,bx)}}else{bx(bw,by)}}}b.extend({active:0,lastModified:{},etag:{}});function bj(bD,bC,bz){var bv=bD.contents,bB=bD.dataTypes,bw=bD.responseFields,by,bA,bx,e;for(bA in bw){if(bA in bz){bC[bw[bA]]=bz[bA]}}while(bB[0]==="*"){bB.shift();if(by===L){by=bD.mimeType||bC.getResponseHeader("content-type")}}if(by){for(bA in bv){if(bv[bA]&&bv[bA].test(by)){bB.unshift(bA);break}}}if(bB[0] in bz){bx=bB[0]}else{for(bA in bz){if(!bB[0]||bD.converters[bA+" "+bB[0]]){bx=bA;break}if(!e){e=bA}}bx=bx||e}if(bx){if(bx!==bB[0]){bB.unshift(bx)}return bz[bx]}}function G(bH,bz){if(bH.dataFilter){bz=bH.dataFilter(bz,bH.dataType)}var bD=bH.dataTypes,bG={},bA,bE,bw=bD.length,bB,bC=bD[0],bx,by,bF,bv,e;for(bA=1;bA<bw;bA++){if(bA===1){for(bE in bH.converters){if(typeof bE==="string"){bG[bE.toLowerCase()]=bH.converters[bE]}}}bx=bC;bC=bD[bA];if(bC==="*"){bC=bx}else{if(bx!=="*"&&bx!==bC){by=bx+" "+bC;bF=bG[by]||bG["* "+bC];if(!bF){e=L;for(bv in bG){bB=bv.split(" ");if(bB[0]===bx||bB[0]==="*"){e=bG[bB[1]+" "+bC];if(e){bv=bG[bv];if(bv===true){bF=e}else{if(e===true){bF=bv}}break}}}}if(!(bF||e)){b.error("No conversion from "+by.replace(" "," to "))}if(bF!==true){bz=bF?bF(bz):e(bv(bz))}}}}return bz}var aC=b.now(),u=/(\=)\?(&|$)|\?\?/i;b.ajaxSetup({jsonp:"callback",jsonpCallback:function(){return b.expando+"_"+(aC++)}});b.ajaxPrefilter("json jsonp",function(bD,bA,bC){var bx=bD.contentType==="application/x-www-form-urlencoded"&&(typeof bD.data==="string");if(bD.dataTypes[0]==="jsonp"||bD.jsonp!==false&&(u.test(bD.url)||bx&&u.test(bD.data))){var bB,bw=bD.jsonpCallback=b.isFunction(bD.jsonpCallback)?bD.jsonpCallback():bD.jsonpCallback,bz=bb[bw],e=bD.url,by=bD.data,bv="$1"+bw+"$2";if(bD.jsonp!==false){e=e.replace(u,bv);if(bD.url===e){if(bx){by=by.replace(u,bv)}if(bD.data===by){e+=(/\?/.test(e)?"&":"?")+bD.jsonp+"="+bw}}}bD.url=e;bD.data=by;bb[bw]=function(bE){bB=[bE]};bC.always(function(){bb[bw]=bz;if(bB&&b.isFunction(bz)){bb[bw](bB[0])}});bD.converters["script json"]=function(){if(!bB){b.error(bw+" was not called")}return bB[0]};bD.dataTypes[0]="json";return"script"}});b.ajaxSetup({accepts:{script:"text/javascript, application/javascript, application/ecmascript, application/x-ecmascript"},contents:{script:/javascript|ecmascript/},converters:{"text script":function(e){b.globalEval(e);return e}}});b.ajaxPrefilter("script",function(e){if(e.cache===L){e.cache=false}if(e.crossDomain){e.type="GET";e.global=false}});b.ajaxTransport("script",function(bw){if(bw.crossDomain){var e,bv=av.head||av.getElementsByTagName("head")[0]||av.documentElement;return{send:function(bx,by){e=av.createElement("script");e.async="async";if(bw.scriptCharset){e.charset=bw.scriptCharset}e.src=bw.url;e.onload=e.onreadystatechange=function(bA,bz){if(bz||!e.readyState||/loaded|complete/.test(e.readyState)){e.onload=e.onreadystatechange=null;if(bv&&e.parentNode){bv.removeChild(e)}e=L;if(!bz){by(200,"success")}}};bv.insertBefore(e,bv.firstChild)},abort:function(){if(e){e.onload(0,1)}}}}});var B=bb.ActiveXObject?function(){for(var e in N){N[e](0,1)}}:false,y=0,N;function aL(){try{return new bb.XMLHttpRequest()}catch(bv){}}function aj(){try{return new bb.ActiveXObject("Microsoft.XMLHTTP")}catch(bv){}}b.ajaxSettings.xhr=bb.ActiveXObject?function(){return !this.isLocal&&aL()||aj()}:aL;(function(e){b.extend(b.support,{ajax:!!e,cors:!!e&&("withCredentials" in e)})})(b.ajaxSettings.xhr());if(b.support.ajax){b.ajaxTransport(function(e){if(!e.crossDomain||b.support.cors){var bv;return{send:function(bB,bw){var bA=e.xhr(),bz,by;if(e.username){bA.open(e.type,e.url,e.async,e.username,e.password)}else{bA.open(e.type,e.url,e.async)}if(e.xhrFields){for(by in e.xhrFields){bA[by]=e.xhrFields[by]}}if(e.mimeType&&bA.overrideMimeType){bA.overrideMimeType(e.mimeType)}if(!e.crossDomain&&!bB["X-Requested-With"]){bB["X-Requested-With"]="XMLHttpRequest"}try{for(by in bB){bA.setRequestHeader(by,bB[by])}}catch(bx){}bA.send((e.hasContent&&e.data)||null);bv=function(bK,bE){var bF,bD,bC,bI,bH;try{if(bv&&(bE||bA.readyState===4)){bv=L;if(bz){bA.onreadystatechange=b.noop;if(B){delete N[bz]}}if(bE){if(bA.readyState!==4){bA.abort()}}else{bF=bA.status;bC=bA.getAllResponseHeaders();bI={};bH=bA.responseXML;if(bH&&bH.documentElement){bI.xml=bH}bI.text=bA.responseText;try{bD=bA.statusText}catch(bJ){bD=""}if(!bF&&e.isLocal&&!e.crossDomain){bF=bI.text?200:404}else{if(bF===1223){bF=204}}}}}catch(bG){if(!bE){bw(-1,bG)}}if(bI){bw(bF,bD,bI,bC)}};if(!e.async||bA.readyState===4){bv()}else{bz=++y;if(B){if(!N){N={};b(bb).unload(B)}N[bz]=bv}bA.onreadystatechange=bv}},abort:function(){if(bv){bv(0,1)}}}}})}var Q={},a8,m,aB=/^(?:toggle|show|hide)$/,aT=/^([+\-]=)?([\d+.\-]+)([a-z%]*)$/i,a3,aH=[["height","marginTop","marginBottom","paddingTop","paddingBottom"],["width","marginLeft","marginRight","paddingLeft","paddingRight"],["opacity"]],a4;b.fn.extend({show:function(bx,bA,bz){var bw,by;if(bx||bx===0){return this.animate(a0("show",3),bx,bA,bz)}else{for(var bv=0,e=this.length;bv<e;bv++){bw=this[bv];if(bw.style){by=bw.style.display;if(!b._data(bw,"olddisplay")&&by==="none"){by=bw.style.display=""}if(by===""&&b.css(bw,"display")==="none"){b._data(bw,"olddisplay",x(bw.nodeName))}}}for(bv=0;bv<e;bv++){bw=this[bv];if(bw.style){by=bw.style.display;if(by===""||by==="none"){bw.style.display=b._data(bw,"olddisplay")||""}}}return this}},hide:function(bx,bA,bz){if(bx||bx===0){return this.animate(a0("hide",3),bx,bA,bz)}else{var bw,by,bv=0,e=this.length;for(;bv<e;bv++){bw=this[bv];if(bw.style){by=b.css(bw,"display");if(by!=="none"&&!b._data(bw,"olddisplay")){b._data(bw,"olddisplay",by)}}}for(bv=0;bv<e;bv++){if(this[bv].style){this[bv].style.display="none"}}return this}},_toggle:b.fn.toggle,toggle:function(bw,bv,bx){var e=typeof bw==="boolean";if(b.isFunction(bw)&&b.isFunction(bv)){this._toggle.apply(this,arguments)}else{if(bw==null||e){this.each(function(){var by=e?bw:b(this).is(":hidden");b(this)[by?"show":"hide"]()})}else{this.animate(a0("toggle",3),bw,bv,bx)}}return this},fadeTo:function(e,bx,bw,bv){return this.filter(":hidden").css("opacity",0).show().end().animate({opacity:bx},e,bw,bv)},animate:function(bz,bw,by,bx){var e=b.speed(bw,by,bx);if(b.isEmptyObject(bz)){return this.each(e.complete,[false])}bz=b.extend({},bz);function bv(){if(e.queue===false){b._mark(this)}var bE=b.extend({},e),bK=this.nodeType===1,bI=bK&&b(this).is(":hidden"),bB,bF,bD,bJ,bH,bC,bG,bL,bA;bE.animatedProperties={};for(bD in bz){bB=b.camelCase(bD);if(bD!==bB){bz[bB]=bz[bD];delete bz[bD]}bF=bz[bB];if(b.isArray(bF)){bE.animatedProperties[bB]=bF[1];bF=bz[bB]=bF[0]}else{bE.animatedProperties[bB]=bE.specialEasing&&bE.specialEasing[bB]||bE.easing||"swing"}if(bF==="hide"&&bI||bF==="show"&&!bI){return bE.complete.call(this)}if(bK&&(bB==="height"||bB==="width")){bE.overflow=[this.style.overflow,this.style.overflowX,this.style.overflowY];if(b.css(this,"display")==="inline"&&b.css(this,"float")==="none"){if(!b.support.inlineBlockNeedsLayout||x(this.nodeName)==="inline"){this.style.display="inline-block"}else{this.style.zoom=1}}}}if(bE.overflow!=null){this.style.overflow="hidden"}for(bD in bz){bJ=new b.fx(this,bE,bD);bF=bz[bD];if(aB.test(bF)){bA=b._data(this,"toggle"+bD)||(bF==="toggle"?bI?"show":"hide":0);if(bA){b._data(this,"toggle"+bD,bA==="show"?"hide":"show");bJ[bA]()}else{bJ[bF]()}}else{bH=aT.exec(bF);bC=bJ.cur();if(bH){bG=parseFloat(bH[2]);bL=bH[3]||(b.cssNumber[bD]?"":"px");if(bL!=="px"){b.style(this,bD,(bG||1)+bL);bC=((bG||1)/bJ.cur())*bC;b.style(this,bD,bC+bL)}if(bH[1]){bG=((bH[1]==="-="?-1:1)*bG)+bC}bJ.custom(bC,bG,bL)}else{bJ.custom(bC,bF,"")}}}return true}return e.queue===false?this.each(bv):this.queue(e.queue,bv)},stop:function(bw,bv,e){if(typeof bw!=="string"){e=bv;bv=bw;bw=L}if(bv&&bw!==false){this.queue(bw||"fx",[])}return this.each(function(){var bx,by=false,bA=b.timers,bz=b._data(this);if(!e){b._unmark(true,this)}function bB(bE,bF,bD){var bC=bF[bD];b.removeData(bE,bD,true);bC.stop(e)}if(bw==null){for(bx in bz){if(bz[bx]&&bz[bx].stop&&bx.indexOf(".run")===bx.length-4){bB(this,bz,bx)}}}else{if(bz[bx=bw+".run"]&&bz[bx].stop){bB(this,bz,bx)}}for(bx=bA.length;bx--;){if(bA[bx].elem===this&&(bw==null||bA[bx].queue===bw)){if(e){bA[bx](true)}else{bA[bx].saveState()}by=true;bA.splice(bx,1)}}if(!(e&&by)){b.dequeue(this,bw)}})}});function bh(){setTimeout(at,0);return(a4=b.now())}function at(){a4=L}function a0(bv,e){var bw={};b.each(aH.concat.apply([],aH.slice(0,e)),function(){bw[this]=bv});return bw}b.each({slideDown:a0("show",1),slideUp:a0("hide",1),slideToggle:a0("toggle",1),fadeIn:{opacity:"show"},fadeOut:{opacity:"hide"},fadeToggle:{opacity:"toggle"}},function(e,bv){b.fn[e]=function(bw,by,bx){return this.animate(bv,bw,by,bx)}});b.extend({speed:function(bw,bx,bv){var e=bw&&typeof bw==="object"?b.extend({},bw):{complete:bv||!bv&&bx||b.isFunction(bw)&&bw,duration:bw,easing:bv&&bx||bx&&!b.isFunction(bx)&&bx};e.duration=b.fx.off?0:typeof e.duration==="number"?e.duration:e.duration in b.fx.speeds?b.fx.speeds[e.duration]:b.fx.speeds._default;if(e.queue==null||e.queue===true){e.queue="fx"}e.old=e.complete;e.complete=function(by){if(b.isFunction(e.old)){e.old.call(this)}if(e.queue){b.dequeue(this,e.queue)}else{if(by!==false){b._unmark(this)}}};return e},easing:{linear:function(bw,bx,e,bv){return e+bv*bw},swing:function(bw,bx,e,bv){return((-Math.cos(bw*Math.PI)/2)+0.5)*bv+e}},timers:[],fx:function(bv,e,bw){this.options=e;this.elem=bv;this.prop=bw;e.orig=e.orig||{}}});b.fx.prototype={update:function(){if(this.options.step){this.options.step.call(this.elem,this.now,this)}(b.fx.step[this.prop]||b.fx.step._default)(this)},cur:function(){if(this.elem[this.prop]!=null&&(!this.elem.style||this.elem.style[this.prop]==null)){return this.elem[this.prop]}var e,bv=b.css(this.elem,this.prop);return isNaN(e=parseFloat(bv))?!bv||bv==="auto"?0:bv:e},custom:function(bz,by,bx){var e=this,bw=b.fx;this.startTime=a4||bh();this.end=by;this.now=this.start=bz;this.pos=this.state=0;this.unit=bx||this.unit||(b.cssNumber[this.prop]?"":"px");function bv(bA){return e.step(bA)}bv.queue=this.options.queue;bv.elem=this.elem;bv.saveState=function(){if(e.options.hide&&b._data(e.elem,"fxshow"+e.prop)===L){b._data(e.elem,"fxshow"+e.prop,e.start)}};if(bv()&&b.timers.push(bv)&&!a3){a3=setInterval(bw.tick,bw.interval)}},show:function(){var e=b._data(this.elem,"fxshow"+this.prop);this.options.orig[this.prop]=e||b.style(this.elem,this.prop);this.options.show=true;if(e!==L){this.custom(this.cur(),e)}else{this.custom(this.prop==="width"||this.prop==="height"?1:0,this.cur())}b(this.elem).show()},hide:function(){this.options.orig[this.prop]=b._data(this.elem,"fxshow"+this.prop)||b.style(this.elem,this.prop);this.options.hide=true;this.custom(this.cur(),0)},step:function(by){var bA,bB,bv,bx=a4||bh(),e=true,bz=this.elem,bw=this.options;if(by||bx>=bw.duration+this.startTime){this.now=this.end;this.pos=this.state=1;this.update();bw.animatedProperties[this.prop]=true;for(bA in bw.animatedProperties){if(bw.animatedProperties[bA]!==true){e=false}}if(e){if(bw.overflow!=null&&!b.support.shrinkWrapBlocks){b.each(["","X","Y"],function(bC,bD){bz.style["overflow"+bD]=bw.overflow[bC]})}if(bw.hide){b(bz).hide()}if(bw.hide||bw.show){for(bA in bw.animatedProperties){b.style(bz,bA,bw.orig[bA]);b.removeData(bz,"fxshow"+bA,true);b.removeData(bz,"toggle"+bA,true)}}bv=bw.complete;if(bv){bw.complete=false;bv.call(bz)}}return false}else{if(bw.duration==Infinity){this.now=bx}else{bB=bx-this.startTime;this.state=bB/bw.duration;this.pos=b.easing[bw.animatedProperties[this.prop]](this.state,bB,0,1,bw.duration);this.now=this.start+((this.end-this.start)*this.pos)}this.update()}return true}};b.extend(b.fx,{tick:function(){var bw,bv=b.timers,e=0;for(;e<bv.length;e++){bw=bv[e];if(!bw()&&bv[e]===bw){bv.splice(e--,1)}}if(!bv.length){b.fx.stop()}},interval:13,stop:function(){clearInterval(a3);a3=null},speeds:{slow:600,fast:200,_default:400},step:{opacity:function(e){b.style(e.elem,"opacity",e.now)},_default:function(e){if(e.elem.style&&e.elem.style[e.prop]!=null){e.elem.style[e.prop]=e.now+e.unit}else{e.elem[e.prop]=e.now}}}});b.each(["width","height"],function(e,bv){b.fx.step[bv]=function(bw){b.style(bw.elem,bv,Math.max(0,bw.now)+bw.unit)}});if(b.expr&&b.expr.filters){b.expr.filters.animated=function(e){return b.grep(b.timers,function(bv){return e===bv.elem}).length}}function x(bx){if(!Q[bx]){var e=av.body,bv=b("<"+bx+">").appendTo(e),bw=bv.css("display");bv.remove();if(bw==="none"||bw===""){if(!a8){a8=av.createElement("iframe");a8.frameBorder=a8.width=a8.height=0}e.appendChild(a8);if(!m||!a8.createElement){m=(a8.contentWindow||a8.contentDocument).document;m.write((av.compatMode==="CSS1Compat"?"<!doctype html>":"")+"<html><body>");m.close()}bv=m.createElement(bx);m.body.appendChild(bv);bw=b.css(bv,"display");e.removeChild(a8)}Q[bx]=bw}return Q[bx]}var V=/^t(?:able|d|h)$/i,ad=/^(?:body|html)$/i;if("getBoundingClientRect" in av.documentElement){b.fn.offset=function(bI){var by=this[0],bB;if(bI){return this.each(function(e){b.offset.setOffset(this,bI,e)})}if(!by||!by.ownerDocument){return null}if(by===by.ownerDocument.body){return b.offset.bodyOffset(by)}try{bB=by.getBoundingClientRect()}catch(bF){}var bH=by.ownerDocument,bw=bH.documentElement;if(!bB||!b.contains(bw,by)){return bB?{top:bB.top,left:bB.left}:{top:0,left:0}}var bC=bH.body,bD=aK(bH),bA=bw.clientTop||bC.clientTop||0,bE=bw.clientLeft||bC.clientLeft||0,bv=bD.pageYOffset||b.support.boxModel&&bw.scrollTop||bC.scrollTop,bz=bD.pageXOffset||b.support.boxModel&&bw.scrollLeft||bC.scrollLeft,bG=bB.top+bv-bA,bx=bB.left+bz-bE;return{top:bG,left:bx}}}else{b.fn.offset=function(bF){var bz=this[0];if(bF){return this.each(function(bG){b.offset.setOffset(this,bF,bG)})}if(!bz||!bz.ownerDocument){return null}if(bz===bz.ownerDocument.body){return b.offset.bodyOffset(bz)}var bC,bw=bz.offsetParent,bv=bz,bE=bz.ownerDocument,bx=bE.documentElement,bA=bE.body,bB=bE.defaultView,e=bB?bB.getComputedStyle(bz,null):bz.currentStyle,bD=bz.offsetTop,by=bz.offsetLeft;while((bz=bz.parentNode)&&bz!==bA&&bz!==bx){if(b.support.fixedPosition&&e.position==="fixed"){break}bC=bB?bB.getComputedStyle(bz,null):bz.currentStyle;bD-=bz.scrollTop;by-=bz.scrollLeft;if(bz===bw){bD+=bz.offsetTop;by+=bz.offsetLeft;if(b.support.doesNotAddBorder&&!(b.support.doesAddBorderForTableAndCells&&V.test(bz.nodeName))){bD+=parseFloat(bC.borderTopWidth)||0;by+=parseFloat(bC.borderLeftWidth)||0}bv=bw;bw=bz.offsetParent}if(b.support.subtractsBorderForOverflowNotVisible&&bC.overflow!=="visible"){bD+=parseFloat(bC.borderTopWidth)||0;by+=parseFloat(bC.borderLeftWidth)||0}e=bC}if(e.position==="relative"||e.position==="static"){bD+=bA.offsetTop;by+=bA.offsetLeft}if(b.support.fixedPosition&&e.position==="fixed"){bD+=Math.max(bx.scrollTop,bA.scrollTop);by+=Math.max(bx.scrollLeft,bA.scrollLeft)}return{top:bD,left:by}}}b.offset={bodyOffset:function(e){var bw=e.offsetTop,bv=e.offsetLeft;if(b.support.doesNotIncludeMarginInBodyOffset){bw+=parseFloat(b.css(e,"marginTop"))||0;bv+=parseFloat(b.css(e,"marginLeft"))||0}return{top:bw,left:bv}},setOffset:function(bx,bG,bA){var bB=b.css(bx,"position");if(bB==="static"){bx.style.position="relative"}var bz=b(bx),bv=bz.offset(),e=b.css(bx,"top"),bE=b.css(bx,"left"),bF=(bB==="absolute"||bB==="fixed")&&b.inArray("auto",[e,bE])>-1,bD={},bC={},bw,by;if(bF){bC=bz.position();bw=bC.top;by=bC.left}else{bw=parseFloat(e)||0;by=parseFloat(bE)||0}if(b.isFunction(bG)){bG=bG.call(bx,bA,bv)}if(bG.top!=null){bD.top=(bG.top-bv.top)+bw}if(bG.left!=null){bD.left=(bG.left-bv.left)+by}if("using" in bG){bG.using.call(bx,bD)}else{bz.css(bD)}}};b.fn.extend({position:function(){if(!this[0]){return null}var bw=this[0],bv=this.offsetParent(),bx=this.offset(),e=ad.test(bv[0].nodeName)?{top:0,left:0}:bv.offset();bx.top-=parseFloat(b.css(bw,"marginTop"))||0;bx.left-=parseFloat(b.css(bw,"marginLeft"))||0;e.top+=parseFloat(b.css(bv[0],"borderTopWidth"))||0;e.left+=parseFloat(b.css(bv[0],"borderLeftWidth"))||0;return{top:bx.top-e.top,left:bx.left-e.left}},offsetParent:function(){return this.map(function(){var e=this.offsetParent||av.body;while(e&&(!ad.test(e.nodeName)&&b.css(e,"position")==="static")){e=e.offsetParent}return e})}});b.each(["Left","Top"],function(bv,e){var bw="scroll"+e;b.fn[bw]=function(bz){var bx,by;if(bz===L){bx=this[0];if(!bx){return null}by=aK(bx);return by?("pageXOffset" in by)?by[bv?"pageYOffset":"pageXOffset"]:b.support.boxModel&&by.document.documentElement[bw]||by.document.body[bw]:bx[bw]}return this.each(function(){by=aK(this);if(by){by.scrollTo(!bv?bz:b(by).scrollLeft(),bv?bz:b(by).scrollTop())}else{this[bw]=bz}})}});function aK(e){return b.isWindow(e)?e:e.nodeType===9?e.defaultView||e.parentWindow:false}b.each(["Height","Width"],function(bv,e){var bw=e.toLowerCase();b.fn["inner"+e]=function(){var bx=this[0];return bx?bx.style?parseFloat(b.css(bx,bw,"padding")):this[bw]():null};b.fn["outer"+e]=function(by){var bx=this[0];return bx?bx.style?parseFloat(b.css(bx,bw,by?"margin":"border")):this[bw]():null};b.fn[bw]=function(bz){var bA=this[0];if(!bA){return bz==null?null:this}if(b.isFunction(bz)){return this.each(function(bE){var bD=b(this);bD[bw](bz.call(this,bE,bD[bw]()))})}if(b.isWindow(bA)){var bB=bA.document.documentElement["client"+e],bx=bA.document.body;return bA.document.compatMode==="CSS1Compat"&&bB||bx&&bx["client"+e]||bB}else{if(bA.nodeType===9){return Math.max(bA.documentElement["client"+e],bA.body["scroll"+e],bA.documentElement["scroll"+e],bA.body["offset"+e],bA.documentElement["offset"+e])}else{if(bz===L){var bC=b.css(bA,bw),by=parseFloat(bC);return b.isNumeric(by)?by:bC}else{return this.css(bw,typeof bz==="string"?bz:bz+"px")}}}}});bb.jQuery=bb.$=b;if(typeof define==="function"&&define.amd&&define.amd.jQuery){define("jquery",[],function(){return b})}})(window);/*
+ * jQuery UI 1.8.18
+ *
+ * Copyright 2011, AUTHORS.txt (http://jqueryui.com/about)
+ * Dual licensed under the MIT or GPL Version 2 licenses.
+ * http://jquery.org/license
+ *
+ * http://docs.jquery.com/UI
+ */
+(function(a,d){a.ui=a.ui||{};if(a.ui.version){return}a.extend(a.ui,{version:"1.8.18",keyCode:{ALT:18,BACKSPACE:8,CAPS_LOCK:20,COMMA:188,COMMAND:91,COMMAND_LEFT:91,COMMAND_RIGHT:93,CONTROL:17,DELETE:46,DOWN:40,END:35,ENTER:13,ESCAPE:27,HOME:36,INSERT:45,LEFT:37,MENU:93,NUMPAD_ADD:107,NUMPAD_DECIMAL:110,NUMPAD_DIVIDE:111,NUMPAD_ENTER:108,NUMPAD_MULTIPLY:106,NUMPAD_SUBTRACT:109,PAGE_DOWN:34,PAGE_UP:33,PERIOD:190,RIGHT:39,SHIFT:16,SPACE:32,TAB:9,UP:38,WINDOWS:91}});a.fn.extend({propAttr:a.fn.prop||a.fn.attr,_focus:a.fn.focus,focus:function(e,f){return typeof e==="number"?this.each(function(){var g=this;setTimeout(function(){a(g).focus();if(f){f.call(g)}},e)}):this._focus.apply(this,arguments)},scrollParent:function(){var e;if((a.browser.msie&&(/(static|relative)/).test(this.css("position")))||(/absolute/).test(this.css("position"))){e=this.parents().filter(function(){return(/(relative|absolute|fixed)/).test(a.curCSS(this,"position",1))&&(/(auto|scroll)/).test(a.curCSS(this,"overflow",1)+a.curCSS(this,"overflow-y",1)+a.curCSS(this,"overflow-x",1))}).eq(0)}else{e=this.parents().filter(function(){return(/(auto|scroll)/).test(a.curCSS(this,"overflow",1)+a.curCSS(this,"overflow-y",1)+a.curCSS(this,"overflow-x",1))}).eq(0)}return(/fixed/).test(this.css("position"))||!e.length?a(document):e},zIndex:function(h){if(h!==d){return this.css("zIndex",h)}if(this.length){var f=a(this[0]),e,g;while(f.length&&f[0]!==document){e=f.css("position");if(e==="absolute"||e==="relative"||e==="fixed"){g=parseInt(f.css("zIndex"),10);if(!isNaN(g)&&g!==0){return g}}f=f.parent()}}return 0},disableSelection:function(){return this.bind((a.support.selectstart?"selectstart":"mousedown")+".ui-disableSelection",function(e){e.preventDefault()})},enableSelection:function(){return this.unbind(".ui-disableSelection")}});a.each(["Width","Height"],function(g,e){var f=e==="Width"?["Left","Right"]:["Top","Bottom"],h=e.toLowerCase(),k={innerWidth:a.fn.innerWidth,innerHeight:a.fn.innerHeight,outerWidth:a.fn.outerWidth,outerHeight:a.fn.outerHeight};function j(m,l,i,n){a.each(f,function(){l-=parseFloat(a.curCSS(m,"padding"+this,true))||0;if(i){l-=parseFloat(a.curCSS(m,"border"+this+"Width",true))||0}if(n){l-=parseFloat(a.curCSS(m,"margin"+this,true))||0}});return l}a.fn["inner"+e]=function(i){if(i===d){return k["inner"+e].call(this)}return this.each(function(){a(this).css(h,j(this,i)+"px")})};a.fn["outer"+e]=function(i,l){if(typeof i!=="number"){return k["outer"+e].call(this,i)}return this.each(function(){a(this).css(h,j(this,i,true,l)+"px")})}});function c(g,e){var j=g.nodeName.toLowerCase();if("area"===j){var i=g.parentNode,h=i.name,f;if(!g.href||!h||i.nodeName.toLowerCase()!=="map"){return false}f=a("img[usemap=#"+h+"]")[0];return !!f&&b(f)}return(/input|select|textarea|button|object/.test(j)?!g.disabled:"a"==j?g.href||e:e)&&b(g)}function b(e){return !a(e).parents().andSelf().filter(function(){return a.curCSS(this,"visibility")==="hidden"||a.expr.filters.hidden(this)}).length}a.extend(a.expr[":"],{data:function(g,f,e){return !!a.data(g,e[3])},focusable:function(e){return c(e,!isNaN(a.attr(e,"tabindex")))},tabbable:function(g){var e=a.attr(g,"tabindex"),f=isNaN(e);return(f||e>=0)&&c(g,!f)}});a(function(){var e=document.body,f=e.appendChild(f=document.createElement("div"));f.offsetHeight;a.extend(f.style,{minHeight:"100px",height:"auto",padding:0,borderWidth:0});a.support.minHeight=f.offsetHeight===100;a.support.selectstart="onselectstart" in f;e.removeChild(f).style.display="none"});a.extend(a.ui,{plugin:{add:function(f,g,j){var h=a.ui[f].prototype;for(var e in j){h.plugins[e]=h.plugins[e]||[];h.plugins[e].push([g,j[e]])}},call:function(e,g,f){var j=e.plugins[g];if(!j||!e.element[0].parentNode){return}for(var h=0;h<j.length;h++){if(e.options[j[h][0]]){j[h][1].apply(e.element,f)}}}},contains:function(f,e){return document.compareDocumentPosition?f.compareDocumentPosition(e)&16:f!==e&&f.contains(e)},hasScroll:function(h,f){if(a(h).css("overflow")==="hidden"){return false}var e=(f&&f==="left")?"scrollLeft":"scrollTop",g=false;if(h[e]>0){return true}h[e]=1;g=(h[e]>0);h[e]=0;return g},isOverAxis:function(f,e,g){return(f>e)&&(f<(e+g))},isOver:function(j,f,i,h,e,g){return a.ui.isOverAxis(j,i,e)&&a.ui.isOverAxis(f,h,g)}})})(jQuery);/*
+ * jQuery UI Widget 1.8.18
+ *
+ * Copyright 2011, AUTHORS.txt (http://jqueryui.com/about)
+ * Dual licensed under the MIT or GPL Version 2 licenses.
+ * http://jquery.org/license
+ *
+ * http://docs.jquery.com/UI/Widget
+ */
+(function(b,d){if(b.cleanData){var c=b.cleanData;b.cleanData=function(f){for(var g=0,h;(h=f[g])!=null;g++){try{b(h).triggerHandler("remove")}catch(j){}}c(f)}}else{var a=b.fn.remove;b.fn.remove=function(e,f){return this.each(function(){if(!f){if(!e||b.filter(e,[this]).length){b("*",this).add([this]).each(function(){try{b(this).triggerHandler("remove")}catch(g){}})}}return a.call(b(this),e,f)})}}b.widget=function(f,h,e){var g=f.split(".")[0],j;f=f.split(".")[1];j=g+"-"+f;if(!e){e=h;h=b.Widget}b.expr[":"][j]=function(k){return !!b.data(k,f)};b[g]=b[g]||{};b[g][f]=function(k,l){if(arguments.length){this._createWidget(k,l)}};var i=new h();i.options=b.extend(true,{},i.options);b[g][f].prototype=b.extend(true,i,{namespace:g,widgetName:f,widgetEventPrefix:b[g][f].prototype.widgetEventPrefix||f,widgetBaseClass:j},e);b.widget.bridge(f,b[g][f])};b.widget.bridge=function(f,e){b.fn[f]=function(i){var g=typeof i==="string",h=Array.prototype.slice.call(arguments,1),j=this;i=!g&&h.length?b.extend.apply(null,[true,i].concat(h)):i;if(g&&i.charAt(0)==="_"){return j}if(g){this.each(function(){var k=b.data(this,f),l=k&&b.isFunction(k[i])?k[i].apply(k,h):k;if(l!==k&&l!==d){j=l;return false}})}else{this.each(function(){var k=b.data(this,f);if(k){k.option(i||{})._init()}else{b.data(this,f,new e(i,this))}})}return j}};b.Widget=function(e,f){if(arguments.length){this._createWidget(e,f)}};b.Widget.prototype={widgetName:"widget",widgetEventPrefix:"",options:{disabled:false},_createWidget:function(f,g){b.data(g,this.widgetName,this);this.element=b(g);this.options=b.extend(true,{},this.options,this._getCreateOptions(),f);var e=this;this.element.bind("remove."+this.widgetName,function(){e.destroy()});this._create();this._trigger("create");this._init()},_getCreateOptions:function(){return b.metadata&&b.metadata.get(this.element[0])[this.widgetName]},_create:function(){},_init:function(){},destroy:function(){this.element.unbind("."+this.widgetName).removeData(this.widgetName);this.widget().unbind("."+this.widgetName).removeAttr("aria-disabled").removeClass(this.widgetBaseClass+"-disabled ui-state-disabled")},widget:function(){return this.element},option:function(f,g){var e=f;if(arguments.length===0){return b.extend({},this.options)}if(typeof f==="string"){if(g===d){return this.options[f]}e={};e[f]=g}this._setOptions(e);return this},_setOptions:function(f){var e=this;b.each(f,function(g,h){e._setOption(g,h)});return this},_setOption:function(e,f){this.options[e]=f;if(e==="disabled"){this.widget()[f?"addClass":"removeClass"](this.widgetBaseClass+"-disabled ui-state-disabled").attr("aria-disabled",f)}return this},enable:function(){return this._setOption("disabled",false)},disable:function(){return this._setOption("disabled",true)},_trigger:function(e,f,g){var j,i,h=this.options[e];g=g||{};f=b.Event(f);f.type=(e===this.widgetEventPrefix?e:this.widgetEventPrefix+e).toLowerCase();f.target=this.element[0];i=f.originalEvent;if(i){for(j in i){if(!(j in f)){f[j]=i[j]}}}this.element.trigger(f,g);return !(b.isFunction(h)&&h.call(this.element[0],f,g)===false||f.isDefaultPrevented())}}})(jQuery);/*
+ * jQuery UI Mouse 1.8.18
+ *
+ * Copyright 2011, AUTHORS.txt (http://jqueryui.com/about)
+ * Dual licensed under the MIT or GPL Version 2 licenses.
+ * http://jquery.org/license
+ *
+ * http://docs.jquery.com/UI/Mouse
+ *
+ * Depends:
+ * jquery.ui.widget.js
+ */
+(function(b,c){var a=false;b(document).mouseup(function(d){a=false});b.widget("ui.mouse",{options:{cancel:":input,option",distance:1,delay:0},_mouseInit:function(){var d=this;this.element.bind("mousedown."+this.widgetName,function(e){return d._mouseDown(e)}).bind("click."+this.widgetName,function(e){if(true===b.data(e.target,d.widgetName+".preventClickEvent")){b.removeData(e.target,d.widgetName+".preventClickEvent");e.stopImmediatePropagation();return false}});this.started=false},_mouseDestroy:function(){this.element.unbind("."+this.widgetName)},_mouseDown:function(f){if(a){return}(this._mouseStarted&&this._mouseUp(f));this._mouseDownEvent=f;var e=this,g=(f.which==1),d=(typeof this.options.cancel=="string"&&f.target.nodeName?b(f.target).closest(this.options.cancel).length:false);if(!g||d||!this._mouseCapture(f)){return true}this.mouseDelayMet=!this.options.delay;if(!this.mouseDelayMet){this._mouseDelayTimer=setTimeout(function(){e.mouseDelayMet=true},this.options.delay)}if(this._mouseDistanceMet(f)&&this._mouseDelayMet(f)){this._mouseStarted=(this._mouseStart(f)!==false);if(!this._mouseStarted){f.preventDefault();return true}}if(true===b.data(f.target,this.widgetName+".preventClickEvent")){b.removeData(f.target,this.widgetName+".preventClickEvent")}this._mouseMoveDelegate=function(h){return e._mouseMove(h)};this._mouseUpDelegate=function(h){return e._mouseUp(h)};b(document).bind("mousemove."+this.widgetName,this._mouseMoveDelegate).bind("mouseup."+this.widgetName,this._mouseUpDelegate);f.preventDefault();a=true;return true},_mouseMove:function(d){if(b.browser.msie&&!(document.documentMode>=9)&&!d.button){return this._mouseUp(d)}if(this._mouseStarted){this._mouseDrag(d);return d.preventDefault()}if(this._mouseDistanceMet(d)&&this._mouseDelayMet(d)){this._mouseStarted=(this._mouseStart(this._mouseDownEvent,d)!==false);(this._mouseStarted?this._mouseDrag(d):this._mouseUp(d))}return !this._mouseStarted},_mouseUp:function(d){b(document).unbind("mousemove."+this.widgetName,this._mouseMoveDelegate).unbind("mouseup."+this.widgetName,this._mouseUpDelegate);if(this._mouseStarted){this._mouseStarted=false;if(d.target==this._mouseDownEvent.target){b.data(d.target,this.widgetName+".preventClickEvent",true)}this._mouseStop(d)}return false},_mouseDistanceMet:function(d){return(Math.max(Math.abs(this._mouseDownEvent.pageX-d.pageX),Math.abs(this._mouseDownEvent.pageY-d.pageY))>=this.options.distance)},_mouseDelayMet:function(d){return this.mouseDelayMet},_mouseStart:function(d){},_mouseDrag:function(d){},_mouseStop:function(d){},_mouseCapture:function(d){return true}})})(jQuery);(function(c,d){c.widget("ui.resizable",c.ui.mouse,{widgetEventPrefix:"resize",options:{alsoResize:false,animate:false,animateDuration:"slow",animateEasing:"swing",aspectRatio:false,autoHide:false,containment:false,ghost:false,grid:false,handles:"e,s,se",helper:false,maxHeight:null,maxWidth:null,minHeight:10,minWidth:10,zIndex:1000},_create:function(){var f=this,k=this.options;this.element.addClass("ui-resizable");c.extend(this,{_aspectRatio:!!(k.aspectRatio),aspectRatio:k.aspectRatio,originalElement:this.element,_proportionallyResizeElements:[],_helper:k.helper||k.ghost||k.animate?k.helper||"ui-resizable-helper":null});if(this.element[0].nodeName.match(/canvas|textarea|input|select|button|img/i)){this.element.wrap(c('<div class="ui-wrapper" style="overflow: hidden;"></div>').css({position:this.element.css("position"),width:this.element.outerWidth(),height:this.element.outerHeight(),top:this.element.css("top"),left:this.element.css("left")}));this.element=this.element.parent().data("resizable",this.element.data("resizable"));this.elementIsWrapper=true;this.element.css({marginLeft:this.originalElement.css("marginLeft"),marginTop:this.originalElement.css("marginTop"),marginRight:this.originalElement.css("marginRight"),marginBottom:this.originalElement.css("marginBottom")});this.originalElement.css({marginLeft:0,marginTop:0,marginRight:0,marginBottom:0});this.originalResizeStyle=this.originalElement.css("resize");this.originalElement.css("resize","none");this._proportionallyResizeElements.push(this.originalElement.css({position:"static",zoom:1,display:"block"}));this.originalElement.css({margin:this.originalElement.css("margin")});this._proportionallyResize()}this.handles=k.handles||(!c(".ui-resizable-handle",this.element).length?"e,s,se":{n:".ui-resizable-n",e:".ui-resizable-e",s:".ui-resizable-s",w:".ui-resizable-w",se:".ui-resizable-se",sw:".ui-resizable-sw",ne:".ui-resizable-ne",nw:".ui-resizable-nw"});if(this.handles.constructor==String){if(this.handles=="all"){this.handles="n,e,s,w,se,sw,ne,nw"}var l=this.handles.split(",");this.handles={};for(var g=0;g<l.length;g++){var j=c.trim(l[g]),e="ui-resizable-"+j;var h=c('<div class="ui-resizable-handle '+e+'"></div>');if(/sw|se|ne|nw/.test(j)){h.css({zIndex:++k.zIndex})}if("se"==j){h.addClass("ui-icon ui-icon-gripsmall-diagonal-se")}this.handles[j]=".ui-resizable-"+j;this.element.append(h)}}this._renderAxis=function(q){q=q||this.element;for(var n in this.handles){if(this.handles[n].constructor==String){this.handles[n]=c(this.handles[n],this.element).show()}if(this.elementIsWrapper&&this.originalElement[0].nodeName.match(/textarea|input|select|button/i)){var o=c(this.handles[n],this.element),p=0;p=/sw|ne|nw|se|n|s/.test(n)?o.outerHeight():o.outerWidth();var m=["padding",/ne|nw|n/.test(n)?"Top":/se|sw|s/.test(n)?"Bottom":/^e$/.test(n)?"Right":"Left"].join("");q.css(m,p);this._proportionallyResize()}if(!c(this.handles[n]).length){continue}}};this._renderAxis(this.element);this._handles=c(".ui-resizable-handle",this.element).disableSelection();this._handles.mouseover(function(){if(!f.resizing){if(this.className){var i=this.className.match(/ui-resizable-(se|sw|ne|nw|n|e|s|w)/i)}f.axis=i&&i[1]?i[1]:"se"}});if(k.autoHide){this._handles.hide();c(this.element).addClass("ui-resizable-autohide").hover(function(){if(k.disabled){return}c(this).removeClass("ui-resizable-autohide");f._handles.show()},function(){if(k.disabled){return}if(!f.resizing){c(this).addClass("ui-resizable-autohide");f._handles.hide()}})}this._mouseInit()},destroy:function(){this._mouseDestroy();var e=function(g){c(g).removeClass("ui-resizable ui-resizable-disabled ui-resizable-resizing").removeData("resizable").unbind(".resizable").find(".ui-resizable-handle").remove()};if(this.elementIsWrapper){e(this.element);var f=this.element;f.after(this.originalElement.css({position:f.css("position"),width:f.outerWidth(),height:f.outerHeight(),top:f.css("top"),left:f.css("left")})).remove()}this.originalElement.css("resize",this.originalResizeStyle);e(this.originalElement);return this},_mouseCapture:function(f){var g=false;for(var e in this.handles){if(c(this.handles[e])[0]==f.target){g=true}}return !this.options.disabled&&g},_mouseStart:function(g){var j=this.options,f=this.element.position(),e=this.element;this.resizing=true;this.documentScroll={top:c(document).scrollTop(),left:c(document).scrollLeft()};if(e.is(".ui-draggable")||(/absolute/).test(e.css("position"))){e.css({position:"absolute",top:f.top,left:f.left})}this._renderProxy();var k=b(this.helper.css("left")),h=b(this.helper.css("top"));if(j.containment){k+=c(j.containment).scrollLeft()||0;h+=c(j.containment).scrollTop()||0}this.offset=this.helper.offset();this.position={left:k,top:h};this.size=this._helper?{width:e.outerWidth(),height:e.outerHeight()}:{width:e.width(),height:e.height()};this.originalSize=this._helper?{width:e.outerWidth(),height:e.outerHeight()}:{width:e.width(),height:e.height()};this.originalPosition={left:k,top:h};this.sizeDiff={width:e.outerWidth()-e.width(),height:e.outerHeight()-e.height()};this.originalMousePosition={left:g.pageX,top:g.pageY};this.aspectRatio=(typeof j.aspectRatio=="number")?j.aspectRatio:((this.originalSize.width/this.originalSize.height)||1);var i=c(".ui-resizable-"+this.axis).css("cursor");c("body").css("cursor",i=="auto"?this.axis+"-resize":i);e.addClass("ui-resizable-resizing");this._propagate("start",g);return true},_mouseDrag:function(e){var h=this.helper,g=this.options,m={},q=this,j=this.originalMousePosition,n=this.axis;var r=(e.pageX-j.left)||0,p=(e.pageY-j.top)||0;var i=this._change[n];if(!i){return false}var l=i.apply(this,[e,r,p]),k=c.browser.msie&&c.browser.version<7,f=this.sizeDiff;this._updateVirtualBoundaries(e.shiftKey);if(this._aspectRatio||e.shiftKey){l=this._updateRatio(l,e)}l=this._respectSize(l,e);this._propagate("resize",e);h.css({top:this.position.top+"px",left:this.position.left+"px",width:this.size.width+"px",height:this.size.height+"px"});if(!this._helper&&this._proportionallyResizeElements.length){this._proportionallyResize()}this._updateCache(l);this._trigger("resize",e,this.ui());return false},_mouseStop:function(h){this.resizing=false;var i=this.options,m=this;if(this._helper){var g=this._proportionallyResizeElements,e=g.length&&(/textarea/i).test(g[0].nodeName),f=e&&c.ui.hasScroll(g[0],"left")?0:m.sizeDiff.height,k=e?0:m.sizeDiff.width;var n={width:(m.helper.width()-k),height:(m.helper.height()-f)},j=(parseInt(m.element.css("left"),10)+(m.position.left-m.originalPosition.left))||null,l=(parseInt(m.element.css("top"),10)+(m.position.top-m.originalPosition.top))||null;if(!i.animate){this.element.css(c.extend(n,{top:l,left:j}))}m.helper.height(m.size.height);m.helper.width(m.size.width);if(this._helper&&!i.animate){this._proportionallyResize()}}c("body").css("cursor","auto");this.element.removeClass("ui-resizable-resizing");this._propagate("stop",h);if(this._helper){this.helper.remove()}return false},_updateVirtualBoundaries:function(g){var j=this.options,i,h,f,k,e;e={minWidth:a(j.minWidth)?j.minWidth:0,maxWidth:a(j.maxWidth)?j.maxWidth:Infinity,minHeight:a(j.minHeight)?j.minHeight:0,maxHeight:a(j.maxHeight)?j.maxHeight:Infinity};if(this._aspectRatio||g){i=e.minHeight*this.aspectRatio;f=e.minWidth/this.aspectRatio;h=e.maxHeight*this.aspectRatio;k=e.maxWidth/this.aspectRatio;if(i>e.minWidth){e.minWidth=i}if(f>e.minHeight){e.minHeight=f}if(h<e.maxWidth){e.maxWidth=h}if(k<e.maxHeight){e.maxHeight=k}}this._vBoundaries=e},_updateCache:function(e){var f=this.options;this.offset=this.helper.offset();if(a(e.left)){this.position.left=e.left}if(a(e.top)){this.position.top=e.top}if(a(e.height)){this.size.height=e.height}if(a(e.width)){this.size.width=e.width}},_updateRatio:function(h,g){var i=this.options,j=this.position,f=this.size,e=this.axis;if(a(h.height)){h.width=(h.height*this.aspectRatio)}else{if(a(h.width)){h.height=(h.width/this.aspectRatio)}}if(e=="sw"){h.left=j.left+(f.width-h.width);h.top=null}if(e=="nw"){h.top=j.top+(f.height-h.height);h.left=j.left+(f.width-h.width)}return h},_respectSize:function(l,g){var j=this.helper,i=this._vBoundaries,r=this._aspectRatio||g.shiftKey,q=this.axis,t=a(l.width)&&i.maxWidth&&(i.maxWidth<l.width),m=a(l.height)&&i.maxHeight&&(i.maxHeight<l.height),h=a(l.width)&&i.minWidth&&(i.minWidth>l.width),s=a(l.height)&&i.minHeight&&(i.minHeight>l.height);if(h){l.width=i.minWidth}if(s){l.height=i.minHeight}if(t){l.width=i.maxWidth}if(m){l.height=i.maxHeight}var f=this.originalPosition.left+this.originalSize.width,p=this.position.top+this.size.height;var k=/sw|nw|w/.test(q),e=/nw|ne|n/.test(q);if(h&&k){l.left=f-i.minWidth}if(t&&k){l.left=f-i.maxWidth}if(s&&e){l.top=p-i.minHeight}if(m&&e){l.top=p-i.maxHeight}var n=!l.width&&!l.height;if(n&&!l.left&&l.top){l.top=null}else{if(n&&!l.top&&l.left){l.left=null}}return l},_proportionallyResize:function(){var k=this.options;if(!this._proportionallyResizeElements.length){return}var g=this.helper||this.element;for(var f=0;f<this._proportionallyResizeElements.length;f++){var h=this._proportionallyResizeElements[f];if(!this.borderDif){var e=[h.css("borderTopWidth"),h.css("borderRightWidth"),h.css("borderBottomWidth"),h.css("borderLeftWidth")],j=[h.css("paddingTop"),h.css("paddingRight"),h.css("paddingBottom"),h.css("paddingLeft")];this.borderDif=c.map(e,function(l,n){var m=parseInt(l,10)||0,o=parseInt(j[n],10)||0;return m+o})}if(c.browser.msie&&!(!(c(g).is(":hidden")||c(g).parents(":hidden").length))){continue}h.css({height:(g.height()-this.borderDif[0]-this.borderDif[2])||0,width:(g.width()-this.borderDif[1]-this.borderDif[3])||0})}},_renderProxy:function(){var f=this.element,i=this.options;this.elementOffset=f.offset();if(this._helper){this.helper=this.helper||c('<div style="overflow:hidden;"></div>');var e=c.browser.msie&&c.browser.version<7,g=(e?1:0),h=(e?2:-1);this.helper.addClass(this._helper).css({width:this.element.outerWidth()+h,height:this.element.outerHeight()+h,position:"absolute",left:this.elementOffset.left-g+"px",top:this.elementOffset.top-g+"px",zIndex:++i.zIndex});this.helper.appendTo("body").disableSelection()}else{this.helper=this.element}},_change:{e:function(g,f,e){return{width:this.originalSize.width+f}},w:function(h,f,e){var j=this.options,g=this.originalSize,i=this.originalPosition;return{left:i.left+f,width:g.width-f}},n:function(h,f,e){var j=this.options,g=this.originalSize,i=this.originalPosition;return{top:i.top+e,height:g.height-e}},s:function(g,f,e){return{height:this.originalSize.height+e}},se:function(g,f,e){return c.extend(this._change.s.apply(this,arguments),this._change.e.apply(this,[g,f,e]))},sw:function(g,f,e){return c.extend(this._change.s.apply(this,arguments),this._change.w.apply(this,[g,f,e]))},ne:function(g,f,e){return c.extend(this._change.n.apply(this,arguments),this._change.e.apply(this,[g,f,e]))},nw:function(g,f,e){return c.extend(this._change.n.apply(this,arguments),this._change.w.apply(this,[g,f,e]))}},_propagate:function(f,e){c.ui.plugin.call(this,f,[e,this.ui()]);(f!="resize"&&this._trigger(f,e,this.ui()))},plugins:{},ui:function(){return{originalElement:this.originalElement,element:this.element,helper:this.helper,position:this.position,size:this.size,originalSize:this.originalSize,originalPosition:this.originalPosition}}});c.extend(c.ui.resizable,{version:"1.8.18"});c.ui.plugin.add("resizable","alsoResize",{start:function(f,g){var e=c(this).data("resizable"),i=e.options;var h=function(j){c(j).each(function(){var k=c(this);k.data("resizable-alsoresize",{width:parseInt(k.width(),10),height:parseInt(k.height(),10),left:parseInt(k.css("left"),10),top:parseInt(k.css("top"),10)})})};if(typeof(i.alsoResize)=="object"&&!i.alsoResize.parentNode){if(i.alsoResize.length){i.alsoResize=i.alsoResize[0];h(i.alsoResize)}else{c.each(i.alsoResize,function(j){h(j)})}}else{h(i.alsoResize)}},resize:function(g,i){var f=c(this).data("resizable"),j=f.options,h=f.originalSize,l=f.originalPosition;var k={height:(f.size.height-h.height)||0,width:(f.size.width-h.width)||0,top:(f.position.top-l.top)||0,left:(f.position.left-l.left)||0},e=function(m,n){c(m).each(function(){var q=c(this),r=c(this).data("resizable-alsoresize"),p={},o=n&&n.length?n:q.parents(i.originalElement[0]).length?["width","height"]:["width","height","top","left"];c.each(o,function(s,u){var t=(r[u]||0)+(k[u]||0);if(t&&t>=0){p[u]=t||null}});q.css(p)})};if(typeof(j.alsoResize)=="object"&&!j.alsoResize.nodeType){c.each(j.alsoResize,function(m,n){e(m,n)})}else{e(j.alsoResize)}},stop:function(e,f){c(this).removeData("resizable-alsoresize")}});c.ui.plugin.add("resizable","animate",{stop:function(i,n){var p=c(this).data("resizable"),j=p.options;var h=p._proportionallyResizeElements,e=h.length&&(/textarea/i).test(h[0].nodeName),f=e&&c.ui.hasScroll(h[0],"left")?0:p.sizeDiff.height,l=e?0:p.sizeDiff.width;var g={width:(p.size.width-l),height:(p.size.height-f)},k=(parseInt(p.element.css("left"),10)+(p.position.left-p.originalPosition.left))||null,m=(parseInt(p.element.css("top"),10)+(p.position.top-p.originalPosition.top))||null;p.element.animate(c.extend(g,m&&k?{top:m,left:k}:{}),{duration:j.animateDuration,easing:j.animateEasing,step:function(){var o={width:parseInt(p.element.css("width"),10),height:parseInt(p.element.css("height"),10),top:parseInt(p.element.css("top"),10),left:parseInt(p.element.css("left"),10)};if(h&&h.length){c(h[0]).css({width:o.width,height:o.height})}p._updateCache(o);p._propagate("resize",i)}})}});c.ui.plugin.add("resizable","containment",{start:function(f,r){var t=c(this).data("resizable"),j=t.options,l=t.element;var g=j.containment,k=(g instanceof c)?g.get(0):(/parent/.test(g))?l.parent().get(0):g;if(!k){return}t.containerElement=c(k);if(/document/.test(g)||g==document){t.containerOffset={left:0,top:0};t.containerPosition={left:0,top:0};t.parentData={element:c(document),left:0,top:0,width:c(document).width(),height:c(document).height()||document.body.parentNode.scrollHeight}}else{var n=c(k),i=[];c(["Top","Right","Left","Bottom"]).each(function(p,o){i[p]=b(n.css("padding"+o))});t.containerOffset=n.offset();t.containerPosition=n.position();t.containerSize={height:(n.innerHeight()-i[3]),width:(n.innerWidth()-i[1])};var q=t.containerOffset,e=t.containerSize.height,m=t.containerSize.width,h=(c.ui.hasScroll(k,"left")?k.scrollWidth:m),s=(c.ui.hasScroll(k)?k.scrollHeight:e);t.parentData={element:k,left:q.left,top:q.top,width:h,height:s}}},resize:function(g,q){var t=c(this).data("resizable"),i=t.options,f=t.containerSize,p=t.containerOffset,m=t.size,n=t.position,r=t._aspectRatio||g.shiftKey,e={top:0,left:0},h=t.containerElement;if(h[0]!=document&&(/static/).test(h.css("position"))){e=p}if(n.left<(t._helper?p.left:0)){t.size.width=t.size.width+(t._helper?(t.position.left-p.left):(t.position.left-e.left));if(r){t.size.height=t.size.width/i.aspectRatio}t.position.left=i.helper?p.left:0}if(n.top<(t._helper?p.top:0)){t.size.height=t.size.height+(t._helper?(t.position.top-p.top):t.position.top);if(r){t.size.width=t.size.height*i.aspectRatio}t.position.top=t._helper?p.top:0}t.offset.left=t.parentData.left+t.position.left;t.offset.top=t.parentData.top+t.position.top;var l=Math.abs((t._helper?t.offset.left-e.left:(t.offset.left-e.left))+t.sizeDiff.width),s=Math.abs((t._helper?t.offset.top-e.top:(t.offset.top-p.top))+t.sizeDiff.height);var k=t.containerElement.get(0)==t.element.parent().get(0),j=/relative|absolute/.test(t.containerElement.css("position"));if(k&&j){l-=t.parentData.left}if(l+t.size.width>=t.parentData.width){t.size.width=t.parentData.width-l;if(r){t.size.height=t.size.width/t.aspectRatio}}if(s+t.size.height>=t.parentData.height){t.size.height=t.parentData.height-s;if(r){t.size.width=t.size.height*t.aspectRatio}}},stop:function(f,n){var q=c(this).data("resizable"),g=q.options,l=q.position,m=q.containerOffset,e=q.containerPosition,i=q.containerElement;var j=c(q.helper),r=j.offset(),p=j.outerWidth()-q.sizeDiff.width,k=j.outerHeight()-q.sizeDiff.height;if(q._helper&&!g.animate&&(/relative/).test(i.css("position"))){c(this).css({left:r.left-e.left-m.left,width:p,height:k})}if(q._helper&&!g.animate&&(/static/).test(i.css("position"))){c(this).css({left:r.left-e.left-m.left,width:p,height:k})}}});c.ui.plugin.add("resizable","ghost",{start:function(g,h){var e=c(this).data("resizable"),i=e.options,f=e.size;e.ghost=e.originalElement.clone();e.ghost.css({opacity:0.25,display:"block",position:"relative",height:f.height,width:f.width,margin:0,left:0,top:0}).addClass("ui-resizable-ghost").addClass(typeof i.ghost=="string"?i.ghost:"");e.ghost.appendTo(e.helper)},resize:function(f,g){var e=c(this).data("resizable"),h=e.options;if(e.ghost){e.ghost.css({position:"relative",height:e.size.height,width:e.size.width})}},stop:function(f,g){var e=c(this).data("resizable"),h=e.options;if(e.ghost&&e.helper){e.helper.get(0).removeChild(e.ghost.get(0))}}});c.ui.plugin.add("resizable","grid",{resize:function(e,m){var p=c(this).data("resizable"),h=p.options,k=p.size,i=p.originalSize,j=p.originalPosition,n=p.axis,l=h._aspectRatio||e.shiftKey;h.grid=typeof h.grid=="number"?[h.grid,h.grid]:h.grid;var g=Math.round((k.width-i.width)/(h.grid[0]||1))*(h.grid[0]||1),f=Math.round((k.height-i.height)/(h.grid[1]||1))*(h.grid[1]||1);if(/^(se|s|e)$/.test(n)){p.size.width=i.width+g;p.size.height=i.height+f}else{if(/^(ne)$/.test(n)){p.size.width=i.width+g;p.size.height=i.height+f;p.position.top=j.top-f}else{if(/^(sw)$/.test(n)){p.size.width=i.width+g;p.size.height=i.height+f;p.position.left=j.left-g}else{p.size.width=i.width+g;p.size.height=i.height+f;p.position.top=j.top-f;p.position.left=j.left-g}}}}});var b=function(e){return parseInt(e,10)||0};var a=function(e){return !isNaN(parseInt(e,10))}})(jQuery);/*
+ * jQuery hashchange event - v1.3 - 7/21/2010
+ * http://benalman.com/projects/jquery-hashchange-plugin/
+ *
+ * Copyright (c) 2010 "Cowboy" Ben Alman
+ * Dual licensed under the MIT and GPL licenses.
+ * http://benalman.com/about/license/
+ */
+(function($,e,b){var c="hashchange",h=document,f,g=$.event.special,i=h.documentMode,d="on"+c in e&&(i===b||i>7);function a(j){j=j||location.href;return"#"+j.replace(/^[^#]*#?(.*)$/,"$1")}$.fn[c]=function(j){return j?this.bind(c,j):this.trigger(c)};$.fn[c].delay=50;g[c]=$.extend(g[c],{setup:function(){if(d){return false}$(f.start)},teardown:function(){if(d){return false}$(f.stop)}});f=(function(){var j={},p,m=a(),k=function(q){return q},l=k,o=k;j.start=function(){p||n()};j.stop=function(){p&&clearTimeout(p);p=b};function n(){var r=a(),q=o(m);if(r!==m){l(m=r,q);$(e).trigger(c)}else{if(q!==m){location.href=location.href.replace(/#.*/,"")+q}}p=setTimeout(n,$.fn[c].delay)}$.browser.msie&&!d&&(function(){var q,r;j.start=function(){if(!q){r=$.fn[c].src;r=r&&r+a();q=$('<iframe tabindex="-1" title="empty"/>').hide().one("load",function(){r||l(a());n()}).attr("src",r||"javascript:0").insertAfter("body")[0].contentWindow;h.onpropertychange=function(){try{if(event.propertyName==="title"){q.document.title=h.title}}catch(s){}}}};j.stop=k;o=function(){return a(q.location.href)};l=function(v,s){var u=q.document,t=$.fn[c].domain;if(v!==s){u.title=h.title;u.open();t&&u.write('<script>document.domain="'+t+'"<\/script>');u.close();q.location.hash=v}}})();return j})()})(jQuery,this);(function(c){var a=c.scrollTo=function(f,e,d){c(window).scrollTo(f,e,d)};a.defaults={axis:"xy",duration:parseFloat(c.fn.jquery)>=1.3?0:1};a.window=function(d){return c(window)._scrollable()};c.fn._scrollable=function(){return this.map(function(){var e=this,d=!e.nodeName||c.inArray(e.nodeName.toLowerCase(),["iframe","#document","html","body"])!=-1;if(!d){return e}var f=(e.contentWindow||e).document||e.ownerDocument||e;return c.browser.safari||f.compatMode=="BackCompat"?f.body:f.documentElement})};c.fn.scrollTo=function(f,e,d){if(typeof e=="object"){d=e;e=0}if(typeof d=="function"){d={onAfter:d}}if(f=="max"){f=9000000000}d=c.extend({},a.defaults,d);e=e||d.speed||d.duration;d.queue=d.queue&&d.axis.length>1;if(d.queue){e/=2}d.offset=b(d.offset);d.over=b(d.over);return this._scrollable().each(function(){var l=this,j=c(l),k=f,i,g={},m=j.is("html,body");switch(typeof k){case"number":case"string":if(/^([+-]=)?\d+(\.\d+)?(px|%)?$/.test(k)){k=b(k);break}k=c(k,this);case"object":if(k.is||k.style){i=(k=c(k)).offset()}}c.each(d.axis.split(""),function(q,r){var s=r=="x"?"Left":"Top",u=s.toLowerCase(),p="scroll"+s,o=l[p],n=a.max(l,r);if(i){g[p]=i[u]+(m?0:o-j.offset()[u]);if(d.margin){g[p]-=parseInt(k.css("margin"+s))||0;g[p]-=parseInt(k.css("border"+s+"Width"))||0}g[p]+=d.offset[u]||0;if(d.over[u]){g[p]+=k[r=="x"?"width":"height"]()*d.over[u]}}else{var t=k[u];g[p]=t.slice&&t.slice(-1)=="%"?parseFloat(t)/100*n:t}if(/^\d+$/.test(g[p])){g[p]=g[p]<=0?0:Math.min(g[p],n)}if(!q&&d.queue){if(o!=g[p]){h(d.onAfterFirst)}delete g[p]}});h(d.onAfter);function h(n){j.animate(g,e,d.easing,n&&function(){n.call(this,f,d)})}}).end()};a.max=function(j,i){var h=i=="x"?"Width":"Height",e="scroll"+h;if(!c(j).is("html,body")){return j[e]-c(j)[h.toLowerCase()]()}var g="client"+h,f=j.ownerDocument.documentElement,d=j.ownerDocument.body;return Math.max(f[e],d[e])-Math.min(f[g],d[g])};function b(d){return typeof d=="object"?d:{top:d,left:d}}})(jQuery);/*
+ PowerTip - v1.2.0 - 2013-04-03
+ http://stevenbenner.github.com/jquery-powertip/
+ Copyright (c) 2013 Steven Benner (http://stevenbenner.com/).
+ Released under MIT license.
+ https://raw.github.com/stevenbenner/jquery-powertip/master/LICENSE.txt
+*/
+(function(a){if(typeof define==="function"&&define.amd){define(["jquery"],a)}else{a(jQuery)}}(function(k){var A=k(document),s=k(window),w=k("body");var n="displayController",e="hasActiveHover",d="forcedOpen",u="hasMouseMove",f="mouseOnToPopup",g="originalTitle",y="powertip",o="powertipjq",l="powertiptarget",E=180/Math.PI;var c={isTipOpen:false,isFixedTipOpen:false,isClosing:false,tipOpenImminent:false,activeHover:null,currentX:0,currentY:0,previousX:0,previousY:0,desyncTimeout:null,mouseTrackingActive:false,delayInProgress:false,windowWidth:0,windowHeight:0,scrollTop:0,scrollLeft:0};var p={none:0,top:1,bottom:2,left:4,right:8};k.fn.powerTip=function(F,N){if(!this.length){return this}if(k.type(F)==="string"&&k.powerTip[F]){return k.powerTip[F].call(this,this,N)}var O=k.extend({},k.fn.powerTip.defaults,F),G=new x(O);h();this.each(function M(){var R=k(this),Q=R.data(y),P=R.data(o),T=R.data(l),S;if(R.data(n)){k.powerTip.destroy(R)}S=R.attr("title");if(!Q&&!T&&!P&&S){R.data(y,S);R.data(g,S);R.removeAttr("title")}R.data(n,new t(R,O,G))});if(!O.manual){this.on({"mouseenter.powertip":function J(P){k.powerTip.show(this,P)},"mouseleave.powertip":function L(){k.powerTip.hide(this)},"focus.powertip":function K(){k.powerTip.show(this)},"blur.powertip":function H(){k.powerTip.hide(this,true)},"keydown.powertip":function I(P){if(P.keyCode===27){k.powerTip.hide(this,true)}}})}return this};k.fn.powerTip.defaults={fadeInTime:200,fadeOutTime:100,followMouse:false,popupId:"powerTip",intentSensitivity:7,intentPollInterval:100,closeDelay:100,placement:"n",smartPlacement:false,offset:10,mouseOnToPopup:false,manual:false};k.fn.powerTip.smartPlacementLists={n:["n","ne","nw","s"],e:["e","ne","se","w","nw","sw","n","s","e"],s:["s","se","sw","n"],w:["w","nw","sw","e","ne","se","n","s","w"],nw:["nw","w","sw","n","s","se","nw"],ne:["ne","e","se","n","s","sw","ne"],sw:["sw","w","nw","s","n","ne","sw"],se:["se","e","ne","s","n","nw","se"],"nw-alt":["nw-alt","n","ne-alt","sw-alt","s","se-alt","w","e"],"ne-alt":["ne-alt","n","nw-alt","se-alt","s","sw-alt","e","w"],"sw-alt":["sw-alt","s","se-alt","nw-alt","n","ne-alt","w","e"],"se-alt":["se-alt","s","sw-alt","ne-alt","n","nw-alt","e","w"]};k.powerTip={show:function z(F,G){if(G){i(G);c.previousX=G.pageX;c.previousY=G.pageY;k(F).data(n).show()}else{k(F).first().data(n).show(true,true)}return F},reposition:function r(F){k(F).first().data(n).resetPosition();return F},hide:function D(G,F){if(G){k(G).first().data(n).hide(F)}else{if(c.activeHover){c.activeHover.data(n).hide(true)}}return G},destroy:function C(G){k(G).off(".powertip").each(function F(){var I=k(this),H=[g,n,e,d];if(I.data(g)){I.attr("title",I.data(g));H.push(y)}I.removeData(H)});return G}};k.powerTip.showTip=k.powerTip.show;k.powerTip.closeTip=k.powerTip.hide;function b(){var F=this;F.top="auto";F.left="auto";F.right="auto";F.bottom="auto";F.set=function(H,G){if(k.isNumeric(G)){F[H]=Math.round(G)}}}function t(K,N,F){var J=null;function L(P,Q){M();if(!K.data(e)){if(!P){c.tipOpenImminent=true;J=setTimeout(function O(){J=null;I()},N.intentPollInterval)}else{if(Q){K.data(d,true)}F.showTip(K)}}}function G(P){M();c.tipOpenImminent=false;if(K.data(e)){K.data(d,false);if(!P){c.delayInProgress=true;J=setTimeout(function O(){J=null;F.hideTip(K);c.delayInProgress=false},N.closeDelay)}else{F.hideTip(K)}}}function I(){var Q=Math.abs(c.previousX-c.currentX),O=Math.abs(c.previousY-c.currentY),P=Q+O;if(P<N.intentSensitivity){F.showTip(K)}else{c.previousX=c.currentX;c.previousY=c.currentY;L()}}function M(){J=clearTimeout(J);c.delayInProgress=false}function H(){F.resetPosition(K)}this.show=L;this.hide=G;this.cancel=M;this.resetPosition=H}function j(){function G(M,L,J,O,P){var K=L.split("-")[0],N=new b(),I;if(q(M)){I=H(M,K)}else{I=F(M,K)}switch(L){case"n":N.set("left",I.left-(J/2));N.set("bottom",c.windowHeight-I.top+P);break;case"e":N.set("left",I.left+P);N.set("top",I.top-(O/2));break;case"s":N.set("left",I.left-(J/2));N.set("top",I.top+P);break;case"w":N.set("top",I.top-(O/2));N.set("right",c.windowWidth-I.left+P);break;case"nw":N.set("bottom",c.windowHeight-I.top+P);N.set("right",c.windowWidth-I.left-20);break;case"nw-alt":N.set("left",I.left);N.set("bottom",c.windowHeight-I.top+P);break;case"ne":N.set("left",I.left-20);N.set("bottom",c.windowHeight-I.top+P);break;case"ne-alt":N.set("bottom",c.windowHeight-I.top+P);N.set("right",c.windowWidth-I.left);break;case"sw":N.set("top",I.top+P);N.set("right",c.windowWidth-I.left-20);break;case"sw-alt":N.set("left",I.left);N.set("top",I.top+P);break;case"se":N.set("left",I.left-20);N.set("top",I.top+P);break;case"se-alt":N.set("top",I.top+P);N.set("right",c.windowWidth-I.left);break}return N}function F(K,J){var O=K.offset(),N=K.outerWidth(),I=K.outerHeight(),M,L;switch(J){case"n":M=O.left+N/2;L=O.top;break;case"e":M=O.left+N;L=O.top+I/2;break;case"s":M=O.left+N/2;L=O.top+I;break;case"w":M=O.left;L=O.top+I/2;break;case"nw":M=O.left;L=O.top;break;case"ne":M=O.left+N;L=O.top;break;case"sw":M=O.left;L=O.top+I;break;case"se":M=O.left+N;L=O.top+I;break}return{top:L,left:M}}function H(O,K){var S=O.closest("svg")[0],N=O[0],W=S.createSVGPoint(),L=N.getBBox(),V=N.getScreenCTM(),M=L.width/2,Q=L.height/2,P=[],I=["nw","n","ne","e","se","s","sw","w"],U,X,R,T;function J(){P.push(W.matrixTransform(V))}W.x=L.x;W.y=L.y;J();W.x+=M;J();W.x+=M;J();W.y+=Q;J();W.y+=Q;J();W.x-=M;J();W.x-=M;J();W.y-=Q;J();if(P[0].y!==P[1].y||P[0].x!==P[7].x){X=Math.atan2(V.b,V.a)*E;R=Math.ceil(((X%360)-22.5)/45);if(R<1){R+=8}while(R--){I.push(I.shift())}}for(T=0;T<P.length;T++){if(I[T]===K){U=P[T];break}}return{top:U.y+c.scrollTop,left:U.x+c.scrollLeft}}this.compute=G}function x(Q){var P=new j(),O=k("#"+Q.popupId);if(O.length===0){O=k("<div/>",{id:Q.popupId});if(w.length===0){w=k("body")}w.append(O)}if(Q.followMouse){if(!O.data(u)){A.on("mousemove",M);s.on("scroll",M);O.data(u,true)}}if(Q.mouseOnToPopup){O.on({mouseenter:function L(){if(O.data(f)){if(c.activeHover){c.activeHover.data(n).cancel()}}},mouseleave:function N(){if(c.activeHover){c.activeHover.data(n).hide()}}})}function I(S){S.data(e,true);O.queue(function R(T){H(S);T()})}function H(S){var U;if(!S.data(e)){return}if(c.isTipOpen){if(!c.isClosing){K(c.activeHover)}O.delay(100).queue(function R(V){H(S);V()});return}S.trigger("powerTipPreRender");U=B(S);if(U){O.empty().append(U)}else{return}S.trigger("powerTipRender");c.activeHover=S;c.isTipOpen=true;O.data(f,Q.mouseOnToPopup);if(!Q.followMouse){G(S);c.isFixedTipOpen=true}else{M()}O.fadeIn(Q.fadeInTime,function T(){if(!c.desyncTimeout){c.desyncTimeout=setInterval(J,500)}S.trigger("powerTipOpen")})}function K(R){c.isClosing=true;c.activeHover=null;c.isTipOpen=false;c.desyncTimeout=clearInterval(c.desyncTimeout);R.data(e,false);R.data(d,false);O.fadeOut(Q.fadeOutTime,function S(){var T=new b();c.isClosing=false;c.isFixedTipOpen=false;O.removeClass();T.set("top",c.currentY+Q.offset);T.set("left",c.currentX+Q.offset);O.css(T);R.trigger("powerTipClose")})}function M(){if(!c.isFixedTipOpen&&(c.isTipOpen||(c.tipOpenImminent&&O.data(u)))){var R=O.outerWidth(),V=O.outerHeight(),U=new b(),S,T;U.set("top",c.currentY+Q.offset);U.set("left",c.currentX+Q.offset);S=m(U,R,V);if(S!==p.none){T=a(S);if(T===1){if(S===p.right){U.set("left",c.windowWidth-R)}else{if(S===p.bottom){U.set("top",c.scrollTop+c.windowHeight-V)}}}else{U.set("left",c.currentX-R-Q.offset);U.set("top",c.currentY-V-Q.offset)}}O.css(U)}}function G(S){var R,T;if(Q.smartPlacement){R=k.fn.powerTip.smartPlacementLists[Q.placement];k.each(R,function(U,W){var V=m(F(S,W),O.outerWidth(),O.outerHeight());T=W;if(V===p.none){return false}})}else{F(S,Q.placement);T=Q.placement}O.addClass(T)}function F(U,T){var R=0,S,W,V=new b();V.set("top",0);V.set("left",0);O.css(V);do{S=O.outerWidth();W=O.outerHeight();V=P.compute(U,T,S,W,Q.offset);O.css(V)}while(++R<=5&&(S!==O.outerWidth()||W!==O.outerHeight()));return V}function J(){var R=false;if(c.isTipOpen&&!c.isClosing&&!c.delayInProgress){if(c.activeHover.data(e)===false||c.activeHover.is(":disabled")){R=true}else{if(!v(c.activeHover)&&!c.activeHover.is(":focus")&&!c.activeHover.data(d)){if(O.data(f)){if(!v(O)){R=true}}else{R=true}}}if(R){K(c.activeHover)}}}this.showTip=I;this.hideTip=K;this.resetPosition=G}function q(F){return window.SVGElement&&F[0] instanceof SVGElement}function h(){if(!c.mouseTrackingActive){c.mouseTrackingActive=true;k(function H(){c.scrollLeft=s.scrollLeft();c.scrollTop=s.scrollTop();c.windowWidth=s.width();c.windowHeight=s.height()});A.on("mousemove",i);s.on({resize:function G(){c.windowWidth=s.width();c.windowHeight=s.height()},scroll:function F(){var I=s.scrollLeft(),J=s.scrollTop();if(I!==c.scrollLeft){c.currentX+=I-c.scrollLeft;c.scrollLeft=I}if(J!==c.scrollTop){c.currentY+=J-c.scrollTop;c.scrollTop=J}}})}}function i(F){c.currentX=F.pageX;c.currentY=F.pageY}function v(F){var H=F.offset(),J=F[0].getBoundingClientRect(),I=J.right-J.left,G=J.bottom-J.top;return c.currentX>=H.left&&c.currentX<=H.left+I&&c.currentY>=H.top&&c.currentY<=H.top+G}function B(I){var G=I.data(y),F=I.data(o),K=I.data(l),H,J;if(G){if(k.isFunction(G)){G=G.call(I[0])}J=G}else{if(F){if(k.isFunction(F)){F=F.call(I[0])}if(F.length>0){J=F.clone(true,true)}}else{if(K){H=k("#"+K);if(H.length>0){J=H.html()}}}}return J}function m(M,L,K){var G=c.scrollTop,J=c.scrollLeft,I=G+c.windowHeight,F=J+c.windowWidth,H=p.none;if(M.top<G||Math.abs(M.bottom-c.windowHeight)-K<G){H|=p.top}if(M.top+K>I||Math.abs(M.bottom-c.windowHeight)>I){H|=p.bottom}if(M.left<J||M.right+L>F){H|=p.left}if(M.left+L>F||M.right<J){H|=p.right}return H}function a(G){var F=0;while(G){G&=G-1;F++}return F}}));
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/modules.html b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/modules.html
new file mode 100644
index 0000000000000000000000000000000000000000..01a2cbd003f46a44861d96efcb0a5bcd61f9e2c6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/modules.html
@@ -0,0 +1,54 @@
+<!-- HTML header for doxygen 1.8.11-->
+<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
+<html xmlns="http://www.w3.org/1999/xhtml">
+<head>
+<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
+<meta http-equiv="X-UA-Compatible" content="IE=9"/>
+<meta name="generator" content="Doxygen 1.8.11"/>
+<title>i2cpwm_board: Modules</title>
+<link href="tabs.css" rel="stylesheet" type="text/css"/>
+<script type="text/javascript" src="jquery.js"></script>
+<script type="text/javascript" src="dynsections.js"></script>
+<link href="doxygen.css" rel="stylesheet" type="text/css" />
+<link href="styles.css" rel="stylesheet" type="text/css"/>
+</head>
+<body>
+<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
+<div id="titlearea">
+<table cellspacing="0" cellpadding="0">
+ <tbody>
+ <tr style="height: 56px;">
+ <td id="projectalign" style="padding-left: 0.5em;">
+ <a href="./index.html"><div id="projectname">i2cpwm_board
+  <span id="projectnumber">0.5.1</span>
+ </div></a>
+ <div id="projectbrief">ROS Package for PCA9685 based 16 channel PWM Board with I2C Interface</div>
+ </td>
+ </tr>
+ </tbody>
+</table>
+</div>
+<!-- end header part -->
+<!-- Generated by Doxygen 1.8.11 -->
+</div><!-- top -->
+<div class="header">
+ <div class="headertitle">
+<div class="title">Modules</div> </div>
+</div><!--header-->
+<div class="contents">
+<div class="textblock">Here is a list of all modules:</div><div class="directory">
+<table class="directory">
+<tr id="row_0_" class="even"><td class="entry"><span style="width:16px;display:inline-block;"> </span><a class="el" href="group___topics.html" target="_self">Topics with subscribers provided by this package</a></td><td class="desc"></td></tr>
+<tr id="row_1_"><td class="entry"><span style="width:16px;display:inline-block;"> </span><a class="el" href="group___services.html" target="_self">Services interfaces provided by this package</a></td><td class="desc"></td></tr>
+</table>
+</div><!-- directory -->
+</div><!-- contents -->
+<!-- HTML footer for doxygen 1.8.11-->
+<!-- start footer part -->
+<hr class="footer"/><address class="footer"><small>
+Generated by  <a href="http://www.doxygen.org/index.html">
+<img class="footer" src="doxygen.png" alt="doxygen"/>
+</a> 1.8.11
+</small></address>
+</body>
+</html>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_f.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_f.png
new file mode 100644
index 0000000000000000000000000000000000000000..72a58a529ed3a9ed6aa0c51a79cf207e026deee2
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_f.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_g.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_g.png
new file mode 100644
index 0000000000000000000000000000000000000000..2093a237a94f6c83e19ec6e5fd42f7ddabdafa81
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_g.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_h.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_h.png
new file mode 100644
index 0000000000000000000000000000000000000000..33389b101d9cd9b4c98ad286b5d9c46a6671f650
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_h.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/open.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/open.png
new file mode 100644
index 0000000000000000000000000000000000000000..30f75c7efe2dd0c9e956e35b69777a02751f048b
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/open.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/splitbar.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/splitbar.png
new file mode 100644
index 0000000000000000000000000000000000000000..fe895f2c58179b471a22d8320b39a4bd7312ec8e
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/splitbar.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/styles.css b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/styles.css
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_off.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_off.png
new file mode 100644
index 0000000000000000000000000000000000000000..3b443fc62892114406e3d399421b2a881b897acc
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_off.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_on.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_on.png
new file mode 100644
index 0000000000000000000000000000000000000000..e08320fb64e6fa33b573005ed6d8fe294e19db76
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_on.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_a.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_a.png
new file mode 100644
index 0000000000000000000000000000000000000000..3b725c41c5a527a3a3e40097077d0e206a681247
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_a.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_b.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_b.png
new file mode 100644
index 0000000000000000000000000000000000000000..e2b4a8638cb3496a016eaed9e16ffc12846dea18
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_b.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_h.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_h.png
new file mode 100644
index 0000000000000000000000000000000000000000..fd5cb705488e60fcf30f56fcc951dee74f3b095b
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_h.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_s.png b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_s.png
new file mode 100644
index 0000000000000000000000000000000000000000..ab478c95b67371d700a20869f7de1ddd73522d50
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_s.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tabs.css b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tabs.css
new file mode 100644
index 0000000000000000000000000000000000000000..9cf578f23a154ff026365d61ea59013ad431466b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tabs.css
@@ -0,0 +1,60 @@
+.tabs, .tabs2, .tabs3 {
+ background-image: url('tab_b.png');
+ width: 100%;
+ z-index: 101;
+ font-size: 13px;
+ font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif;
+}
+
+.tabs2 {
+ font-size: 10px;
+}
+.tabs3 {
+ font-size: 9px;
+}
+
+.tablist {
+ margin: 0;
+ padding: 0;
+ display: table;
+}
+
+.tablist li {
+ float: left;
+ display: table-cell;
+ background-image: url('tab_b.png');
+ line-height: 36px;
+ list-style: none;
+}
+
+.tablist a {
+ display: block;
+ padding: 0 20px;
+ font-weight: bold;
+ background-image:url('tab_s.png');
+ background-repeat:no-repeat;
+ background-position:right;
+ color: #283A5D;
+ text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9);
+ text-decoration: none;
+ outline: none;
+}
+
+.tabs3 .tablist a {
+ padding: 0 10px;
+}
+
+.tablist a:hover {
+ background-image: url('tab_h.png');
+ background-repeat:repeat-x;
+ color: #fff;
+ text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0);
+ text-decoration: none;
+}
+
+.tablist li.current a {
+ background-image: url('tab_a.png');
+ background-repeat:repeat-x;
+ color: #fff;
+ text-shadow: 0px 1px 1px rgba(0, 0, 0, 1.0);
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/styles.css b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/styles.css
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/launch/i2cpwm_node.launch b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/launch/i2cpwm_node.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9741c381abbc7c71c4dd17f56ba71cec401af180
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/launch/i2cpwm_node.launch
@@ -0,0 +1,4 @@
+<launch>
+ <node pkg="i2cpwm_board" name="i2cpwm_board_node" type="i2cpwm_board" output="screen" launch-prefix="sudo -E ">
+ </node>
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Position.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Position.msg
new file mode 100644
index 0000000000000000000000000000000000000000..8519c223517c69fce55e16234b40743123678ee2
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Position.msg
@@ -0,0 +1,13 @@
+# the position message is used when configuring drive mode to
+# assign a servo to a specific wheel positon in the drive system
+# postions are specific toe the desired drive mode
+# ackerman has only one position
+# 1 = drive
+# differential has two positons
+# 1 = left, 2 = right
+# mecanum has four positions
+# 1 = front left, 2 = front right, 3 = rear left, 4 = rear right
+# multiple servos/motors may be used for each positon
+
+int16 servo
+int16 position
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/PositionArray.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/PositionArray.msg
new file mode 100644
index 0000000000000000000000000000000000000000..c9a45f3adac0b9292adc8fb2fec3e060fe86af36
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/PositionArray.msg
@@ -0,0 +1,4 @@
+# the PositionArray message handles multiple position assignments
+# of servos.
+
+Position[] servos
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Servo.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Servo.msg
new file mode 100644
index 0000000000000000000000000000000000000000..46f0bfda44a8a3495662ac692b90087fb5028be4
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Servo.msg
@@ -0,0 +1,6 @@
+# the Servo message is commonly used message in this package to
+# assign a value to a specific servo. the purpose of the value is
+# dependent on the topic or service being called
+
+int16 servo
+float32 value
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoArray.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoArray.msg
new file mode 100644
index 0000000000000000000000000000000000000000..b44c72f4e32c1c8a1a4f4c490a142b0a53465c2d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoArray.msg
@@ -0,0 +1,5 @@
+# the ServoArray message is commonly used message in this package to
+# handle multiple assignments of values to servos. the purpose of
+# the value is dependent on the topic or service being called
+
+Servo[] servos
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfig.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfig.msg
new file mode 100644
index 0000000000000000000000000000000000000000..59febe23bd0eaafcf1f95cf710eb6a35b3a3fbc4
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfig.msg
@@ -0,0 +1,9 @@
+# the ServoConfig message is used to assign specific configuration
+# data to a servo. the data is needed to normalize servos to
+# common values to isolate variations from the rest of the user's
+# robot motion code.
+
+int16 servo
+int16 center
+int16 range
+int16 direction
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfigArray.msg b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfigArray.msg
new file mode 100644
index 0000000000000000000000000000000000000000..8ac383368b3943271d3be25501335ab6b3dd6784
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfigArray.msg
@@ -0,0 +1,6 @@
+# the ServoConfigArray message is used to handle multiple assignments
+# of config data to servos. the data is needed to normalize servos to
+# common values to isolate variations from the rest of the user's
+# robot motion code.
+
+ServoConfig[] servos
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..076d0a50935ce0f89cb89b343fcfe26fdf4018a6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package>
+ <name>i2cpwm_board</name>
+ <version>0.5.1</version>
+ <description>
+ Controller for the Adafruit 16 channel PWM servo I2C board, hat, and similar PCA9685 based boards.
+ Provides absolute proportional PWM control as well as supports geometry_msgs/Twist.
+ </description>
+
+ <author>Bradan Lane Studio</author>
+ <maintainer email="locoro@bradanlane.com">bradanlane</maintainer>
+ <license>GPLv3</license>
+ <url>https://gitlab.com/bradanlane/ros-i2cpwmboard</url>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>message_generation</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>std_srvs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>libi2c-dev</build_depend>
+
+ <run_depend>message_runtime</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>std_srvs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>libi2c-dev</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/src/i2cpwm_controller.cpp b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/src/i2cpwm_controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5723020cd70dc5e78d658dfa902543c86c79e02
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/src/i2cpwm_controller.cpp
@@ -0,0 +1,1617 @@
+/**
+ *
+ \file
+ \brief controller for I2C interfaced 16 channel PWM boards with PCA9685 chip
+ \author Bradan Lane Studio <info@bradanlane.com>
+ \copyright Copyright (c) 2016, Bradan Lane Studio
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ - Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+ - The name of Bradan Lane, Bradan Lane Studio nor the
+ names of its contributors may be used to endorse or promote products
+ derived from this software without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ DISCLAIMED. IN NO EVENT SHALL BRADAN LANE STUDIOS BE LIABLE FOR ANY
+ DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ Please send comments, questions, or patches to info@bradanlane.com
+
+*/
+
+/**
+\mainpage
+
+ Controller for I2C interfaced 16 channel PWM boards with PCA9685 chip<br/>
+ Bradan Lane Studio <info@bradanlane.com><br/>
+ Copyright (c) 2016, Bradan Lane Studio<br/>
+ Licensed under GPLv3<br/>
+
+--------
+
+
+\section overview FILE STRUCTURE AND CODE
+
+ The file is broken into sections:
+ - private properties (all private properties have a leading underscore)
+ - private methods (all private methods have a leading underscore)
+ - public topic subscribers:
+ - servos_absolute()
+ - servos_proportional()
+ - servos_drive()
+ - public services:
+ - set_pwm_frequency()
+ - set_active_board()
+ - config_servos()
+ - config_drive_mode()
+ - stop_servos()
+
+ The code is currently authored in C and should be rewritten as proper C++.
+
+ This documentation refers to 'servo' and 'RC servo' but is equally applicable to any PWM or PPM controlled DC motor.
+
+ All published services and topics use a one-based count syntax. For example, the first servo is '1' and the default board is '1'.
+ The hardware uses zero-based values. For example the first channel on the 16 channel 12-bit PWM board is '0' and the first I2C board is '0' with address 0x40.
+ The switch from one-based to zero-based is done at the lowest level of this code. All public interactions should assume one-based values.
+
+ _WARNING_: The code has only been tested with a single board using the default I2C address of 0x40. Once testing has been done with additional configurations, this warning will be removed or amended.
+
+
+\section pwmservos USING PWM WITH SERVOS
+
+ While the PCA9685 chip and related boards are called "PWM" for pulse width modulation, there use with servos
+ is more accurately PPM for pulse position modulation.
+
+ For standard 180 degree servos with a motion arc of ±90 degrees the pulse moves the servo arm to specifc position and holds it.
+ For continuous motion servos, the pulse moves the servo at a specific speed.
+
+ The documentation will refer to positon, speed or position/speed.
+ These are interchangeable as the two terms are dependent on whether the servo is a fixed rotation servo or a continuous rotation servo and independent of the board itself.
+
+ Analog RC servos are most often designed for 20ms pulses. This is achieved with a frequency of 50Hz.
+ This software defaults to 50Hz. Use the set_pwm_frequncy() to change this frequency value.
+ It may be necessary or convenient to change the PWM frequency if using DC motors connected to PWM controllers.
+ It may also be convenient if using PWM to control LEDs.
+
+ A commonly available board is the Adafruit 16 channel 12-bit PWM board or the similarly named HAT. There are similar boards as well as clones.
+ All of these boards use the PCA9685 chip. Not all boards have been tested.
+
+ The PWM boards drive LED and servos using pulse width modulation. The 12 bit interface means values are in the range of 0..4096.
+ The pulse is defined as a start value and end value. When driving servos, the start point is typically 0 and the end point is the duration.
+
+ FYI: If using more than one PWM board or when using a board with an I2C address other than the default 0x40,
+ the set_active_board() service must be used before using other services or topics from this package.
+
+
+\section configuration CONFIGURING SERVOS
+
+ The tolerance of the resistors in RC servos means each servo may have a slightly different center point.
+
+ The servos_absolute() topic controls servos with absolute pulse start and stop values.
+ This topic subscriber is not generally useful in robot operations, however it is convenient for testing and for determining configuration values.
+ Use this topic to determine the center position for a standard servo or the stop position for a continuous servo.
+
+ Also use this topic to determine the range of each servo -
+ either the ±90 postion for a standard servo or
+ the max forward and reverse speed for a continuous rotation servo.
+
+ __note:__ The centering value and range of servos is dependent on the pulse frequency.
+ If you use set_pwm_frequency() to change the system value, you will need to determine new center and range values.
+
+ The servos_proportional() topic controls servos through their range of motion using values between -1.0 and 1.0.
+ Use the config_servos() service to set the center values, range values, and directions of rotation for servos.
+ This enables servo motion to be generalized to a standard proportion of ±1.0.
+ Use of the config_servos() service is required before proportional control is available.
+
+
+\section drivemode ROBOT DRIVE MODE
+
+ In addition to absolute and proportional topics for controling servos, this package provides support for the geometry 'Twist' message.
+ The servos_drive() topic handles the calculations to convert linear and angular data to servo drive data.
+
+ Drive mode requires details for each servo. Use of the config_servos() before using drive mode.
+
+ The geometry_msgs::Twist providesf linear and angular velocity measured in m/s (meters per second) and r/s (radians per second) respectively.
+ To perform the necessary calculations, data about the drive system is required.
+ Use config_drive_mode() to provide the RPM of the drive wheels, their radius, and the track distance between left and right. These values determine speed and turn rates.
+
+ Specifiy the desired drive mode with the `mode` property to cofig_drive_mode(). This package supports three drive modes:
+
+ -# __ackerman__ - A single velocity drive using one or more servos with a non-drive servo controlling steering.
+ -# __differential__ - Skid steer or track steer using one or more servos for each of the left and right sides.
+ The result is full speed forward or reverse, min/max radius turns, and the ability to perform zero-radius turns
+ -# __mecanum__ - Independent drive on all four wheel capable of holonomic drive - moving in any combination of forward, reverse, turning, and sideways.
+ The servos are assigned positons for left-front, right-front, left-rear, and right-rear wheels. This mode supports
+ similar drive characteristics to differential drive with the additional of lateral motion.
+
+
+ The servos on the PWM board are assigned to their respective drive positons using the config_drive_mode() service.
+ The applicable servos are assigned positions as follows:
+
+ positon | ackerman | differential | mecanum
+ --------|----------|--------------|--------
+ position 1 corresponds to | drive | left | left-front
+ position 2 corresponds to | | right | right-front
+ position 3 corresponds to | | | left-rear
+ position 4 corresponds to | | | right-rear
+
+ The drive topic requires that use of both the config_servos() service and config_drive_mode() service before drive mode will result in expected behavior.
+
+ All non-drive servos may still be operated with the proportion or absolute topics.
+
+ __Note:__ _This controller does not implement encoders for PWM motors. There is no guarantee that the drive velocity will exactly match the Twist message input.
+ While this may not be acceptable for a commercial or scientific application, it may not be of convern for education and amatuer competitions. Encoders could be added and feedback applied to the PWM values.
+ Additionally, when drive control is a product of positional feedback - such as line following and navigation via camera vision - drive encoders are usually not required._
+
+ The stop_servos() service is provided as convenience to stop all servos and place then is a powered off state. This is different from setting each servo to its center value.
+ The stop service is useful as a safety operation.
+
+\section testing TESTING
+
+ Basic testing is available from the command line. Start the I2C PWM node with `roslaunch i2cpwm_board i2cpwm_node.launch` (or `roscore` and `rosrun i2cpwm_board i2cpwm_board`) and then proceed with
+ example commands contained within the documentation for each service and topic subscriber.
+
+ */
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <linux/i2c-dev.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+// messages used for any service with no parameters
+#include <std_srvs/Empty.h>
+// messages used for drive movement topic
+#include <geometry_msgs/Twist.h>
+
+// messages used for the absolute and proportional movement topics
+#include "i2cpwm_board/Servo.h"
+#include "i2cpwm_board/ServoArray.h"
+// messages used for the servo setup service
+#include "i2cpwm_board/ServoConfig.h"
+#include "i2cpwm_board/ServoConfigArray.h"
+// request/response of the servo setup service
+#include "i2cpwm_board/ServosConfig.h"
+// request/response of the drive mode service
+#include "i2cpwm_board/DriveMode.h"
+#include "i2cpwm_board/Position.h"
+#include "i2cpwm_board/PositionArray.h"
+// request/response of the integer parameter services
+#include "i2cpwm_board/IntValue.h"
+
+extern "C" {
+ #include <linux/i2c.h>
+ #include <linux/i2c-dev.h>
+ #include <i2c/smbus.h>
+}
+
+
+/// @cond PRIVATE_NO_PUBLIC DOC
+
+typedef struct _servo_config {
+ int center;
+ int range;
+ int direction;
+ int mode_pos;
+} servo_config;
+
+typedef struct _drive_mode {
+ int mode;
+ float rpm;
+ float radius;
+ float track;
+ float scale;
+} drive_mode;
+
+enum drive_modes {
+ MODE_UNDEFINED = 0,
+ MODE_ACKERMAN = 1,
+ MODE_DIFFERENTIAL = 2,
+ MODE_MECANUM = 3,
+ MODE_INVALID = 4
+};
+
+enum drive_mode_positions {
+ POSITION_UNDEFINED = 0,
+ POSITION_LEFTFRONT = 1,
+ POSITION_RIGHTFRONT = 2,
+ POSITION_LEFTREAR = 3,
+ POSITION_RIGHTREAR = 4,
+ POSITION_INVALID = 5
+};
+
+#define _BASE_ADDR 0x40
+#ifndef _PI
+#define _PI 3.14159265358979323846
+#endif
+#define _CONST(s) ((char*)(s))
+
+enum pwm_regs {
+ // Registers/etc.
+ __MODE1 = 0x00,
+ __MODE2 = 0x01,
+ __SUBADR1 = 0x02, // enable sub address 1 support
+ __SUBADR2 = 0x03, // enable sub address 2 support
+ __SUBADR3 = 0x04, // enable sub address 2 support
+ __PRESCALE = 0xFE,
+ __CHANNEL_ON_L = 0x06,
+ __CHANNEL_ON_H = 0x07,
+ __CHANNEL_OFF_L = 0x08,
+ __CHANNEL_OFF_H = 0x09,
+ __ALL_CHANNELS_ON_L = 0xFA,
+ __ALL_CHANNELS_ON_H = 0xFB,
+ __ALL_CHANNELS_OFF_L = 0xFC,
+ __ALL_CHANNELS_OFF_H = 0xFD,
+ __RESTART = 0x80,
+ __SLEEP = 0x10, // enable low power mode
+ __ALLCALL = 0x01,
+ __INVRT = 0x10, // invert the output control logic
+ __OUTDRV = 0x04
+};
+
+#define MAX_BOARDS 62
+#define MAX_SERVOS (16*MAX_BOARDS)
+
+servo_config _servo_configs[MAX_SERVOS]; // we can support up to 62 boards (1..62), each with 16 PWM devices (1..16)
+drive_mode _active_drive; // used when converting Twist geometry to PWM values and which servos are for motion
+int _last_servo = -1;
+
+int _pwm_boards[MAX_BOARDS]; // we can support up to 62 boards (1..62)
+int _active_board = 0; // used to determine if I2C SLAVE change is needed
+int _controller_io_handle; // linux file handle for I2C
+int _controller_io_device; // linux file for I2C
+
+int _pwm_frequency = 50; // frequency determines the size of a pulse width; higher numbers make RC servos buzz
+
+
+/// @endcond PRIVATE_NO_PUBLIC DOC
+
+
+
+//* ------------------------------------------------------------------------------------------------------------------------------------
+// local private methods
+//* ------------------------------------------------------------------------------------------------------------------------------------
+
+static float _abs (float v1)
+{
+ if (v1 < 0)
+ return (0 - v1);
+ return v1;
+}
+
+static float _min (float v1, float v2)
+{
+ if (v1 > v2)
+ return v2;
+ return v1;
+}
+
+static float _max (float v1, float v2)
+{
+ if (v1 < v2)
+ return v2;
+ return v1;
+}
+
+static float _absmin (float v1, float v2)
+{
+ float a1, a2;
+ float sign = 1.0;
+ // if (v1 < 0)
+ // sign = -1.0;
+ a1 = _abs(v1);
+ a2 = _abs(v2);
+ if (a1 > a2)
+ return (sign * a2);
+ return v1;
+}
+
+static float _absmax (float v1, float v2)
+{
+ float a1, a2;
+ float sign = 1.0;
+ // if (v1 < 0)
+ // sign = -1.0;
+ a1 = _abs(v1);
+ a2 = _abs(v2);
+ if (a1 < a2)
+ return (sign * a2);
+ return v1;
+}
+
+
+
+/**
+ \private method to smooth a speed value
+
+ we calculate each speed using a cosine 'curve', this results in the output curve
+ being shallow at 'stop', full forward, and full reverse and becoming
+ more aggressive in the middle or each direction
+
+ @param speed an int value (±1.0) indicating original speed
+ @returns an integer value (±1.0) smoothed for more gentle acceleration
+ */
+static int _smoothing (float speed)
+{
+ /* if smoothing is desired, then remove the commented code */
+ // speed = (cos(_PI*(((float)1.0 - speed))) + 1) / 2;
+ return speed;
+}
+
+
+/**
+ \private method to convert meters per second to a proportional value in the range of ±1.0
+
+ @param speed float requested speed in meters per second
+ @returns float value (±1.0) for servo speed
+ */
+static float _convert_mps_to_proportional (float speed)
+{
+ /* we use the drive mouter output rpm and wheel radius to compute the conversion */
+
+ float initial, max_rate; // the max m/s is ((rpm/60) * (2*PI*radius))
+
+ initial = speed;
+
+ if (_active_drive.rpm <= 0.0) {
+ ROS_ERROR("Invalid active drive mode RPM %6.4f :: RPM must be greater than 0", _active_drive.rpm);
+ return 0.0;
+ }
+ if (_active_drive.radius <= 0.0) {
+ ROS_ERROR("Invalid active drive mode radius %6.4f :: wheel radius must be greater than 0", _active_drive.radius);
+ return 0.0;
+ }
+
+ max_rate = (_active_drive.radius * _PI * 2) * (_active_drive.rpm / 60.0);
+
+ speed = speed / max_rate;
+ // speed = _absmin (speed, 1.0);
+
+ ROS_DEBUG("%6.4f = convert_mps_to_proportional ( speed(%6.4f) / ((radus(%6.4f) * pi(%6.4f) * 2) * (rpm(%6.4f) / 60.0)) )", speed, initial, _active_drive.radius, _PI, _active_drive.rpm);
+ return speed;
+}
+
+
+
+/**
+ * \private method to set a pulse frequency
+ *
+ *The pulse defined by start/stop will be active on all channels until any subsequent call changes it.
+ *@param frequency an int value (1..15000) indicating the pulse frequency where 50 is typical for RC servos
+ *Example _set_frequency (68) // set the pulse frequency to 68Hz
+ */
+static void _set_pwm_frequency (int freq)
+{
+ int prescale;
+ char oldmode, newmode;
+ int res;
+
+ _pwm_frequency = freq; // save to global
+
+ ROS_DEBUG("_set_pwm_frequency prescale");
+ float prescaleval = 25000000.0; // 25MHz
+ prescaleval /= 4096.0;
+ prescaleval /= (float)freq;
+ prescaleval -= 1.0;
+ //ROS_INFO("Estimated pre-scale: %6.4f", prescaleval);
+ prescale = floor(prescaleval + 0.5);
+ // ROS_INFO("Final pre-scale: %d", prescale);
+
+
+ ROS_INFO("Setting PWM frequency to %d Hz", freq);
+
+ nanosleep ((const struct timespec[]){{1, 000000L}}, NULL);
+
+
+ oldmode = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
+ newmode = (oldmode & 0x7F) | 0x10; // sleep
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, newmode)) // go to sleep
+ ROS_ERROR("Unable to set PWM controller to sleep mode");
+
+ if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __PRESCALE, (int)(floor(prescale))))
+ ROS_ERROR("Unable to set PWM controller prescale");
+
+ if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode))
+ ROS_ERROR("Unable to set PWM controller to active mode");
+
+ nanosleep((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec,
+
+ if (0 > i2c_smbus_write_byte_data(_controller_io_handle, __MODE1, oldmode | 0x80))
+ ROS_ERROR("Unable to restore PWM controller to active mode");
+}
+
+
+
+/**
+ * \private method to set a common value for all PWM channels on the active board
+ *
+ *The pulse defined by start/stop will be active on all channels until any subsequent call changes it.
+ *@param start an int value (0..4096) indicating when the pulse will go high sending power to each channel.
+ *@param end an int value (0..4096) indicating when the pulse will go low stoping power to each channel.
+ *Example _set_pwm_interval_all (0, 108) // set all servos with a pulse width of 105
+ */
+static void _set_pwm_interval_all (int start, int end)
+{
+ // the public API is ONE based and hardware is ZERO based
+ if ((_active_board<1) || (_active_board>62)) {
+ ROS_ERROR("Internal error - invalid active board number %d :: PWM board numbers must be between 1 and 62", _active_board);
+ return;
+ }
+ int board = _active_board - 1;
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_L, start & 0xFF))
+ ROS_ERROR ("Error setting PWM start low byte for all servos on board %d", _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_ON_H, start >> 8))
+ ROS_ERROR ("Error setting PWM start high byte for all servos on board %d", _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_L, end & 0xFF))
+ ROS_ERROR ("Error setting PWM end low byte for all servos on board %d", _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __ALL_CHANNELS_OFF_H, end >> 8))
+ ROS_ERROR ("Error setting PWM end high byte for all servos on board %d", _active_board);
+}
+
+
+
+/**
+ * \private method to set the active board
+ *
+ *@param board an int value (1..62) indicating which board to activate for subsequent service and topic subscription activity where 1 coresponds to the default board address of 0x40 and value increment up
+ *Example _set_active_board (68) // set the pulse frequency to 68Hz
+ */
+static void _set_active_board (int board)
+{
+ char mode1res;
+
+ if ((board<1) || (board>62)) {
+ ROS_ERROR("Internal error :: invalid board number %d :: board numbers must be between 1 and 62", board);
+ return;
+ }
+ if (_active_board != board) {
+ _active_board = board; // save to global
+
+ // the public API is ONE based and hardware is ZERO based
+ board--;
+
+ if (0 > ioctl (_controller_io_handle, I2C_SLAVE, (_BASE_ADDR+(board)))) {
+ ROS_FATAL ("Failed to acquire bus access and/or talk to I2C slave at address 0x%02X", (_BASE_ADDR+board));
+ return; /* exit(1) */ /* additional ERROR HANDLING information is available with 'errno' */
+ }
+
+ if (_pwm_boards[board]<0) {
+ _pwm_boards[board] = 1;
+
+ /* this is guess but I believe the following needs to be done on each board only once */
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE2, __OUTDRV))
+ ROS_ERROR ("Failed to enable PWM outputs for totem-pole structure");
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, __ALLCALL))
+ ROS_ERROR ("Failed to enable ALLCALL for PWM channels");
+
+ nanosleep ((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec, wait for osci
+
+
+ mode1res = i2c_smbus_read_byte_data (_controller_io_handle, __MODE1);
+ mode1res = mode1res & ~__SLEEP; // # wake up (reset sleep)
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __MODE1, mode1res))
+ ROS_ERROR ("Failed to recover from low power mode");
+
+ nanosleep((const struct timespec[]){{0, 5000000L}}, NULL); //sleep 5microsec, wait for osci
+
+ // the first time we activate a board, we mark it and set all of its servo channels to 0
+ _set_pwm_interval_all (0, 0);
+ }
+ }
+}
+
+
+
+/**
+ * \private method to set a value for a PWM channel on the active board
+ *
+ *The pulse defined by start/stop will be active on the specified servo channel until any subsequent call changes it.
+ *@param servo an int value (1..16) indicating which channel to change power
+ *@param start an int value (0..4096) indicating when the pulse will go high sending power to each channel.
+ *@param end an int value (0..4096) indicating when the pulse will go low stoping power to each channel.
+ *Example _set_pwm_interval (3, 0, 350) // set servo #3 (fourth position on the hardware board) with a pulse of 350
+ */
+extern "C" {
+ #include <linux/i2c.h>
+ #include <linux/i2c-dev.h>
+ #include <i2c/smbus.h>
+}
+static void _set_pwm_interval (int servo, int start, int end)
+{
+ ROS_DEBUG("_set_pwm_interval enter");
+
+ if ((servo<1) || (servo>(MAX_SERVOS))) {
+ ROS_ERROR("Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_BOARDS);
+ return;
+ }
+
+ int board = ((int)((servo-1)/16))+1; // servo 1..16 is board #1, servo 17..32 is board #2, etc.
+ _set_active_board(board);
+
+ servo = ((servo-1) % 16) + 1; // servo numbers are 1..16
+
+
+ // the public API is ONE based and hardware is ZERO based
+ board = _active_board - 1; // the hardware enumerates boards as 0..61
+ int channel = servo - 1; // the hardware enumerates servos as 0..15
+ ROS_DEBUG("_set_pwm_interval board=%d servo=%d", board, servo);
+
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_L+4*channel, start & 0xFF))
+ ROS_ERROR ("Error setting PWM start low byte on servo %d on board %d", servo, _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_ON_H+4*channel, start >> 8))
+ ROS_ERROR ("Error setting PWM start high byte on servo %d on board %d", servo, _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_L+4*channel, end & 0xFF))
+ ROS_ERROR ("Error setting PWM end low byte on servo %d on board %d", servo, _active_board);
+ if (0 > i2c_smbus_write_byte_data (_controller_io_handle, __CHANNEL_OFF_H+4*channel, end >> 8))
+ ROS_ERROR ("Error setting PWM end high byte on servo %d on board %d", servo, _active_board);
+}
+
+
+
+/**
+ * \private method to set a value for a PWM channel, based on a range of ±1.0, on the active board
+ *
+ *The pulse defined by start/stop will be active on the specified servo channel until any subsequent call changes it.
+ *@param servo an int value (1..16) indicating which channel to change power
+ *@param value an int value (±1.0) indicating when the size of the pulse for the channel.
+ *Example _set_pwm_interval (3, 0, 350) // set servo #3 (fourth position on the hardware board) with a pulse of 350
+ */
+static void _set_pwm_interval_proportional (int servo, float value)
+{
+ // need a little wiggle room to allow for accuracy of a floating point value
+ if ((value < -1.0001) || (value > 1.0001)) {
+ ROS_ERROR("Invalid proportion value %f :: proportion values must be between -1.0 and 1.0", value);
+ return;
+ }
+
+ servo_config* configp = &(_servo_configs[servo-1]);
+
+ if ((configp->center < 0) ||(configp->range < 0)) {
+ ROS_ERROR("Missing servo configuration for servo[%d]", servo);
+ return;
+ }
+
+ int pos = (configp->direction * (((float)(configp->range) / 2) * value)) + configp->center;
+
+ if ((pos < 0) || (pos > 4096)) {
+ ROS_ERROR("Invalid computed position servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d", servo, configp->direction, configp->range, value, configp->center, pos);
+ return;
+ }
+ _set_pwm_interval (servo, 0, pos);
+ ROS_DEBUG("servo[%d] = (direction(%d) * ((range(%d) / 2) * value(%6.4f))) + %d = %d", servo, configp->direction, configp->range, value, configp->center, pos);
+}
+
+
+
+/**
+ * \private method to configure a servo on the active board
+ *
+ *@param servo an int value (1..16)
+ *@param center an int value gt 1
+ *@param range int value gt 1
+ *@param direction an int either -1 or 1
+ *Example _config_server (1, 300, 100, -1) // configure the first servo with a center of 300 and range of 100 and reversed direction
+ */
+static void _config_servo (int servo, int center, int range, int direction)
+{
+ if ((servo < 1) || (servo > (MAX_SERVOS))) {
+ ROS_ERROR("Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_SERVOS);
+ return;
+ }
+
+ if ((center < 0) || (center > 4096))
+ ROS_ERROR("Invalid center value %d :: center values must be between 0 and 4096", center);
+
+ if ((center < 0) || (center > 4096))
+ ROS_ERROR("Invalid range value %d :: range values must be between 0 and 4096", range);
+
+ if (((center - (range/2)) < 0) || (((range/2) + center) > 4096))
+ ROS_ERROR("Invalid range center combination %d ± %d :: range/2 ± center must be between 0 and 4096", center, (range/2));
+
+ _servo_configs[servo-1].center = center;
+ _servo_configs[servo-1].range = range;
+ _servo_configs[servo-1].direction = direction;
+ // _servo_configs[servo-1].mode_pos = POSITION_UNDEFINED;
+
+ if (servo > _last_servo) // used for internal optimizations
+ _last_servo = servo;
+
+ ROS_INFO("Servo #%d configured: center=%d, range=%d, direction=%d", servo, center, range, direction);
+}
+
+
+static int _config_servo_position (int servo, int position)
+{
+ if ((servo < 1) || (servo > (MAX_SERVOS))) {
+ ROS_ERROR("Invalid servo number %d :: servo numbers must be between 1 and %d", servo, MAX_SERVOS);
+ return -1;
+ }
+ if ((position < POSITION_UNDEFINED) || (position > POSITION_RIGHTREAR)) {
+ ROS_ERROR("Invalid drive mode position %d :: positions are 0 = non-drive, 1 = left front, 2 = right front, 3 = left rear, and 4 = right rear", position);
+ return -1;
+ }
+ _servo_configs[servo-1].mode_pos = position;
+ ROS_INFO("Servo #%d configured: position=%d", servo, position);
+ return 0;
+}
+
+
+static int _config_drive_mode (std::string mode, float rpm, float radius, float track, float scale)
+{
+ int mode_val = MODE_UNDEFINED;
+
+ // assumes the parameter was provided in the proper case
+ if (0 == strcmp (mode.c_str(), _CONST("ackerman")))
+ mode_val = MODE_ACKERMAN;
+ else if (0 == strcmp (mode.c_str(), _CONST("differential")))
+ mode_val = MODE_DIFFERENTIAL;
+ else if (0 == strcmp (mode.c_str(), _CONST("mecanum")))
+ mode_val = MODE_MECANUM;
+ else {
+ mode_val = MODE_INVALID;
+ ROS_ERROR("Invalid drive mode %s :: drive mode must be one of ackerman, differential, or mecanum", mode.c_str());
+ return -1;
+ }
+
+ if (rpm <= 0.0) {
+ ROS_ERROR("Invalid RPM %6.4f :: the motor's output RPM must be greater than 0.0", rpm);
+ return -1;
+ }
+
+ if (radius <= 0.0) {
+ ROS_ERROR("Invalid radius %6.4f :: the wheel radius must be greater than 0.0 meters", radius);
+ return -1;
+ }
+
+ if (track <= 0.0) {
+ ROS_ERROR("Invalid track %6.4f :: the axel track must be greater than 0.0 meters", track);
+ return -1;
+ }
+
+ if (scale <= 0.0) {
+ ROS_ERROR("Invalid scale %6.4f :: the scalar for Twist messages must be greater than 0.0", scale);
+ return -1;
+ }
+
+ _active_drive.mode = mode_val;
+ _active_drive.rpm = rpm;
+ _active_drive.radius = radius; // the service takes the radius in meters
+ _active_drive.track = track; // the service takes the track in meters
+ _active_drive.scale = scale;
+
+ ROS_INFO("Drive mode configured: mode=%s, rpm=%6.4f, radius=%6.4f, track=%6.4f, scale=%6.4f", mode.c_str(), rpm, radius, track, scale);
+ return 0;
+}
+
+
+
+/**
+ \private method to initialize private internal data structures at startup
+
+@param devicename a string value indicating the linux I2C device
+
+Example _init ("/dev/i2c-1"); // default I2C device on RPi2 and RPi3 = "/dev/i2c-1"
+ */
+static void _init (const char* filename)
+{
+ int res;
+ char mode1res;
+ int i;
+
+ /* initialize all of the global data objects */
+
+ for (i=0; i<MAX_BOARDS;i++)
+ _pwm_boards[i] = -1;
+ _active_board = -1;
+
+ for (i=0; i<(MAX_SERVOS);i++) {
+ // these values have not useful meaning
+ _servo_configs[i].center = -1;
+ _servo_configs[i].range = -1;
+ _servo_configs[i].direction = 1;
+ _servo_configs[i].mode_pos = -1;
+ }
+ _last_servo = -1;
+
+ _active_drive.mode = MODE_UNDEFINED;
+ _active_drive.rpm = -1.0;
+ _active_drive.radius = -1.0;
+ _active_drive.track = -1.0;
+ _active_drive.scale = -1.0;
+
+
+ if ((_controller_io_handle = open (filename, O_RDWR)) < 0) {
+ ROS_FATAL ("Failed to open I2C bus %s", filename);
+ return; /* exit(1) */ /* additional ERROR HANDLING information is available with 'errno' */
+ }
+ ROS_INFO ("I2C bus opened on %s", filename);
+}
+
+
+
+// ------------------------------------------------------------------------------------------------------------------------------------
+/**
+\defgroup Topics Topics with subscribers provided by this package
+@{ */
+// ------------------------------------------------------------------------------------------------------------------------------------
+
+/**
+ \brief subscriber topic to move servos in a physical position
+
+ Subscriber for the servos_absolute topic which processes one or more servos and sets their physical pulse value.
+
+ When working with a continuous rotation servo,
+ the topic is used to find the center position of a servo by sending successive values until a stopped position is identified.
+ The topic is also used to find the range of a servo - the fastest forward and fasted reverse values. The difference between these two is the servo's range.
+ Due to variences in servos, each will likely have a slightly different center value.
+
+ When working with a fixed 180 degree rotation servo,
+ the topic is used to find the center position of a servo by sending successive values until a the desired middle is identified.
+ The topic is also used to find the range of a servo - the maximum clockwise and anti clockwise positions. The difference between these two is the servo's range.
+ If the servo rotates slightly more in one direction from center than the other, then '2X' the lesser value should be used as the range to preserve the middle stop position.
+
+ Hint: setting the servo pulse value to zero (0) causes the servo to power off. This is refered to as 'coast'. setting a servo to its center value leaves the servo powered and is refered to as 'brake'.
+
+ @param msg a 'ServoArray' message (array of one or more 'Servo') where the servo:value is the pulse position/speed
+
+ __i2cpwm_board::ServoArray__
+ \include "ServoArray.msg"
+ __i2cpwm_board::Servo__
+ \include "Servo.msg"
+
+ __Example__
+ \code{.sh}
+ # this example makes the following assumptions about the hardware
+ # the servos have been connected to the PWM board starting with the first connector and proceeding in order
+ # any drive servos used for the left side servos are mounted opposite of those for the right side
+
+ # the follow message sets the PWM value of the first two servos to 250 and 350 respectively.
+ # depending on center value of each servo, these values may casue forward or backward rotation
+
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 250}, {servo: 2, value: 350}]}"
+
+ # the following messages are an example of finding a continuous servo's center
+ # in this example the center is found to be 333
+
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 300}]}"
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 350}]}"
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 320}]}"
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 330}]}"
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 335}]}"
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 333}]}"
+
+ # power off servo - eg 'coast' rather than 'brake'
+ rostopic pub -1 /servos_absolute i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 0}]}"
+ \endcode
+
+ */
+void servos_absolute (const i2cpwm_board::ServoArray::ConstPtr& msg)
+{
+ /* this subscription works on the active_board */
+
+ for(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {
+ int servo = sp->servo;
+ int value = sp->value;
+
+ if ((value < 0) || (value > 4096)) {
+ ROS_ERROR("Invalid PWM value %d :: PWM values must be between 0 and 4096", value);
+ continue;
+ }
+ _set_pwm_interval (servo, 0, value);
+ ROS_DEBUG("servo[%d] = %d", servo, value);
+ }
+}
+
+
+/**
+ \brief subscriber topic to move servos in the range of ±1.0
+
+ Subscriber for controlling servos using proportional values.
+ This topic processes one or more servos and sets their physical pulse value based on
+ each servos physical range proportional to a range of ±1.0.
+
+ When working with a continuous rotation servo,
+ the topic is used to adjust the speed of the servo.
+
+ When working with a fixed 180 degree rotation servo,
+ the topic is used to adjust the position of the servo.
+
+ This topic requires the use of the config_servos() service.
+ Once the configuration of the servos - center position, direction of rotation, and PWM range - has been set,
+ these data are to convert the proportional value to a physical PWM value specific to each servo.
+
+ @param msg a 'ServoArray' message (array of one or more 'Servo') where the servo:value is a relative position/speed
+
+ __i2cpwm_board::ServoArray Message__
+ \include "ServoArray.msg"
+ __i2cpwm_board::Servo Message__
+ \include "Servo.msg"
+
+ __Example__
+ \code{.sh}
+ # this example makes the following assumptions about the hardware
+ # the servos have been connected to the PWM board starting with the first connector and proceeding in order
+ # any drive servos used for the left side servos are mounted opposite of those for the right side
+
+ # this example uses 2 servos
+ # the first servo is the left and the second servo is the right
+
+ # configure two continuous rotation servos associated with the drive system - these servos were determined to have a ragee of ±50
+
+ rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \
+ {servo: 2, center: 336, range: 108, direction: 1}]"
+
+ # drive both servos forward at 40% of maximum speed
+
+ rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 1, value: 0.40}, {servo: 2, value: 0.40}]}"
+
+ # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188
+
+ rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]"
+
+ # drive the arm servo to its 45 degree position and then to its -45 degree position
+
+ rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: 0.50}]}"
+ rostopic pub -1 /servos_proportional i2cpwm_board/ServoArray "{servos:[{servo: 9, value: -0.50}]}"
+ \endcode
+ */
+void servos_proportional (const i2cpwm_board::ServoArray::ConstPtr& msg)
+{
+ /* this subscription works on the active_board */
+
+ for(std::vector<i2cpwm_board::Servo>::const_iterator sp = msg->servos.begin(); sp != msg->servos.end(); ++sp) {
+ int servo = sp->servo;
+ float value = sp->value;
+ _set_pwm_interval_proportional (servo, value);
+ }
+}
+
+
+
+
+/**
+ \brief subscriber topic to move servos based on a drive mode
+
+ Subscriber for controlling a group of servos in concert based on a geometry_msgs::Twist message.
+
+ This topic requires the use the config_servos() service to configure the servos for proportional control
+ and the use of the config_drive_mode() to speccify the desired type of drive and to assign individual servos to the positions in the drive system.
+
+ @param msg a geometry Twist message
+
+ __geometry_msgs::Twist__
+ \include "Twist.msg"
+ __geometry_msgs::Vector3__
+ \include "Vector3.msg"
+
+ __Example__
+ \code{.sh}
+ # this example makes the following assumptions about the hardware
+ # the servos have been connected to the PWM board starting with the first connector and proceeding in order
+ # any drive servos used for the left side servos are mounted opposite of those for the right side
+
+ # ROS used m/s (meters per second) as th standard unit for velocity.
+ # The geometry_msgs::Twist linear and angular vectors are in m/s and radians/s respectively.
+
+ # a typical high-speed servo is capable of 0.16-0.2 sec/60deg or aproximately 50-65 RPM.
+ # using wheel with 11 cm diameter results in a circumference of 0.35 meters
+ # the theoretical maximum speed of this combination is 0.30 m/s - 0.40 m/s
+
+
+ # differential drive example
+ # this example uses 2 servos
+ # the first servo is the left and the second servo is the right
+
+ # configure drive mode for two RC servos attached to 110mm diameter wheels
+
+ rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \
+ {servo: 2, center: 336, range: 108, direction: 1}]"
+
+ rosservice call /config_drive_mode "{mode: differential, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \
+ servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"
+
+ # moving forward at 0.35 m/s (or best speed)
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"
+
+ # left 45 degrees per second turn while moving forward at 0.35 m/s (or best speed)
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, -0.7854]}"
+
+ # pivoting clockwise at 90 degrees per second
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"
+
+
+ # a mecanum drive example
+ # this example assumes there are 4 servos
+ # the servos are 1, 2, 3, & 4 and correspond to left-front, right-front, left-rear, and right-rear
+
+ # configure drive mode for four servos
+
+ rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: 1}, \
+ {servo: 2, center: 336, range: 108, direction: -1}, \
+ {servo: 3, center: 337, range: 102, direction: -1}, \
+ {servo: 4, center: 330, range: 100, direction: -1}]"
+
+ rosservice call /config_drive_mode "{mode: mecanum, rpm: 60.0, radius: 5.5, track: 5, scale: 1.0, \
+ servos: [{servo: 1, value: 2}, {servo: 2, value: 1}, \
+ {servo: 3, value: 3}, {servo: 4, value: 4}]}"
+
+ # moving forward at full speed
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35.0, 0.0, 0.0], angular: [0.0, 0.0, 0.0]}"
+
+ # moving forward and to the right while always facing forward
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.15, 0.0], angular: [0.0, 0.0, 0.0]}"
+
+ # right turn while moving forward
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.35, 0.0, 0.0], angular: [0.0, 0.0, 0.7854]}"
+
+ # pivoting clockwise
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.0, 0.0], angular: [0.0, 0.0, 1.5708]}"
+
+ # moving sideways to the right
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, 0.2, 0.0], angular: [0.0, 0.0, 0.0]}"
+
+ # moving sideways to the left
+
+ rostopic pub -1 /servos_drive geometry_msgs/Twist "{linear: [0.0, -0.2, 0.0], angular: [0.0, 0.0, 0.0]}"
+
+ \endcode
+ */
+void servos_drive (const geometry_msgs::Twist::ConstPtr& msg)
+{
+ /* this subscription works on the active_board */
+
+ int i;
+ float delta, range, ratio;
+ float temp_x, temp_y, temp_r;
+ float dir_x, dir_y, dir_r;
+ float speed[4];
+
+ /* msg is a pointer to a Twist message: msg->linear and msg->angular each of which have members .x .y .z */
+ /* the subscriber uses the maths from: http://robotsforroboticists.com/drive-kinematics/ */
+
+ ROS_DEBUG("servos_drive Twist = [%5.2f %5.2f %5.2f] [%5.2f %5.2f %5.2f]",
+ msg->linear.x, msg->linear.y, msg->linear.z, msg->angular.x, msg->angular.y, msg->angular.z);
+
+ if (_active_drive.mode == MODE_UNDEFINED) {
+ ROS_ERROR("drive mode not set");
+ return;
+ }
+ if ((_active_drive.mode < MODE_UNDEFINED) || (_active_drive.mode >= MODE_INVALID)) {
+ ROS_ERROR("unrecognized drive mode set %d", _active_drive.mode);
+ return;
+ }
+
+ dir_x = ((msg->linear.x < 0) ? -1 : 1);
+ dir_y = ((msg->linear.y < 0) ? -1 : 1);
+ dir_r = ((msg->angular.z < 0) ? -1 : 1);
+
+ temp_x = _active_drive.scale * _abs(msg->linear.x);
+ temp_y = _active_drive.scale * _abs(msg->linear.y);
+ temp_r = _abs(msg->angular.z); // radians
+
+ // temp_x = _smoothing (temp_x);
+ // temp_y = _smoothing (temp_y);
+ // temp_r = _smoothing (temp_r) / 2;
+
+ // the differential rate is the robot rotational circumference / angular velocity
+ // since the differential rate is applied to both sides in opposite amounts it is halved
+ delta = (_active_drive.track / 2) * temp_r;
+ // delta is now in meters/sec
+
+ // determine if we will over-speed the motor and scal accordingly
+ ratio = _convert_mps_to_proportional(temp_x + delta);
+ if (ratio > 1.0)
+ temp_x /= ratio;
+
+
+ switch (_active_drive.mode) {
+
+ case MODE_ACKERMAN:
+ /*
+ with ackerman drive, steering is handled by a separate servo
+ we drive assigned servos exclusively by the linear.x
+ */
+ speed[0] = temp_x * dir_x;
+ speed[0] = _convert_mps_to_proportional(speed[0]);
+ if (_abs(speed[0]) > 1.0)
+ speed[0] = 1.0 * dir_x;
+
+ ROS_DEBUG("ackerman drive mode speed=%6.4f", speed[0]);
+ break;
+
+ case MODE_DIFFERENTIAL:
+ /*
+ with differential drive, steering is handled by the relative speed of left and right servos
+ we drive assigned servos by mixing linear.x and angular.z
+ we compute the delta for left and right components
+ we use the sign of the angular velocity to determine which is the faster / slower
+ */
+
+ /* the delta is the angular velocity * half the drive track */
+
+ if (dir_r > 0) { // turning right
+ speed[0] = (temp_x + delta) * dir_x;
+ speed[1] = (temp_x - delta) * dir_x;
+ } else { // turning left
+ speed[0] = (temp_x - delta) * dir_x;
+ speed[1] = (temp_x + delta) * dir_x;
+ }
+
+ ROS_DEBUG("computed differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
+
+ /* if any of the results are greater that 1.0, we need to scale all the results down */
+ range = _max (_abs(speed[0]), _abs(speed[1]));
+
+ ratio = _convert_mps_to_proportional(range);
+ if (ratio > 1.0) {
+ speed[0] /= ratio;
+ speed[1] /= ratio;
+ }
+ ROS_DEBUG("adjusted differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
+
+ speed[0] = _convert_mps_to_proportional(speed[0]);
+ speed[1] = _convert_mps_to_proportional(speed[1]);
+
+ ROS_DEBUG("differential drive mode speed left=%6.4f right=%6.4f", speed[0], speed[1]);
+ break;
+
+ case MODE_MECANUM:
+ /*
+ with mecanum drive, steering is handled by the relative speed of left and right servos
+ with mecanum drive, lateral motion is handled by the rotation of front and rear servos
+ we drive assigned servos by mixing linear.x and angular.z and linear.y
+ */
+
+ if (dir_r > 0) { // turning right
+ speed[0] = speed[2] = (temp_x + delta) * dir_x;
+ speed[1] = speed[3] = (temp_x - delta) * dir_x;
+ } else { // turning left
+ speed[0] = speed[2] = (temp_x - delta) * dir_x;
+ speed[1] = speed[3] = (temp_x + delta) * dir_x;
+ }
+
+ speed[0] += temp_y * dir_y;
+ speed[3] += temp_y * dir_y;
+ speed[1] -= temp_y * dir_y;
+ speed[2] -= temp_y * dir_y;
+ ROS_DEBUG("computed mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
+
+ range = _max (_max (_max (_abs(speed[0]), _abs(speed[1])), _abs(speed[2])), _abs(speed[3]));
+ ratio = _convert_mps_to_proportional(range);
+ if (ratio > 1.0) {
+ speed[0] /= ratio;
+ speed[1] /= ratio;
+ speed[2] /= ratio;
+ speed[3] /= ratio;
+ }
+ ROS_DEBUG("adjusted mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
+
+ speed[0] = _convert_mps_to_proportional(speed[0]);
+ speed[1] = _convert_mps_to_proportional(speed[1]);
+ speed[2] = _convert_mps_to_proportional(speed[2]);
+ speed[3] = _convert_mps_to_proportional(speed[3]);
+
+ ROS_DEBUG("mecanum drive mode speed leftfront=%6.4f rightfront=%6.4f leftrear=%6.4f rightreer=%6.4f", speed[0], speed[1], speed[2], speed[3]);
+ break;
+
+ default:
+ break;
+
+ }
+
+ /* find all drive servos and set their new speed */
+ for (i=0; i<(_last_servo); i++) {
+ // we use 'fall thru' on the switch statement to allow all necessary servos to be controlled
+ switch (_active_drive.mode) {
+ case MODE_MECANUM:
+ if (_servo_configs[i].mode_pos == POSITION_RIGHTREAR)
+ _set_pwm_interval_proportional (i+1, speed[3]);
+ if (_servo_configs[i].mode_pos == POSITION_LEFTREAR)
+ _set_pwm_interval_proportional (i+1, speed[2]);
+ case MODE_DIFFERENTIAL:
+ if (_servo_configs[i].mode_pos == POSITION_RIGHTFRONT)
+ _set_pwm_interval_proportional (i+1, speed[1]);
+ case MODE_ACKERMAN:
+ if (_servo_configs[i].mode_pos == POSITION_LEFTFRONT)
+ _set_pwm_interval_proportional (i+1, speed[0]);
+ }
+ }
+}
+
+
+// ------------------------------------------------------------------------------------------------------------------------------------
+/**@}*/
+/**
+\defgroup Services Services interfaces provided by this package
+@{ */
+// services
+// ------------------------------------------------------------------------------------------------------------------------------------
+
+/**
+ \brief service to set set PWM frequency
+
+ The PWM boards drive LED and servos using pulse width modulation. The 12 bit interface means values are 0..4096.
+ The size of the minimum width is determined by the frequency. is service is needed when using a board configured other than with the default I2C address and when using multiple boards.
+
+ If using the set_active_board() service, it must be used before using other services or topics from this package.
+
+ __Warning:__ Changing the frequency will affect any active servos.
+
+ @param [in] req an Int16 value for the requested pulse frequency
+ @param [out] res the return value will be the new active frequency
+ @returns true
+
+ __i2cpwm_board::IntValue__
+ \include "IntValue.srv"
+
+ __Example__
+ \code{.sh}
+ # Analog RC servos are most often designed for 20ms pulses. This is achieved with a frequency of 50Hz.
+ # This software defaults to 50Hz. Use the set_pwm_frequncy() to change this frequency value.
+ # It may be necessary or convenient to change the PWM frequency if using DC motors connected to PWM controllers.
+ # It may also be convenient if using PWM to control LEDs.
+
+ rosservice call /set_pwm_frequency "{value: 50}"
+ \endcode
+
+ */
+bool set_pwm_frequency (i2cpwm_board::IntValue::Request &req, i2cpwm_board::IntValue::Response &res)
+{
+ int freq;
+ freq = req.value;
+ if ((freq<12) || (freq>1024)) {
+ ROS_ERROR("Invalid PWM frequency %d :: PWM frequencies should be between 12 and 1024", freq);
+ freq = 50; // most analog RC servos are designed for 20ms pulses.
+ res.error = freq;
+ }
+ _set_pwm_frequency (freq); // I think we must reset frequency when we change boards
+ res.error = freq;
+ return true;
+}
+
+
+/**
+ \brief store configuration data for servos on the active board
+
+ A service to set each servo's center value, direction of rotation (1 for forward and -1 for reverse motion),
+ the center or nul value, range, and direction of one or more servos.
+ and range between full left and right or maximun forward and backward speed.
+
+ Setting these data are required before sending messages to the servos_proportional() topic as well as the servos_drive() topic.
+
+ If more than one PWM board is present, the set_active_board() service is used to switch between boards prior to configuring servos.
+
+ @param [in] req an array of 'ServoConfig' which consists of a servo number (one based), center(0..4096), range(0..4096), and direction (±1).
+ @param [out] res integer non-zero if an error occured
+ @returns true
+
+ __i2cpwm_board::ServosConfig__
+ \include "ServosConfig.srv"
+ __i2cpwm_board::ServoConfig__
+ \include "ServoConfig.msg"
+
+
+ __Example__
+ \code{.sh}
+ # this example makes the following assumptions about the hardware
+ # the servos have been connected to the PWM board starting with the first connector and proceeding in order
+ # any drive servos used for the left side servos are mounted opposite of those for the right side
+
+ # this example uses 2 servos
+ # the first servo is the left and the second servo is the right
+
+ # configure two continuous rotation servos associated with the drive system - these servos were determined to have a ragee of ±50
+
+ rosservice call /config_servos "servos: [{servo: 1, center: 333, range: 100, direction: -1}, \
+ {servo: 2, center: 336, range: 108, direction: 1}]"
+
+ # additionally configure one 180 degree servo (±90) used for a robot arm - this servo was determine to have a ragee of ±188
+
+ rosservice call /config_servos "servos: [{servo: 9, center: 344, range: 376, direction: -1}]"
+
+ \endcode
+ */
+bool config_servos (i2cpwm_board::ServosConfig::Request &req, i2cpwm_board::ServosConfig::Response &res)
+{
+ /* this service works on the active_board */
+ int i;
+
+ res.error = 0;
+
+ if ((_active_board<1) || (_active_board>62)) {
+ ROS_ERROR("Internal error - invalid board number %d :: PWM board numbers must be between 1 and 62", _active_board);
+ res.error = -1;
+ return true;
+ }
+
+ for (i=0;i<req.servos.size();i++) {
+ int servo = req.servos[i].servo;
+ int center = req.servos[i].center;
+ int range = req.servos[i].range;
+ int direction = req.servos[i].direction;
+
+ _config_servo (servo, center, range, direction);
+ }
+
+ return true;
+}
+
+
+/**
+ \brief set drive mode and drive servos
+
+ A service to set the desired drive mode. It must be called before messages are handled by the servos_drive() topic.
+ Setting these data are required before sending messages to the servos_proportional() topic.
+
+ The drive mode consists of a string value for the type of drive desired: ackerman, differential, or mecanum.
+ For each mode, the drive servos must be specified.
+
+ @param req [in] DriveMode configuration data
+ @param res [out] non-zero on error
+ @returns true
+
+ The DriveMode input requires drive system details included: wheel RPM, wheel radius, and track width.
+ ROS uses meters for measurements. The values of radius and track are expected in meters.
+
+ _A scale factor is available if necessary to compensate for linear vector values._
+
+ The mode string is one of the following drive systems:
+ -# 'ackerman' - (automobile steering) requires minimum of one servo for drive and uses some other servo for stearing..
+ -# 'differential' - requires multiples of two servos, designated as left and right.
+ -# 'mecanum' - requires multiples of four servos, designated as left-front, right-front, left-rear, and right-rear.
+
+ The servo message is used for indicating which servos are used for the drive system.
+ The message consists of 'servo' number, and data 'value'.
+
+ The 'value' field indicates the positon the corresponding servo within the drive system.
+ The applicable servos are assigned positions as follows:
+
+ positon | ackerman | differential | mecanum
+ --------|----------|--------------|--------
+ position 1 corresponds to | drive | left | left-front
+ position 2 corresponds to | | right | right-front
+ position 3 corresponds to | | | left-rear
+ position 4 corresponds to | | | right-rear
+
+ __i2cpwm_board::DriveMode__
+ \include "DriveMode.srv"
+ __i2cpwm_board::Position__
+ \include "Position.msg"
+
+ __Example__
+ \code{.sh}
+ # this example makes the following assumptions about the hardware
+ # the servos have been connected to the PWM board starting with the first connector and proceeding in order
+ # any drive servos used for the left side servos are mounted opposite of those for the right side
+
+ # ROS used m/s (meters per second) as th standard unit for velocity.
+ # The geometry_msgs::Twist linear and angular vectors are in m/s and radians/s respectively.
+
+ # differential drive example
+ # this example uses 2 servos
+ # the first servo is the left and the second servo is the right
+
+ rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
+ servos: [{servo: 1, value: 1}, {servo: 2, value: 2}]}"
+
+ # this example uses 4 servos
+ # there are two servos for the left and two fors the right
+
+ rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
+ servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \
+ {servo: 3, value: 1}, {servo: 4, value: 2}]}"
+
+ # mecanum drive example
+ # this example uses 4 servos
+
+ rosservice call /config_drive_mode "{mode: differential, rpm: 56.0, radius: 0.0055, track: 0.015, scale: 1.0, \
+ servos: [{servo: 1, value: 1}, {servo: 2, value: 2}, \
+ {servo: 3, value: 1}, {servo: 4, value: 2}]}"
+
+ \endcode
+ */
+bool config_drive_mode (i2cpwm_board::DriveMode::Request &req, i2cpwm_board::DriveMode::Response &res)
+{
+ res.error = 0;
+
+ int i;
+
+ if ((res.error = _config_drive_mode (req.mode, req.rpm, req.radius, req.track, req.scale)))
+ return true;
+
+ for (i=0;i<req.servos.size();i++) {
+ int servo = req.servos[i].servo;
+ int position = req.servos[i].position;
+
+ if (_config_servo_position (servo, position) != 0) {
+ res.error = servo; /* this needs to be more specific and indicate a bad server ID was provided */
+ continue;
+ }
+ }
+
+ return true;
+}
+
+
+
+/**
+ \brief service to stop all servos on all boards
+
+ A service to stop all of the servos on all of the PWM boards and set their power state to off / coast.
+
+ This is different from setting each servo to its center value. A centered servo is still under power and it's in a brake state.
+
+ @param req is empty
+ @param res is empty
+ @returns true
+
+ __Example__
+ \code{.sh}
+ # stop all servos on all boards and setting them to coast rather than brake
+
+ rosservice call /stop_servos
+ \endcode
+
+ */
+bool stop_servos (std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
+{
+ int save_active = _active_board;
+ int i;
+
+ for (i=0; i<MAX_BOARDS; i++) {
+ if (_pwm_boards[i] > 0) {
+ _set_active_board (i+1); // API is ONE based
+ _set_pwm_interval_all (0, 0);
+ }
+ }
+ _set_active_board (save_active); // restore last active board
+ return true;
+}
+
+
+
+
+
+static std::string _get_string_param (XmlRpc::XmlRpcValue obj, std::string param_name)
+{
+ XmlRpc::XmlRpcValue &item = obj[param_name];
+ if (item.getType() == XmlRpc::XmlRpcValue::TypeString)
+ return item;
+
+ ROS_WARN("invalid paramter type for %s - expected TypeString", param_name.c_str());
+ return 0;
+}
+
+
+static int _get_int_param (XmlRpc::XmlRpcValue obj, std::string param_name)
+{
+ XmlRpc::XmlRpcValue &item = obj[param_name];
+ if (item.getType() == XmlRpc::XmlRpcValue::TypeInt)
+ return item;
+
+ ROS_WARN("invalid paramter type for %s - expected TypeInt", param_name.c_str());
+ return 0;
+}
+
+
+static double _get_float_param (XmlRpc::XmlRpcValue obj, std::string param_name)
+{
+ XmlRpc::XmlRpcValue &item = obj[param_name];
+ if (item.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+ return item;
+
+ ROS_WARN("invalid paramter type for %s - expected TypeDouble", param_name.c_str());
+ return 0;
+}
+
+
+static int _load_params (void)
+{
+ ros::NodeHandle nhp; // not currently private namespace
+
+ // default I2C device on RPi2 and RPi3 = "/dev/i2c-1" Orange Pi Lite = "/dev/i2c-0"
+ nhp.param ("i2c_device_number", _controller_io_device, 1);
+ std::stringstream device;
+ device << "/dev/i2c-" << _controller_io_device;
+ _init (device.str().c_str());
+
+ _set_active_board (1);
+
+ int pwm;
+ nhp.param ("pwm_frequency", pwm, 50);
+ _set_pwm_frequency (pwm);
+
+
+ /*
+ // note: servos are numbered sequntially with '1' being the first servo on board #1, '17' is the first servo on board #2
+
+ servo_config:
+ - {servo: 1, center: 333, direction: -1, range: 100}
+ - {servo: 2, center: 336, direction: 1, range: 108}
+
+ */
+ // attempt to load configuration for servos
+ if(nhp.hasParam ("servo_config")) {
+ XmlRpc::XmlRpcValue servos;
+ nhp.getParam ("servo_config", servos);
+
+ if(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
+ ROS_DEBUG("Retrieving members from 'servo_config' in namespace(%s)", nhp.getNamespace().c_str());
+
+ for(int32_t i = 0; i < servos.size(); i++) {
+ XmlRpc::XmlRpcValue servo;
+ servo = servos[i]; // get the data from the iterator
+ if(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
+ ROS_DEBUG("Retrieving items from 'servo_config' member %d in namespace(%s)", i, nhp.getNamespace().c_str());
+
+ // get the servo settings
+ int id, center, direction, range;
+ id = _get_int_param (servo, "servo");
+ center = _get_int_param (servo, "center");
+ direction = _get_int_param (servo, "direction");
+ range = _get_int_param (servo, "range");
+
+ if (id && center && direction && range) {
+ if ((id >= 1) && (id <= MAX_SERVOS)) {
+ int board = ((int)(id / 16)) + 1;
+ _set_active_board (board);
+ _set_pwm_frequency (pwm);
+ _config_servo (id, center, range, direction);
+ }
+ else
+ ROS_WARN("Parameter servo=%d is out of bounds", id);
+ }
+ else
+ ROS_WARN("Invalid parameters for servo=%d'", id);
+ }
+ else
+ ROS_WARN("Invalid type %d for member of 'servo_config' - expected TypeStruct(%d)", servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
+ }
+ }
+ else
+ ROS_WARN("Invalid type %d for 'servo_config' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
+ }
+ else
+ ROS_DEBUG("Parameter Server namespace[%s] does not contain 'servo_config", nhp.getNamespace().c_str());
+
+
+ /*
+ drive_config:
+ mode: mecanum
+ radius: 0.062
+ rpm: 60.0
+ scale: 0.3
+ track: 0.2
+ servos:
+ - {servo: 1, position: 1}
+ - {servo: 2, position: 2}
+ - {servo: 3, position: 3}
+ - {servo: 4, position: 4}
+ */
+
+ // attempt to load configuration for drive mode
+ if(nhp.hasParam ("drive_config")) {
+ XmlRpc::XmlRpcValue drive;
+ nhp.getParam ("drive_config", drive);
+
+ if(drive.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
+ ROS_DEBUG("Retrieving members from 'drive_config' in namespace(%s)", nhp.getNamespace().c_str());
+
+ // get the drive mode settings
+ std::string mode;
+ float radius, rpm, scale, track;
+ int id, position;
+
+ mode = _get_string_param (drive, "mode");
+ rpm = _get_float_param (drive, "rpm");
+ radius = _get_float_param (drive, "radius");
+ track = _get_float_param (drive, "track");
+ scale = _get_float_param (drive, "scale");
+
+ _config_drive_mode (mode, rpm, radius, track, scale);
+
+ XmlRpc::XmlRpcValue &servos = drive["servos"];
+ if(servos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
+ ROS_DEBUG("Retrieving members from 'drive_config/servos' in namespace(%s)", nhp.getNamespace().c_str());
+
+ for(int32_t i = 0; i < servos.size(); i++) {
+ XmlRpc::XmlRpcValue servo;
+ servo = servos[i]; // get the data from the iterator
+ if(servo.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
+ ROS_DEBUG("Retrieving items from 'drive_config/servos' member %d in namespace(%s)", i, nhp.getNamespace().c_str());
+
+ // get the servo position settings
+ int id, position;
+ id = _get_int_param (servo, "servo");
+ position = _get_int_param (servo, "position");
+
+ if (id && position)
+ _config_servo_position (id, position); // had its own error reporting
+ }
+ else
+ ROS_WARN("Invalid type %d for member %d of 'drive_config/servos' - expected TypeStruct(%d)", i, servo.getType(), XmlRpc::XmlRpcValue::TypeStruct);
+ }
+ }
+ else
+ ROS_WARN("Invalid type %d for 'drive_config/servos' - expected TypeArray(%d)", servos.getType(), XmlRpc::XmlRpcValue::TypeArray);
+ }
+ else
+ ROS_WARN("Invalid type %d for 'drive_config' - expected TypeStruct(%d)", drive.getType(), XmlRpc::XmlRpcValue::TypeStruct);
+ }
+ else
+ ROS_DEBUG("Parameter Server namespace[%s] does not contain 'drive_config", nhp.getNamespace().c_str());
+}
+
+
+// ------------------------------------------------------------------------------------------------------------------------------------
+/**@}*/
+// main
+// ------------------------------------------------------------------------------------------------------------------------------------
+
+int main (int argc, char **argv)
+{
+ // globals
+ _controller_io_device = 1; // default I2C device on RPi2 and RPi3 = "/dev/i2c-1" Orange Pi Lite = "/dev/i2c-0"
+ _controller_io_handle = 0;
+ _pwm_frequency = 50; // set the initial pulse frequency to 50 Hz which is standard for RC servos
+
+
+
+ ros::init (argc, argv, "i2cpwm_controller");
+
+ ros::NodeHandle n;
+
+ ros::ServiceServer freq_srv = n.advertiseService ("set_pwm_frequency", set_pwm_frequency);
+ ros::ServiceServer config_srv = n.advertiseService ("config_servos", config_servos); // 'config' will setup the necessary properties of continuous servos and is helpful for standard servos
+ ros::ServiceServer mode_srv = n.advertiseService ("config_drive_mode", config_drive_mode); // 'mode' specifies which servos are used for motion and which behavior will be applied when driving
+ ros::ServiceServer stop_srv = n.advertiseService ("stop_servos", stop_servos); // the 'stop' service can be used at any time
+
+ ros::Subscriber abs_sub = n.subscribe ("servos_absolute", 500, servos_absolute); // the 'absolute' topic will be used for standard servo motion and testing of continuous servos
+ ros::Subscriber rel_sub = n.subscribe ("servos_proportional", 500, servos_proportional); // the 'proportion' topic will be used for standard servos and continuous rotation aka drive servos
+ ros::Subscriber drive_sub = n.subscribe ("servos_drive", 500, servos_drive); // the 'drive' topic will be used for continuous rotation aka drive servos controlled by Twist messages
+
+ _load_params(); // loads parameters and performs initialization
+
+
+ ros::spin();
+
+ close (_controller_io_handle);
+
+ return 0;
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/DriveMode.srv b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/DriveMode.srv
new file mode 100644
index 0000000000000000000000000000000000000000..2af5f06305506e5d6b109529b87304ac679970ab
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/DriveMode.srv
@@ -0,0 +1,18 @@
+# the drive_mode service is used to assigned servos to various drive modes
+# the drive modes determine how the assigned servos respond to geometry_msgs::Twist messages
+# drive modes are one of: ackerman, differential, or mecanum
+# to accurately convert velocity in m/s the controller needs to know three values:
+# the RPM - the maximum output speed available from the drive motors
+# the radius - the drive wheel radius in meters
+# the track - the distance between the left and right wheels in meters
+# use the scale value to adjust incoming Twist values as needed to match the servo/motor capability
+
+
+string mode
+float32 rpm
+float32 radius
+float32 track
+float32 scale
+Position[] servos
+---
+int16 error
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/IntValue.srv b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/IntValue.srv
new file mode 100644
index 0000000000000000000000000000000000000000..ba18dfd1275570e67ad03aa0ebd4b8d318d771e2
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/IntValue.srv
@@ -0,0 +1,7 @@
+# there are a few services whic take a single integer as input
+# these services share the IntValue service definition
+
+int16 value
+---
+int16 error
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/ServosConfig.srv b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/ServosConfig.srv
new file mode 100644
index 0000000000000000000000000000000000000000..83587c1c73efe6c603595a1c58ef56c1aff5a0ab
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/ServosConfig.srv
@@ -0,0 +1,8 @@
+# the servos_config service is used to assign useful configuration data to servos
+# the tollerance of electronis varies in RC servos so it is important to calibate
+# each servo, indicating its PWM value for direction of rotation, centering, and
+# range which is used to scale servo motion a standard ±1000 scale
+
+ServoConfig[] servos
+---
+int16 error
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/StopServos.srv b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/StopServos.srv
new file mode 100644
index 0000000000000000000000000000000000000000..76819baa2c8035384449a433e22d1c0f5989dadc
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/StopServos.srv
@@ -0,0 +1,3 @@
+
+
+---
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7823aad92358e0673decd496e32a2826b63b119d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/CMakeLists.txt
@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(servo_move_keyboard)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ i2cpwm_board
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES servo_move_keyboard
+# CATKIN_DEPENDS rospy
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/servo_move_keyboard.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/servo_move_keyboard_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_servo_move_keyboard.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/launch/keyboard_move.launch b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/launch/keyboard_move.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d6ea0bb58db82043c0725e13831b7bef219970cb
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/launch/keyboard_move.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Launch the servo move via keyboard command node, and, optionally, the i2c_pwmboard node -->
+
+<launch>
+ <!-- Optional command line argument to also run i2c_pwmboard, if running on a rpi -->
+ <arg name="run_i2c_pwmboard" default="false"/>
+
+ <!-- Defining the node and executable and publishing the output on terminal-->
+ <node name="servo_move_keyboard_node" pkg="servo_move_keyboard" type="servoMoveKeyboard.py" output="screen">
+ </node>
+
+ <!-- If run_i2c_pwmboard is true, also run the i2c_pwm_board node -->
+ <node if="$(arg run_i2c_pwmboard)" name="i2cpwm_board_node" pkg="i2cpwm_board" type="i2cpwm_board" output="screen"> │walk: Start walk mode and keyboard motion control
+ </node>
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..860071ba0fee099cb6d725456fa7254e39387a6d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/package.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>servo_move_keyboard</name>
+ <version>0.0.0</version>
+ <description>Package to move servos via keyboard inputs</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/servo_move_keyboard</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>rospy</build_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <exec_depend>rospy</exec_depend>
+
+ <build_depend>i2cpwm_board</build_depend>
+ <exec_depend>i2cpwm_board</exec_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoConfigTest.py b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoConfigTest.py
new file mode 100755
index 0000000000000000000000000000000000000000..58a47d756e705b0a1a292ce7f1eb13b031c84124
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoConfigTest.py
@@ -0,0 +1,297 @@
+#!/usr/bin/python
+
+"""
+Class for testing control of 12 servos. It assumes ros-12cpwmboard has been
+installed
+"""
+import rospy
+import sys, select, termios, tty # For terminal keyboard key press reading
+from i2cpwm_board.msg import Servo, ServoArray, ServoConfig, ServoConfigArray
+from i2cpwm_board.srv import ServosConfig
+
+# Global variable for number of servos
+numServos = 1
+
+msg = """
+Servo Control Module for 12 Servos.
+
+Enter one of the following options:
+-----------------------------
+quit: stop and quit the program
+oneServo: Move one servo manually, all others will be commanded to their center position
+allServos: Move all servo's manually together
+
+Keyboard commands for One Servo Control
+---------------------------
+ q t y u
+ f g j k
+ b n m
+
+ q: Quit current command mode and go back to Option Select
+ t: Command servo min value
+ y: Command servo center value
+ u: Command servo max value
+ f: Manually decrease servo command value by 10
+ g: Manually decrease servo command value by 1
+ j: Manually increase servo command value by 1
+ k: Manually increase servo command value by 10
+ b: Save new min command value
+ n: Save new center command value
+ m: Save new max command value
+
+
+ anything else : Prompt again for command
+
+
+CTRL-C to quit
+"""
+# Dictionary with anonomous helper functions to execute key commands
+keyDict = {
+ 'q': None,
+ 't': lambda x: x.set_value(x._min),
+ 'y': lambda x: x.set_value(x._center),
+ 'u': lambda x: x.set_value(x._max),
+ 'f': lambda x: x.set_value(x.value-0.01),
+ 'g': lambda x: x.set_value(x.value-.1),
+ 'j': lambda x: x.set_value(x.value+.1),
+ 'k': lambda x: x.set_value(x.value+0.01),
+ 'b': lambda x: x.set_min(x.value),
+ 'n': lambda x: x.set_center(x.value),
+ 'm': lambda x: x.set_max(x.value),
+}
+
+validCmds = ['quit','oneServo','allServos']
+
+class ServoConvert():
+ '''
+ ServoConvert Class encapsulates a servo
+ Servo has a center value, and range, and is commanded by a value between 0 and 4095.
+ This coorsponds to the duty cycle in a 12 bit pwm cycle. Nominally, a servo is commanded with pulses of
+ 1 to 2 ms in a 20 ms cycle, with 1.5 ms being the value for center position.
+ These nominal values would coorespond to integer values of approximately 204, 306, and 409
+ for 1 ms, 1.5 ms, and 2 ms, respectively
+ '''
+ def __init__(self, id=1, center_value=0, direction=1):
+ self.value = center_value
+ self._center = center_value
+ self._min = -1
+ self._max = 1
+ self._dir = direction
+ self.id = id
+
+ def set_value(self, value_in):
+ '''
+ Set Servo value
+ Input: Value between 0 and 4095
+ '''
+ if False:
+ print('Servo value not in range [0,4095]')
+ else:
+ self.value = value_in
+
+
+ def set_center(self,center_val):
+ '''
+ Set Servo center value
+ Input: Value between 0 and 4095
+ '''
+ if False:
+ print('Servo value not in range [0,4095]')
+ else:
+ self._center = center_val
+ print('Servo %2i center set to %4i'%(self.id+1,center_val))
+
+ def set_max(self,max_val):
+ '''
+ Set Servo max value
+ Input: Value between 0 and 4095
+ '''
+ if False:
+ print('Servo value not in range [0,4095]')
+ else:
+ self._max = max_val
+ print('Servo %2i max set to %4i'%(self.id+1,max_val))
+
+ def set_min(self,min_val):
+ '''
+ Set Servo min value
+ Input: Value between 0 and 4095
+ '''
+ if False:
+ print('Servo value not in range [0,4095]')
+ else:
+ self._min = min_val
+ print('Servo %2i min set to %4i'%(self.id+1,min_val))
+
+class SpotMicroServoControl():
+ def __init__(self):
+ # Create a ServoConfig messag for one servo
+ self._servo_config_msg = ServoConfigArray()
+ self._servo1_config = ServoConfig()
+
+ self._servo1_config.center = 300
+ self._servo1_config.range = 400
+ self._servo1_config.servo = 1
+ self._servo1_config.direction = 1
+
+ self._servo_config_msg.servos.append(self._servo1_config)
+
+ rospy.loginfo("> Waiting for config_servos service...")
+ print('test1')
+ rospy.wait_for_service('config_servos')
+ rospy.loginfo("> Config_servos service found!")
+ print('test2')
+ try:
+ servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig)
+ resp = servoConfigService(self._servo_config_msg.servos)
+ print("Config servos done!!, returned value: %i"%resp.error)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
+
+
+ rospy.loginfo("Setting Up the Spot Micro Servo Control Node...")
+
+ # Set up and title the ros node for this code
+ rospy.init_node('spot_micro_servo_control')
+
+ # Intialize empty servo dictionary
+ self.servos = {}
+
+ # Create a servo dictionary with 12 ServoConvert objects
+ # keys: integers 0 through 12
+ # values: ServoConvert objects
+ for i in range(numServos):
+ self.servos[i] = ServoConvert(id=i)
+ rospy.loginfo("> Servos corrrectly initialized")
+
+ # Create empty ServoArray message with n number of Servos in its array
+ self._servo_msg = ServoArray()
+ for i in range(numServos):
+ self._servo_msg.servos.append(Servo())
+
+ # Create the servo array publisher
+ self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1)
+ rospy.loginfo("> Publisher corrrectly initialized")
+
+ rospy.loginfo("Initialization complete")
+
+ # Setup terminal input reading, taken from teleop_twist_keyboard
+ self.settings = termios.tcgetattr(sys.stdin)
+
+ def send_servo_msg(self):
+ for servo_key, servo_obj in self.servos.iteritems():
+ self._servo_msg.servos[servo_obj.id].servo = servo_obj.id+1
+ self._servo_msg.servos[servo_obj.id].value = servo_obj.value
+ #rospy.loginfo("Sending to %s command %d"%(servo_key, servo_obj.value))
+
+ self.ros_pub_servo_array.publish(self._servo_msg)
+
+ def reset_all_servos_center(self):
+ '''
+ Reset all servos to their center value
+ '''
+ for s in self.servos:
+ self.servos[s].value = self.servos[s]._center
+
+ def getKey(self):
+ tty.setraw(sys.stdin.fileno())
+ select.select([sys.stdin], [], [], 0)
+ key = sys.stdin.read(1)
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ return key
+
+ def run(self):
+
+ # Set all servos to their center values
+ self.reset_all_servos_center()
+ self.send_servo_msg()
+
+ # Prompt user with keyboard command information
+ # Ability to control individual servo to find limits and center values
+ # and ability to control all servos together
+
+ while not rospy.is_shutdown():
+ print(msg)
+ userInput = raw_input("Command?: ")
+
+ if userInput not in validCmds:
+ print('Valid command not entered, try again...')
+ else:
+ if userInput == 'quit':
+ print("Ending program...")
+ print('Final Servo Values')
+ print('--------------------')
+ for i in range(numServos):
+ print('Servo %2i: Min: %1.2f, Center: %1.2f, Max: %1.2f'%(i,self.servos[i]._min,self.servos[i]._center,self.servos[i]._max))
+ break
+
+ elif userInput == 'oneServo':
+ # Reset all servos to center value, and send command
+ self.reset_all_servos_center()
+ self.send_servo_msg()
+
+ # First get servo number to command
+ nSrv = -1
+ while (1):
+ userInput = input('Which servo to control? Enter a number 1 through 12: ')
+
+ if userInput not in range(1,numServos+1):
+ print("Invalid servo number entered, try again")
+ else:
+ nSrv = userInput - 1
+ break
+
+ # Loop and act on user command
+ print('Enter command, q to go back to option select: ')
+ while (1):
+
+ userInput = self.getKey()
+
+ if userInput == 'q':
+ break
+ elif userInput not in keyDict:
+ print('Key not in valid key commands, try again')
+ else:
+ keyDict[userInput](self.servos[nSrv])
+ print('Servo %2i cmd: %1.2f'%(nSrv,self.servos[nSrv].value))
+ self.send_servo_msg()
+
+ elif userInput == 'allServos':
+ # Reset all servos to center value, and send command
+ self.reset_all_servos_center()
+ self.send_servo_msg()
+
+ print('Enter command, q to go back to option select: ')
+ while (1):
+
+ userInput = self.getKey()
+
+ if userInput == 'q':
+ break
+ elif userInput not in keyDict:
+ print('Key not in valid key commands, try again')
+ elif userInput in ('b','n','m'):
+ print('Saving values not supported in all servo control mode')
+ else:
+ for s in self.servos.values():
+ keyDict[userInput](s)
+ print('All Servos Commanded')
+ self.send_servo_msg()
+
+
+
+
+
+
+
+ # print self._last_time_cmd_rcv, self.is_controller_connected
+ # if not self.is_controller_connected:
+ # self.set_actuators_idle()
+
+ # Set the control rate in Hz
+ rate = rospy.Rate(10)
+ rate.sleep()
+
+if __name__ == "__main__":
+ smsc = SpotMicroServoControl()
+ smsc.run()
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoMoveKeyboard.py b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoMoveKeyboard.py
new file mode 100755
index 0000000000000000000000000000000000000000..c8ddd09d60cbfb7e0cc56ff3188fffad4a98548b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoMoveKeyboard.py
@@ -0,0 +1,277 @@
+#!/usr/bin/python
+
+"""
+Class for testing control of 12 servos. It assumes ros-12cpwmboard has been
+installed
+"""
+import rospy
+import sys, select, termios, tty # For terminal keyboard key press reading
+from i2cpwm_board.msg import Servo, ServoArray
+
+# Global variable for number of servos
+numServos = 12
+
+msg = """
+Servo Control Module for 12 Servos.
+
+Enter one of the following options:
+-----------------------------
+quit: stop and quit the program
+oneServo: Move one servo manually, all others will be commanded to their center position
+allServos: Move all servo's manually together
+
+Keyboard commands for One Servo Control
+---------------------------
+ q y
+ f g j k
+ z x b n m
+
+ q: Quit current command mode and go back to Option Select
+ z: Command servo min value
+ y: Command servo center value
+ x: Command servo max value
+ f: Manually decrease servo command value by 10
+ g: Manually decrease servo command value by 1
+ j: Manually increase servo command value by 1
+ k: Manually increase servo command value by 10
+ b: Save new min command value
+ n: Save new center command value
+ m: Save new max command value
+
+
+ anything else : Prompt again for command
+
+
+CTRL-C to quit
+"""
+# Dictionary with anonomous helper functions to execute key commands
+keyDict = {
+ 'q': None,
+ 'z': lambda x: x.set_value(x._min),
+ 'y': lambda x: x.set_value(x._center),
+ 'x': lambda x: x.set_value(x._max),
+ 'f': lambda x: x.set_value(x.value-10),
+ 'g': lambda x: x.set_value(x.value-1),
+ 'j': lambda x: x.set_value(x.value+1),
+ 'k': lambda x: x.set_value(x.value+10),
+ 'b': lambda x: x.set_min(x.value),
+ 'n': lambda x: x.set_center(x.value),
+ 'm': lambda x: x.set_max(x.value),
+}
+
+validCmds = ['quit','oneServo','allServos']
+
+class ServoConvert():
+ '''
+ ServoConvert Class encapsulates a servo
+ Servo has a center value, and range, and is commanded by a value between 0 and 4095.
+ This coorsponds to the duty cycle in a 12 bit pwm cycle. Nominally, a servo is commanded with pulses of
+ 1 to 2 ms in a 20 ms cycle, with 1.5 ms being the value for center position.
+ These nominal values would coorespond to integer values of approximately 204, 306, and 409
+ for 1 ms, 1.5 ms, and 2 ms, respectively
+ '''
+ def __init__(self, id=1, center_value=306, direction=1):
+ self.value = center_value
+ self._center = center_value
+ self._min = 83
+ self._max = 520
+ self._dir = direction
+ self.id = id
+
+ def set_value(self, value_in):
+ '''
+ Set Servo value
+ Input: Value between 0 and 4095
+ '''
+ if value_in not in range(4096):
+ print('Servo value not in range [0,4095]')
+ else:
+ self.value = value_in
+
+
+ def set_center(self,center_val):
+ '''
+ Set Servo center value
+ Input: Value between 0 and 4095
+ '''
+ if center_val not in range(4096):
+ print('Servo value not in range [0,4095]')
+ else:
+ self._center = center_val
+ print('Servo %2i center set to %4i'%(self.id+1,center_val))
+
+ def set_max(self,max_val):
+ '''
+ Set Servo max value
+ Input: Value between 0 and 4095
+ '''
+ if max_val not in range(4096):
+ print('Servo value not in range [0,4095]')
+ else:
+ self._max = max_val
+ print('Servo %2i max set to %4i'%(self.id+1,max_val))
+
+ def set_min(self,min_val):
+ '''
+ Set Servo min value
+ Input: Value between 0 and 4095
+ '''
+ if min_val not in range(4096):
+ print('Servo value not in range [0,4095]')
+ else:
+ self._min = min_val
+ print('Servo %2i min set to %4i'%(self.id+1,min_val))
+
+class SpotMicroServoControl():
+ def __init__(self):
+ rospy.loginfo("Setting Up the Spot Micro Servo Control Node...")
+
+ # Set up and title the ros node for this code
+ rospy.init_node('spot_micro_servo_control')
+
+ # Intialize empty servo dictionary
+ self.servos = {}
+
+ # Create a servo dictionary with 12 ServoConvert objects
+ # keys: integers 0 through 12
+ # values: ServoConvert objects
+ for i in range(numServos):
+ self.servos[i] = ServoConvert(id=i)
+ rospy.loginfo("> Servos corrrectly initialized")
+
+ # Create empty ServoArray message with n number of Servos in its array
+ self._servo_msg = ServoArray()
+ for i in range(numServos):
+ self._servo_msg.servos.append(Servo())
+
+ # Create the servo array publisher
+ self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
+ rospy.loginfo("> Publisher corrrectly initialized")
+
+ rospy.loginfo("Initialization complete")
+
+ # Setup terminal input reading, taken from teleop_twist_keyboard
+ self.settings = termios.tcgetattr(sys.stdin)
+
+ def send_servo_msg(self):
+ for servo_key, servo_obj in self.servos.iteritems():
+ self._servo_msg.servos[servo_obj.id].servo = servo_obj.id+1
+ self._servo_msg.servos[servo_obj.id].value = servo_obj.value
+ #rospy.loginfo("Sending to %s command %d"%(servo_key, servo_obj.value))
+
+ self.ros_pub_servo_array.publish(self._servo_msg)
+
+ def reset_all_servos_center(self):
+ '''
+ Reset all servos to their center value
+ '''
+ for s in self.servos:
+ self.servos[s].value = self.servos[s]._center
+
+ def reset_all_servos_off(self):
+ '''Set all servos to off/freewheel value (pwm of 0)'''
+ for s in self.servos:
+ self.servos[s].value = 0
+
+ def getKey(self):
+ tty.setraw(sys.stdin.fileno())
+ select.select([sys.stdin], [], [], 0)
+ key = sys.stdin.read(1)
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ return key
+
+ def run(self):
+
+ # Set all servos to their center values
+ self.reset_all_servos_center()
+ self.send_servo_msg()
+
+ # Prompt user with keyboard command information
+ # Ability to control individual servo to find limits and center values
+ # and ability to control all servos together
+
+ while not rospy.is_shutdown():
+ print(msg)
+ userInput = raw_input("Command?: ")
+
+ if userInput not in validCmds:
+ print('Valid command not entered, try again...')
+ else:
+ if userInput == 'quit':
+ print("Ending program...")
+ print('Final Servo Values')
+ print('--------------------')
+ for i in range(numServos):
+ print('Servo %2i: Min: %4i, Center: %4i, Max: %4i'%(i,self.servos[i]._min,self.servos[i]._center,self.servos[i]._max))
+ break
+
+ elif userInput == 'oneServo':
+ # Reset all servos to center value, and send command
+ self.reset_all_servos_off()
+ self.send_servo_msg()
+
+ # First get servo number to command
+ nSrv = -1
+ while (1):
+ userInput = input('Which servo to control? Enter a number 1 through 12: ')
+
+ if userInput not in range(1,numServos+1):
+ print("Invalid servo number entered, try again")
+ else:
+ nSrv = userInput - 1
+ break
+
+ # Loop and act on user command
+ print('Enter command, q to go back to option select: ')
+ while (1):
+
+ userInput = self.getKey()
+
+ if userInput == 'q':
+ break
+ elif userInput not in keyDict:
+ print('Key not in valid key commands, try again')
+ else:
+ keyDict[userInput](self.servos[nSrv])
+ print('Servo %2i cmd: %4i'%(nSrv,self.servos[nSrv].value))
+ self.send_servo_msg()
+
+ elif userInput == 'allServos':
+ # Reset all servos to center value, and send command
+ self.reset_all_servos_center()
+ self.send_servo_msg()
+
+ print('Enter command, q to go back to option select: ')
+ while (1):
+
+ userInput = self.getKey()
+
+ if userInput == 'q':
+ break
+ elif userInput not in keyDict:
+ print('Key not in valid key commands, try again')
+ elif userInput in ('b','n','m'):
+ print('Saving values not supported in all servo control mode')
+ else:
+ for s in self.servos.values():
+ keyDict[userInput](s)
+ print('All Servos Commanded')
+ self.send_servo_msg()
+
+
+
+
+
+
+
+ # print self._last_time_cmd_rcv, self.is_controller_connected
+ # if not self.is_controller_connected:
+ # self.set_actuators_idle()
+
+ # Set the control rate in Hz
+ rate = rospy.Rate(10)
+ rate.sleep()
+
+if __name__ == "__main__":
+ smsc = SpotMicroServoControl()
+ smsc.run()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c60f1cce7edb4179136e6c923921a6fbd92516fe
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/CMakeLists.txt
@@ -0,0 +1,206 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(spot_micro_joy)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ joy
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES spot_micro_joy
+# CATKIN_DEPENDS joy rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/spot_micro_joy.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/spot_micro_joy_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_joy.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/launch/everything.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/launch/everything.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d77e968c347d0ab406de1fd23a102ce53a0d0efc
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/launch/everything.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+ <arg name="debug_mode" default="false"/>
+
+ <include file="$(find spot_micro_motion_cmd)/launch/motion_cmd.launch" >
+ <arg name="debug_mode" value="$(arg debug_mode)"/>
+ </include>
+
+ <node name="joy_node" pkg="joy" type="joy_node" output="screen">
+ </node>
+
+ <node name="spotMicroJoystickMove" pkg="spot_micro_joy" type="spotMicroJoystickMove.py" output="screen">
+ </node>
+
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b09bbb8514b573b9eadf18cc9ffcd1ca03f6fff4
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_joy</name>
+ <version>0.0.0</version>
+ <description>The spot_micro_joy package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/spot_micro_joy</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>joy</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_export_depend>joy</build_export_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>joy</exec_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/scripts/spotMicroJoystickMove.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/scripts/spotMicroJoystickMove.py
new file mode 100644
index 0000000000000000000000000000000000000000..834a28d0ad7ce65d41bb8127061459ad900cf6de
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/scripts/spotMicroJoystickMove.py
@@ -0,0 +1,159 @@
+#!/usr/bin/python
+
+import rospy
+from std_msgs.msg import Float32, Bool
+from sensor_msgs.msg import Joy
+from geometry_msgs.msg import Vector3
+from geometry_msgs.msg import Twist
+from math import pi
+
+
+class SpotMicroJoystickControl():
+ BUTTON_IDLE = 0
+ BUTTON_WALK = 1
+ BUTTON_STAND = 2
+ BUTTON_ANGLE = 3
+
+ ANGLE_AXES_ROLL = 0
+ ANGLE_AXES_HEIGHT = 1
+ ANGLE_AXES_YAW = 2
+ ANGLE_AXES_PITCH = 3
+
+ WALK_AXES_FORWARD = 1
+ WALK_AXES_STRAFE = 0
+ WALK_AXES_YAW = 2
+
+ MODE_IDLE = 0
+ MODE_STAND = 1
+ MODE_ANGLE = 2
+ MODE_WALK = 3
+
+ MAX_ROLL_DEG = 45
+ MAX_YAW_DEG = 45
+ MAX_PATCH_DEG = 45
+
+ MAX_FORWARD_SPEED = 0.05
+ MAX_STRAFE_SPEED = 0.05
+ MAX_YAW_SPEED_DEG = 15
+
+ def __init__(self):
+
+ self._angle_cmd_msg = Vector3()
+ self._angle_cmd_msg.x = 0
+ self._angle_cmd_msg.y = 0
+ self._angle_cmd_msg.z = 0
+
+ self._vel_cmd_msg = Twist()
+ self._vel_cmd_msg.linear.x = 0
+ self._vel_cmd_msg.linear.y = 0
+ self._vel_cmd_msg.linear.z = 0
+ self._vel_cmd_msg.angular.x = 0
+ self._vel_cmd_msg.angular.y = 0
+ self._vel_cmd_msg.angular.z = 0
+
+ self._walk_event_cmd_msg = Bool()
+ self._walk_event_cmd_msg.data = True # Mostly acts as an event driven action on receipt of a true message
+
+ self._stand_event_cmd_msg = Bool()
+ self._stand_event_cmd_msg.data = True
+
+ self._idle_event_cmd_msg = Bool()
+ self._idle_event_cmd_msg.data = True
+
+ rospy.loginfo("Setting Up the Spot Micro Joystick Control Node...")
+
+ # Set up and title the ros node for this code
+ rospy.init_node('spot_micro_joystick_control')
+
+ # Create publishers for commanding velocity, angle, and robot states
+ self._ros_pub_angle_cmd = rospy.Publisher('/angle_cmd', Vector3, queue_size=1)
+ self._ros_pub_vel_cmd = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+ self._ros_pub_walk_cmd = rospy.Publisher('/walk_cmd', Bool, queue_size=1)
+ self._ros_pub_stand_cmd = rospy.Publisher('/stand_cmd', Bool, queue_size=1)
+ self._ros_pub_idle_cmd = rospy.Publisher('/idle_cmd', Bool, queue_size=1)
+
+ rospy.loginfo("Joystick control node publishers corrrectly initialized")
+
+ def reset_all_motion_commands_to_zero(self):
+ '''Reset body motion cmd states to zero and publish zero value body motion commands'''
+
+ self._vel_cmd_msg.linear.x = 0
+ self._vel_cmd_msg.linear.y = 0
+ self._vel_cmd_msg.linear.z = 0
+ self._vel_cmd_msg.angular.x = 0
+ self._vel_cmd_msg.angular.y = 0
+ self._vel_cmd_msg.angular.z = 0
+
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ def reset_all_angle_commands_to_zero(self):
+ '''Reset angle cmd states to zero and publish them'''
+
+ self._angle_cmd_msg.x = 0
+ self._angle_cmd_msg.y = 0
+ self._angle_cmd_msg.z = 0
+
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ def on_joy(self, msg):
+ self.on_joy_buttons(msg.buttons)
+ self.on_joy_axes(msg.axes)
+
+ def on_joy_buttons(self, buttons):
+ if buttons[self.BUTTON_IDLE] == 1:
+ self._ros_pub_idle_cmd.publish(self._idle_event_cmd_msg)
+ rospy.loginfo('Idle command issued from joystick.')
+ self.mode = self.MODE_IDLE
+ elif buttons[self.BUTTON_STAND] == 1:
+ self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)
+ rospy.loginfo('Stand command issued from joystick.')
+ self.mode = self.MODE_STAND
+ elif buttons[self.BUTTON_ANGLE] == 1:
+ self.reset_all_angle_commands_to_zero()
+ rospy.loginfo('Entering joystick angle command mode.')
+ self.mode = self.MODE_ANGLE
+ elif buttons[self.BUTTON_WALK] == 1:
+ self.reset_all_angle_commands_to_zero()
+ self._ros_pub_walk_cmd.publish(self._walk_event_cmd_msg)
+ rospy.loginfo('Entering joystick walk command mode.')
+ self.mode = self.MODE_WALK
+
+ def on_joy_axes(self, axes):
+ if self.mode == self.MODE_ANGLE:
+ self.on_joy_angle_mode(axes)
+ elif self.mode == self.MODE_WALK:
+ self.on_joy_walk_mode(axes)
+
+ def on_joy_walk_mode(self, axes):
+ self._vel_cmd_msg.linear.x = axes[self.WALK_AXES_FORWARD] * self.MAX_FORWARD_SPEED
+ self._vel_cmd_msg.linear.y = axes[self.WALK_AXES_STRAFE] * self.MAX_STRAFE_SPEED
+ self._vel_cmd_msg.angular.z = pi / 180 * axes[self.WALK_AXES_YAW] * self.MAX_YAW_SPEED_DEG
+ print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s ' \
+ % (self._vel_cmd_msg.linear.x, self._vel_cmd_msg.linear.y, self._vel_cmd_msg.angular.z * 180 / pi))
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ def on_joy_angle_mode(self, axes):
+ self._angle_cmd_msg.x = pi / 180 * axes[self.ANGLE_AXES_ROLL] * self.MAX_ROLL_DEG * -1
+ self._angle_cmd_msg.y = pi / 180 * axes[self.ANGLE_AXES_PITCH] * self.MAX_PATCH_DEG * -1
+ self._angle_cmd_msg.z = pi / 180 * axes[self.ANGLE_AXES_YAW] * self.MAX_YAW_DEG
+ print('Cmd Values: phi: %1.3f deg, theta: %1.3f deg, psi: %1.3f deg ' \
+ % (
+ self._angle_cmd_msg.x * 180 / pi, self._angle_cmd_msg.y * 180 / pi,
+ self._angle_cmd_msg.z * 180 / pi))
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ def run(self):
+ print("green = idle")
+ print("yellow = stand")
+ print("blue = angle")
+ print("red = walk")
+
+ # Publish all body motion commands to 0
+ self.reset_all_motion_commands_to_zero()
+ rospy.Subscriber("/joy", Joy, self.on_joy)
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ smjc = SpotMicroJoystickControl()
+ smjc.run()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..29689dd6c263a301bb162cf1f3baa5c5eec8148f
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/CMakeLists.txt
@@ -0,0 +1,220 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spot_micro_keyboard_command)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ roscpp
+ std_msgs
+ geometry_msgs
+)
+
+
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES spot_micro_keyboard_command
+# CATKIN_DEPENDS rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ ${PROJECT_SOURCE_DIR}/include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/spot_micro_keyboard_command.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+#add_executable(${PROJECT_NAME}_node src/spot_micro_keyboard_command_node.cpp)
+add_executable(move ~/3rdYear/RWR/Robot-With-Reflexes/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp)
+
+add_dependencies(move
+ ${${PROJECT_NAME}_EXPORTED_TARGETS}
+ ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(move
+ ${catkin_LIBRARIES}
+)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_keyboard_command.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/launch/keyboard_command.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/launch/keyboard_command.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9442cacdcc8db52c503f64a8c951629e47f7721f
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/launch/keyboard_command.launch
@@ -0,0 +1,31 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Launch keyboard command node and, optionally, other nodes via command line arguments -->
+
+<launch>
+ <!-- Optional command line argument to additionally run python plotting node -->
+ <!-- Useful for running or debugging program on a PC instead of a rpi -->
+ <arg name="run_plot" default="false"/>
+
+ <!-- Optional command line argument to also run lcd monitor node on rpi --> -->
+ <arg name="run_rviz" default="true"/>
+
+
+ <!-- Run the keyboard command node-->
+ <node name="spot_micro_keyboard_command_node" pkg="spot_micro_keyboard_command" type="spotMicroKeyboardMove.py" output="screen">
+ </node>
+
+ <!-- Run the hard left node -->
+ <node name="postObjDetMovement" pkg="spot_micro_keyboard_command" type="postObjDetect.py" output="screen">
+ </node>
+
+ <!-- If run_plot is true, run the plotting node by including it's launch file -->
+ <group if="$(arg run_plot)">
+ <include file="$(find spot_micro_plot)/launch/start_plotting.launch" />
+ </group>
+
+ <!-- If run_rviz is true, run the rviz node by including it's launch file -->
+ <group if="$(arg run_rviz)">
+ <include file="$(find spot_micro_rviz)/launch/show_model.launch" />
+ </group>
+
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7222ba4a1f467de4d418da488f4daa231127738
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp
@@ -0,0 +1,26 @@
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h>
+#include <std_msgs/Bool.h>
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "move_publisher");
+ ros::NodeHandle nh;
+ ros::Publisher stand_pub = nh.advertise<std_msgs::Bool>("/stand_cmd", 1000);
+ ros::Publisher walk_pub = nh.advertise<std_msgs::Bool>("/walk_cmd",1000);
+
+ //ros::Rate loop_rate(10);
+ //int count = 0;
+
+ std_msgs::Bool stand_msg;
+ std_msgs::Bool walk_msg;
+ stand_msg.data = true;
+ walk_msg.data = true;
+
+ stand_pub.publish(stand_msg);
+ //walk_pub.publish(walk_msg);
+
+ //ros::spinOnce();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..22c0e0aec055ad19e4d40f0ce675ce93691237fe
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_keyboard_command</name>
+ <version>0.0.0</version>
+ <description>The spot_micro_keyboard_command package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/spot_micro_keyboard_command</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/postObjDetect.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/postObjDetect.py
new file mode 100644
index 0000000000000000000000000000000000000000..deb6672fdb51fd3248c2589233e980e24b098f11
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/postObjDetect.py
@@ -0,0 +1,29 @@
+#!/usr/bin/python3
+
+import rospy
+import sys, select, termios, tty # For terminal keyboard key press reading
+from std_msgs.msg import Bool
+from geometry_msgs.msg import Vector3
+from geometry_msgs.msg import Twist
+from math import pi
+
+def stand_walk():
+ inputArgs = Twist()
+
+ stand_msg = Bool()
+ stand_msg.data = True
+ walk_msg = Bool()
+ walk_msg.data = True
+
+ standPub = rospy.Publisher('/stand_cmd',Bool,queue_size=1)
+ walkPub = rospy.Publish('/walk_cmd',Bool, queue_size=1)
+
+ standPub.publish(stand_msg)
+ walkPub.publish(walk_msg)
+
+ rospy.loginfo("issued obj detected - transitioning to walk mode")
+
+
+
+if __name__ == '__main__':
+ stand_walk()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py
new file mode 100755
index 0000000000000000000000000000000000000000..25f0218ce02461dc6e4a343745110854ac81be4c
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py
@@ -0,0 +1,272 @@
+#!/usr/bin/python3
+
+"""
+Class for sending keyboard commands to spot micro walk node, control velocity, yaw rate, and walk event
+"""
+import rospy
+import sys, select, termios, tty # For terminal keyboard key press reading
+from std_msgs.msg import Float32, Bool
+from geometry_msgs.msg import Vector3
+from geometry_msgs.msg import Twist
+from math import pi
+
+msg = """
+Spot Micro Walk Command
+
+Enter one of the following options:
+-----------------------------
+quit: stop and quit the program
+walk: Start walk mode and keyboard motion control
+stand: Stand robot up
+idle: Lay robot down
+angle_cmd: enter angle control mode
+
+Keyboard commands for body motion
+---------------------------
+ q w e u
+ a s d
+
+
+ u: Quit body motion command mode and go back to rest mode
+ w: Increment forward speed command / decrease pitch angle
+ a: Increment left speed command / left roll angle
+ s: Increment backward speed command / increase pitch angle
+ d: Increment right speed command / right roll angle
+ q: Increment body yaw rate command / left yaw angle (negative left, positive right)
+ e: Increment body yaw rate command / right yaw angle (negative left, positive right)
+ f: In walk mode, zero out all rate commands.
+
+ anything else : Prompt again for command
+
+
+CTRL-C to quit
+"""
+valid_cmds = ('quit','Quit','walk','stand','idle', 'angle_cmd', 'obj')
+
+# Global body motion increment values
+speed_inc = 0.02
+yaw_rate_inc = 3*pi/180
+angle_inc = 2.5*pi/180
+
+class SpotMicroKeyboardControl():
+ def __init__(self):
+
+ self._angle_cmd_msg = Vector3()
+ self._angle_cmd_msg.x = 0
+ self._angle_cmd_msg.y = 0
+ self._angle_cmd_msg.z = 0
+
+ self._vel_cmd_msg = Twist()
+ self._vel_cmd_msg.linear.x = 0
+ self._vel_cmd_msg.linear.y = 0
+ self._vel_cmd_msg.linear.z = 0
+ self._vel_cmd_msg.angular.x = 0
+ self._vel_cmd_msg.angular.y = 0
+ self._vel_cmd_msg.angular.z = 0
+
+ self._walk_event_cmd_msg = Bool()
+ self._walk_event_cmd_msg.data = True # Mostly acts as an event driven action on receipt of a true message
+
+ self._stand_event_cmd_msg = Bool()
+ self._stand_event_cmd_msg.data = True
+
+ self._idle_event_cmd_msg = Bool()
+ self._idle_event_cmd_msg.data = True
+
+ self._object_detected_msg = Bool()
+ self._object_detected_msg = False
+
+ rospy.loginfo("Setting Up the Spot Micro Keyboard Control Node...")
+
+ # Set up and title the ros node for this code
+ rospy.init_node('spot_micro_keyboard_control')
+
+ # Create publishers for commanding velocity, angle, and robot states
+ self._ros_pub_angle_cmd = rospy.Publisher('/angle_cmd',Vector3,queue_size=1)
+ self._ros_pub_vel_cmd = rospy.Publisher('/cmd_vel',Twist,queue_size=1)
+ self._ros_pub_walk_cmd = rospy.Publisher('/walk_cmd',Bool, queue_size=1)
+ self._ros_pub_stand_cmd = rospy.Publisher('/stand_cmd',Bool,queue_size=1)
+ self._ros_pub_idle_cmd = rospy.Publisher('/idle_cmd',Bool,queue_size=1)
+ self._object_detected_cmd = rospy.Publisher('/obj_det',Bool, queue_size=1)
+
+ rospy.loginfo("Keyboard control node publishers corrrectly initialized")
+
+ # Setup terminal input reading, taken from teleop_twist_keyboard
+ self.settings = termios.tcgetattr(sys.stdin)
+
+ rospy.loginfo("Keyboard control node initialization complete")
+
+ def reset_all_motion_commands_to_zero(self):
+ '''Reset body motion cmd states to zero and publish zero value body motion commands'''
+
+ self._vel_cmd_msg.linear.x = 0
+ self._vel_cmd_msg.linear.y = 0
+ self._vel_cmd_msg.linear.z = 0
+ self._vel_cmd_msg.angular.x = 0
+ self._vel_cmd_msg.angular.y = 0
+ self._vel_cmd_msg.angular.z = 0
+
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ def reset_all_angle_commands_to_zero(self):
+ '''Reset angle cmd states to zero and publish them'''
+
+ self._angle_cmd_msg.x = 0
+ self._angle_cmd_msg.y = 0
+ self._angle_cmd_msg.z = 0
+
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ def getKey(self):
+ tty.setraw(sys.stdin.fileno())
+ select.select([sys.stdin], [], [], 0)
+ key = sys.stdin.read(1)
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ return key
+
+ def run(self):
+ # Publish all body motion commands to 0
+ self.reset_all_motion_commands_to_zero()
+ rospy.loginfo('Main keyboard control loop started.')
+
+ # Main loop
+ while not rospy.is_shutdown():
+ # Prompt user with keyboard command information, and wait for input keystroke
+ #remove the object detected param
+ #self._object_detected_msg = False
+
+ print(msg)
+ userInput = input("Command?: ")
+
+ if userInput not in valid_cmds:
+ rospy.logwarn('Invalid keyboard command entered: %s', userInput)
+ else:
+ if userInput == 'quit':
+ rospy.loginfo("Exiting keyboard control node...")
+ break
+
+ elif userInput == 'stand':
+ #Publish stand command event
+ self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)
+ rospy.loginfo('Stand command issued from keyboard.')
+
+ elif userInput == 'idle':
+ #Publish idle command event
+ self._ros_pub_idle_cmd.publish(self._idle_event_cmd_msg)
+ self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)
+ rospy.loginfo('Idle command issued from keyboard.')
+
+ elif userInput == 'obj':
+ self._object_detected_msg = True
+ self._object_detected_cmd.publish(self._object_detected_msg)
+ rospy.loginfo('obj command issued')
+ #python
+
+ elif userInput == 'angle_cmd':
+ # Reset all angle commands
+ self.reset_all_angle_commands_to_zero()
+ rospy.loginfo('Entering keyboard angle command mode.')
+
+ # Enter loop to act on user command
+ print('Enter command, u to go back to command select: ')
+
+ while (1):
+ print('Cmd Values: phi: %1.3f deg, theta: %1.3f deg, psi: %1.3f deg '\
+ %(self._angle_cmd_msg.x*180/pi, self._angle_cmd_msg.y*180/pi, self._angle_cmd_msg.z*180/pi))
+
+ userInput = self.getKey()
+
+ if userInput == 'u':
+ # Break out of angle command mode
+ break
+
+ elif userInput not in ('w','a','s','d','q','e','u'):
+ rospy.logwarn('Invalid keyboard command issued in angle command mode: %s', userInput)
+ else:
+ if userInput == 'w':
+ self._angle_cmd_msg.y = self._angle_cmd_msg.y - angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 's':
+ self._angle_cmd_msg.y = self._angle_cmd_msg.y + angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 'q':
+ self._angle_cmd_msg.z = self._angle_cmd_msg.z + angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 'e':
+ self._angle_cmd_msg.z = self._angle_cmd_msg.z - angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 'a':
+ self._angle_cmd_msg.x = self._angle_cmd_msg.x - angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 'd':
+ self._angle_cmd_msg.x = self._angle_cmd_msg.x + angle_inc
+ self._ros_pub_angle_cmd.publish(self._angle_cmd_msg)
+
+ elif userInput == 'walk':
+ # Reset all body motion commands to zero
+ self.reset_all_motion_commands_to_zero()
+
+ # Publish walk event
+ self._ros_pub_walk_cmd.publish(self._walk_event_cmd_msg)
+ rospy.loginfo('Walk command issued from keyboard.')
+
+ # Prompt user with info and enter loop to act on user command
+ print('Enter command, u to go back to stand mode: ')
+
+ while (1):
+ print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s '\
+ %(self._vel_cmd_msg.linear.x,self._vel_cmd_msg.linear.y,self._vel_cmd_msg.angular.z*180/pi))
+
+ userInput = self.getKey()
+
+ if userInput == 'u':
+ # Send stand event message, this will take robot back to standing mode
+ self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg)
+ rospy.loginfo('Stand command issued from keyboard.')
+ break
+
+ elif userInput not in ('w','a','s','d','q','e','u','f'):
+ print('Key not in valid key commands, try again')
+ rospy.logwarn('Invalid keyboard command issued in walk mode: %s', userInput)
+ else:
+ if userInput == 'w':
+ self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x + speed_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 's':
+ self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x - speed_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 'a':
+ self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y - speed_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 'd':
+ self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y + speed_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 'q':
+ self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z - yaw_rate_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 'e':
+ self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z + yaw_rate_inc
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+
+ elif userInput == 'f':
+ self._vel_cmd_msg.linear.x = 0
+ self._vel_cmd_msg.linear.y = 0
+ self._vel_cmd_msg.angular.z = 0
+
+ self._ros_pub_vel_cmd.publish(self._vel_cmd_msg)
+ rospy.loginfo('Command issued to zero all rate commands.')
+
+
+if __name__ == "__main__":
+ smkc = SpotMicroKeyboardControl()
+ smkc.run()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a6f1916e60d8348831d347e0333f07f6bcdab01d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/CMakeLists.txt
@@ -0,0 +1,212 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(spot_micro_launch)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ hector_geotiff
+ hector_geotiff_plugins
+ hector_map_server
+ hector_mapping
+ hector_trajectory_server
+ rviz
+ tf
+ tf2
+ topic_tools
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES spot_micro_launch
+# CATKIN_DEPENDS hector_geotiff hector_geotiff_plugins hector_map_server hector_mapping hector_trajectory_server rviz tf tf2 topic_tools
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/spot_micro_launch.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/spot_micro_launch_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_launch.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/keyboard_control_and_rviz.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/keyboard_control_and_rviz.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d06c36ea8fad2c12987e98e10fbef919918452b6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/keyboard_control_and_rviz.launch
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+
+<launch>
+
+ <!-- Argument for running rviz with SLAM visualization-->
+ <arg name="rviz_slam" default="false"/>
+
+ <!-- Run the keyboard command node-->
+ <include file="$(find spot_micro_keyboard_command)/launch/keyboard_command.launch" />
+
+ <!-- Launch spot micro motion command and servo conrol nodes -->
+ <!-- If run_rviz is true, run the rviz node by including it's launch file -->
+ <group if="$(arg rviz_slam)">
+ <include file="$(find spot_micro_rviz)/launch/slam.launch" />
+ </group>
+
+ <!-- If run_rviz is true, run the rviz node by including it's launch file -->
+ <group unless="$(arg rviz_slam)">
+ <include file="$(find spot_micro_rviz)/launch/show_model.launch" />
+ </group>
+
+</launch>
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/motion_control_and_hector_slam.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/motion_control_and_hector_slam.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9669fc940a6d8d5e5fdd1276e835aea861f49a70
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/motion_control_and_hector_slam.launch
@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+
+<launch>
+
+ <!-- Argument for running post processing mode. If run_post_proc is true, -->
+ <!-- then only hector_mapping node is run. -->
+ <arg name="run_post_proc" default="false"/>
+
+ <!-- Launch spot micro motion command and rplidar nodes, only if -->
+ <!-- not in post processing mode -->
+ <group unless="$(arg run_post_proc)">
+ <!-- Spot micro motion command node, also runs servo control node -->
+ <include file="$(find spot_micro_motion_cmd)/launch/motion_cmd.launch" />
+
+
+ <!-- Launch rplidar node for lidar driver -->
+ <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
+ <param name="serial_port" type="string" value="/dev/ttyUSB0"/>
+ <param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 -->
+ <!--param name="serial_baudrate" type="int" value="256000"--><!--A3 -->
+ <param name="frame_id" type="string" value="lidar_link"/>
+ <param name="inverted" type="bool" value="false"/>
+ <param name="angle_compensate" type="bool" value="true"/>
+ </node>
+ </group>
+
+
+ <!-- Launch hector mapping node for SLAM -->
+ <arg name="geotiff_map_file_path" default="$(find hector_geotiff)/maps"/>
+ <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
+ <!-- Frame names -->
+ <param name="map_frame" value="map" />
+ <param name="base_frame" value="base_footprint" />
+ <param name="odom_frame" value="odom" />
+
+ <!-- Tf use -->
+ <param name="use_tf_scan_transformation" value="true"/>
+ <param name="use_tf_pose_start_estimate" value="false"/>
+ <param name="pub_map_odom_transform" value="true"/>
+
+ <!-- Map size / start point -->
+ <param name="map_resolution" value="0.050"/>
+ <param name="map_size" value="2048"/>
+ <param name="map_start_x" value="0.5"/>
+ <param name="map_start_y" value="0.5" />
+ <param name="map_multi_res_levels" value="2" />
+
+ <!-- Map update parameters -->
+ <param name="update_factor_free" value="0.4"/>
+ <param name="update_factor_occupied" value="0.9" />
+ <param name="map_update_distance_thresh" value="0.4"/>
+ <param name="map_update_angle_thresh" value="0.06" />
+ <param name="laser_z_min_value" value = "-1.0" />
+ <param name="laser_z_max_value" value = "1.0" />
+
+ <!-- Advertising config -->
+ <param name="advertise_map_service" value="true"/>
+
+ <param name="scan_subscriber_queue_size" value="5"/>
+ <param name="scan_topic" value="scan"/>
+
+ <!-- Debug parameters -->
+ <!--
+ <param name="output_timing" value="false"/>
+ <param name="pub_drawings" value="true"/>
+ <param name="pub_debug_output" value="true"/>
+ -->
+ <param name="tf_map_scanmatch_transform_frame_name" value="scanmatcher_frame" />
+ </node>
+
+<!-- Optional hector geotiff mapper node for creating geotiff map output -->
+<!-- This does not create a map in a format useable for navigation, but only an image -->
+ <!-- <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
+ <arg name="trajectory_source_frame_name" value="scanmatcher_frame"/>
+ <arg name="map_file_path" value="$(arg geotiff_map_file_path)"/>
+ </include> -->
+
+</launch>
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1e26096dd95a74e31567a86d78c4fdd1deba390a
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/package.xml
@@ -0,0 +1,89 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_launch</name>
+ <version>0.0.0</version>
+ <description>The spot_micro_launch package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/spot_micro_launch</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>hector_geotiff</build_depend>
+ <build_depend>hector_geotiff_plugins</build_depend>
+ <build_depend>hector_map_server</build_depend>
+ <build_depend>hector_mapping</build_depend>
+ <build_depend>hector_trajectory_server</build_depend>
+ <build_depend>rviz</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>tf2</build_depend>
+ <build_depend>topic_tools</build_depend>
+ <build_depend>rplidar_ros</build_depend>
+ <build_export_depend>hector_geotiff</build_export_depend>
+ <build_export_depend>hector_geotiff_plugins</build_export_depend>
+ <build_export_depend>hector_map_server</build_export_depend>
+ <build_export_depend>hector_mapping</build_export_depend>
+ <build_export_depend>hector_trajectory_server</build_export_depend>
+ <build_export_depend>rviz</build_export_depend>
+ <build_export_depend>tf</build_export_depend>
+ <build_export_depend>tf2</build_export_depend>
+ <build_export_depend>topic_tools</build_export_depend>
+ <build_export_depend>rplidar_ros</build_export_depend>
+ <exec_depend>hector_geotiff</exec_depend>
+ <exec_depend>hector_geotiff_plugins</exec_depend>
+ <exec_depend>hector_map_server</exec_depend>
+ <exec_depend>hector_mapping</exec_depend>
+ <exec_depend>hector_trajectory_server</exec_depend>
+ <exec_depend>rviz</exec_depend>
+ <exec_depend>tf</exec_depend>
+ <exec_depend>tf2</exec_depend>
+ <exec_depend>topic_tools</exec_depend>
+ <exec_depend>rplidar_ros</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e0f10a5cabfba064e8f73d3a70aa75d925269e27
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/CMakeLists.txt
@@ -0,0 +1,227 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spot_micro_motion_cmd)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ std_msgs
+ geometry_msgs
+ i2cpwm_board
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+)
+
+add_subdirectory(libs/spot_micro_kinematics_cpp)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include/spot_micro_motion_cmd
+# LIBRARIES spot_micro_motion_cmd
+ CATKIN_DEPENDS roscpp std_msgs geometry_msgs tf2 tf2_ros
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include/spot_micro_motion_cmd
+ src/smfsm
+ src/rate_limited_first_order_filter
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(smfsm
+ src/smfsm/spot_micro_state.cpp
+ src/smfsm/spot_micro_idle.cpp
+ src/smfsm/spot_micro_stand.cpp
+ src/smfsm/spot_micro_object.cpp
+ src/smfsm/spot_micro_transition_stand.cpp
+ src/smfsm/spot_micro_transition_idle.cpp
+ src/smfsm/spot_micro_walk.cpp
+)
+
+target_link_libraries(smfsm
+ spot_micro_kinematics
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/spot_micro_motion_cmd.cpp src/spot_micro_motion_cmd_node.cpp src/utils.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}_node
+ ${catkin_LIBRARIES}
+ smfsm
+ spot_micro_kinematics
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_motion_cmd.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c7238c572a40d52f6bd3d41393134c93eec7d7a0
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml
@@ -0,0 +1,109 @@
+# Configuration parameters
+
+# Robot wireframe size parameters
+# Lengths representening a wireframe model of the robot. All lengths joint to joint
+hip_link_length: 0.055 # Straight line distance of the hip link (horizontal leg link)
+upper_leg_link_length: 0.1075 # Straight line distance of the upper leg link, joint to joint
+lower_leg_link_length: 0.130 # Straight line distance of the lower leg link, joint to joint
+body_width: 0.078 # Horizontal width between hip joints
+body_length: 0.186 # Length between shoulder joints
+
+# Stance parameters
+default_stand_height: 0.155 # Height of robot body center when standing
+stand_front_x_offset: 0.015 # Fwb/back offset of front two legs from default stance
+stand_back_x_offset: -0.000 # Fwd/back offset of back two legs from default stance
+#stand_front_x_offset: -0.010 # Offset better tuned for trot gait
+#stand_back_x_offset: -0.010 # Offset better tuned for trot gait
+lie_down_height: 0.083 # Height of body center when sitting
+lie_down_foot_x_offset: 0.065 # Fwd/back offset of all(?) feet from default stance when sitting
+
+# Servo parameters
+num_servos: 12
+servo_max_angle_deg: 82.5
+RF_3: {num: 1, center: 306,range: 372,direction: 1, center_angle_deg: 88.2}
+RF_2: {num: 2, center: 306,range: 389,direction: 1, center_angle_deg: -27.6}
+RF_1: {num: 3, center: 306,range: 396,direction: -1, center_angle_deg: -5.4}
+RB_3: {num: 4, center: 306,range: 396,direction: 1, center_angle_deg: 85.8}
+RB_2: {num: 5, center: 306,range: 396,direction: 1, center_angle_deg: -35.4}
+RB_1: {num: 6, center: 306,range: 411,direction: 1, center_angle_deg: -4.4}
+LB_3: {num: 7, center: 306,range: 390,direction: 1, center_angle_deg: -73.9}
+LB_2: {num: 8, center: 306,range: 392,direction: 1, center_angle_deg: 38.7}
+LB_1: {num: 9, center: 306,range: 374,direction: -1, center_angle_deg: -0.4}
+LF_3: {num: 10, center: 306,range: 387,direction: 1, center_angle_deg: -82.8}
+LF_2: {num: 11, center: 306,range: 397,direction: 1, center_angle_deg: 38.6}
+LF_1: {num: 12, center: 306,range: 389,direction: 1, center_angle_deg: -7.6}
+
+# Control Parameters
+transit_tau: 0.3 # Time constant in seconds for first order filters during transition state
+transit_rl: 0.06 # Body motion rate limit in m/s for transition state
+transit_angle_rl: 0.35 # Body motion angular rate limit in rad/s for transition state
+max_fwd_velocity: 0.4
+max_side_velocity: 0.4
+max_yaw_rate: 0.35
+
+# Gait parameters
+z_clearance: 0.050 # Max height of foot during swing phase. Increase to have feet go
+ # higher during swing. Feet travel in a triangular motion during swing
+alpha: 0.5 # Controls whether feet are centered within fwd/back motion
+beta: 0.5 # Controls whether feet are centered within side to side motion
+foot_height_time_constant: 0.02
+
+##################
+# GAIT SELECTION #
+##################
+# Uncomment one set of gait parameters for the desired
+# gait to use, and comment out the other set.
+
+############################################
+############## 8 Phase Gait ################
+############################################
+# Only one leg is ever in swing phase at one time
+# 4 swing phases + 4 body motion phases to balance body over
+# three legs that will remain in contact with ground
+num_phases: 8
+rb_contact_phases: [1, 0, 1, 1, 1, 1, 1, 1]
+rf_contact_phases: [1, 1, 1, 0, 1, 1, 1, 1]
+lf_contact_phases: [1, 1, 1, 1, 1, 1, 1, 0]
+lb_contact_phases: [1, 1, 1, 1, 1, 0, 1, 1]
+overlap_time: 0.0
+swing_time: 0.36
+body_shift_phases: [1, 2, 3, 4, 5, 6, 7, 8]
+fwd_body_balance_shift: 0.035 # Amount the body center will shift forward when walking
+back_body_balance_shift: 0.005 # Amount the body center will shift backward when walking
+side_body_balance_shift: 0.015 # Amount the body center will shift side to side when walking
+
+#################################################
+############## 4 Phase Trot Gait ################
+#################################################
+# 4 Phase gait where the two diagonally oposite legs move in
+# tandem together during swing phase
+# 2 swing phases + 2 phases where all four feet are on the ground
+# num_phases: 4
+# rb_contact_phases: [1, 0, 1, 1]
+# rf_contact_phases: [1, 1, 1, 0]
+# lf_contact_phases: [1, 0, 1, 1]
+# lb_contact_phases: [1, 1, 1, 0]
+# overlap_time: 0.18
+# swing_time: 0.18
+# body_shift_phases: [0,0,0,0]
+# fwd_body_balance_shift: 0
+# back_body_balance_shift: 0
+# side_body_balance_shift: 0
+
+# TF Parameter
+lidar_x_pos: 0.045 # X offset of lidar coordinate frame center from robot body center
+lidar_y_pos: 0.0 # Y offset of lidar coordinate frame center from robot body center
+lidar_z_pos: 0.085 # Z offset of lidar coordinate frame center from robot body center
+lidar_yaw_angle: 180 # Angle in degrees of yaw angle of the mounted lidar. Pitch and roll are assumed to be 0
+
+# Node parameters
+dt: 0.02 # Loop time of main loop, seconds (0.02 s is 50 hz)
+publish_odom: true # Determines whether odometry is published or not
+
+# Debug mode
+debug_mode: false # Prints out additional information
+plot_mode: false # Publishes data for the python plotting node to work
+
+# Standalone mode
+standalone_mode: false # Enables node to run without i2cpwnboard
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/data/usage.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/data/usage.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e6c4bfaa06087554780648a15a209c847b8f1a65
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/data/usage.txt
@@ -0,0 +1 @@
+This folder is to store the data related to the program
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h
new file mode 100644
index 0000000000000000000000000000000000000000..5add6c21f9f7b8c8aeb57f31eac31aaa24898f6f
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h
@@ -0,0 +1,235 @@
+#pragma once //designed to include the current source file only once in a single compilation.
+#ifndef SPOT_MICRO_MOTION_CMD //usd for conditional compiling.
+#define SPOT_MICRO_MOTION_CMD
+
+#include <ros/ros.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include "std_msgs/Bool.h"
+#include "std_msgs/String.h"
+#include "geometry_msgs/Vector3.h"
+#include "geometry_msgs/Twist.h"
+#include "std_msgs/Float32MultiArray.h"
+#include "i2cpwm_board/Servo.h"
+#include "i2cpwm_board/ServoArray.h"
+
+#include "command.h"
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+#include "spot_micro_state.h"
+
+// Define a configuration struct
+// To hold configuration parameters from parameter server/config file
+struct SpotMicroNodeConfig {
+ smk::SpotMicroConfig smc;
+ float default_stand_height;
+ float stand_front_x_offset;
+ float stand_back_x_offset;
+ float lie_down_height;
+ float lie_down_feet_x_offset;
+ int num_servos;
+ float servo_max_angle_deg;
+ std::map<std::string, std::map<std::string, float>> servo_config;
+ float dt;
+ float transit_tau;
+ float transit_rl;
+ float transit_angle_rl;
+ bool debug_mode;
+ bool run_standalone;
+ bool plot_mode;
+ float max_fwd_velocity;
+ float max_side_velocity;
+ float max_yaw_rate;
+ float z_clearance;
+ float alpha;
+ float beta;
+ int num_phases;
+ std::vector<int> rb_contact_phases;
+ std::vector<int> rf_contact_phases;
+ std::vector<int> lf_contact_phases;
+ std::vector<int> lb_contact_phases;
+ float overlap_time;
+ float swing_time;
+ int overlap_ticks;
+ int swing_ticks;
+ int stance_ticks;
+ std::vector<int> phase_ticks;
+ int phase_length;
+ float foot_height_time_constant;
+ std::vector<int> body_shift_phases;
+ float fwd_body_balance_shift;
+ float side_body_balance_shift;
+ float back_body_balance_shift;
+ bool publish_odom;
+ float lidar_x_pos;
+ float lidar_y_pos;
+ float lidar_z_pos;
+ float lidar_yaw_angle;
+};
+
+
+/* defining the class */
+class SpotMicroMotionCmd
+{
+ public:
+ // Constructor
+ SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh);
+
+ // Destructor
+ ~SpotMicroMotionCmd();
+
+ // Main loop runner, called periodically at the loop rate
+ void runOnce();
+
+ // Publish a servo configuration message
+ bool publishServoConfiguration();
+
+ // Set servo proprotional message data
+ void setServoCommandMessageData();
+
+ // Publishes a servo proportional command message
+ void publishServoProportionalCommand();
+
+ // Publishes a servo absolute command message with all servos set to a command
+ // value of 0. This effectively disables the servos (stops them from holding
+ // position, should just freewheel)
+ void publishZeroServoAbsoluteCommand();
+
+ // Returns the loaded parameters
+ SpotMicroNodeConfig getNodeConfig();
+
+ // Returns leg positions representing a neutral stance
+ smk::LegsFootPos getNeutralStance();
+
+ // Returns leg positions representing a lieing down stance
+ smk::LegsFootPos getLieDownStance();
+
+ // Manually override and command idle mode, used for shutdown
+ void commandIdle();
+
+ // Returns current state name
+ std::string getCurrentStateName();
+
+ private:
+ // Declare SpotMicroState a friend so it can access and modify private
+ // members of this class
+ friend class SpotMicroState;
+
+ // Pointer to state object
+ std::unique_ptr<SpotMicroState> state_;
+
+ // Command object for encapsulating external commands
+ Command cmd_; // Command object, encapsulate commands
+
+ // Spot Micro Kinematics object. Holds kinematic state of robot, and holds
+ // kinematics operations for setting position/orientation of the robot
+ smk::SpotMicroKinematics sm_;
+
+ // Spot Micro Node Config object
+ SpotMicroNodeConfig smnc_;
+
+ // Holds the body state to be commanded: feet position, body position and
+ // angles
+ smk::BodyState body_state_cmd_;
+
+ // Odometry of the robot position and orientation based on integrated rate
+ // commands. Only x and y position, and yaw angle, will be integrated from
+ // rate commands
+ smk::BodyState robot_odometry_;
+
+ // Map to hold servo command values in radians
+ std::map<std::string, float> servo_cmds_rad_ = { {"RF_3", 0.0f}, {"RF_2", 0.0f}, {"RF_1", 0.0f},
+ {"RB_3", 0.0f}, {"RB_2", 0.0f}, {"RB_1", 0.0f},
+ {"LB_3", 0.0f}, {"LB_2", 0.0f}, {"LB_1", 0.0f},
+ {"LF_3", 0.0f}, {"LF_2", 0.0f}, {"LF_1", 0.0f} };
+
+ // Reads parameters from parameter server to initialize spot micro node config
+ // struct
+ void readInConfigParameters();
+
+ // Servo array message for servo proportional command
+ i2cpwm_board::ServoArray servo_array_;
+
+ // Servo array message for servo absolute command
+ i2cpwm_board::ServoArray servo_array_absolute_;
+
+
+ // ROS publisher and subscriber handles
+ ros::NodeHandle nh_; // Defining the ros NodeHandle variable for registrating the same with the master
+ ros::NodeHandle pnh_; // Private version of node handle
+ ros::Subscriber stand_sub_; // ros subscriber handle for stand_cmd topic
+ ros::Subscriber idle_sub_; // ros subscriber handle for idle_cmd topic
+ ros::Subscriber walk_sub_;
+ ros::Subscriber obj_det_sub_;
+ ros::Subscriber vel_cmd_sub_;
+ ros::Subscriber body_angle_cmd_sub_;
+ ros::Publisher servos_absolute_pub_;
+ ros::Publisher servos_proportional_pub_;
+ ros::Publisher body_state_pub_;
+ ros::Publisher lcd_vel_cmd_pub_;
+ ros::Publisher lcd_angle_cmd_pub_;
+ ros::Publisher lcd_state_pub_;
+ ros::ServiceClient servos_config_client_;
+ tf2_ros::TransformBroadcaster transform_br_;
+ tf2_ros::StaticTransformBroadcaster static_transform_br_;
+
+ // Message for encapsulating robot body state
+ std_msgs::Float32MultiArray body_state_msg_;
+
+ // Messages to hold robot state information for displaying on LCD monitor
+ // and for any other monitoring purposes.
+ std_msgs::String lcd_state_string_msg_;
+ geometry_msgs::Twist lcd_vel_cmd_msg_;
+ geometry_msgs::Vector3 lcd_angle_cmd_msg_;
+
+ // Callback method for stand command
+ void standCommandCallback(const std_msgs::Bool::ConstPtr& msg);
+
+ // Callback method for idle command
+ void idleCommandCallback(const std_msgs::Bool::ConstPtr& msg);
+
+ // Callback method for walk command
+ void walkCommandCallback(const std_msgs::Bool::ConstPtr& msg);
+
+ // Callback method for angle command
+ void angleCommandCallback(const geometry_msgs::Vector3ConstPtr& msg);
+
+ // Callback method for velocity command
+ // Currently, the only supported commands from this message are
+ // x and y axis linear velocity, and z axis angular rate
+ void velCommandCallback(const geometry_msgs::TwistConstPtr& msg);
+
+ // Resets all events if they were true
+ void resetEventCommands();
+
+ // State Machine Related Methods
+ // Handle input commands, delegate to state machine
+ void handleInputCommands();
+
+ // Changes state of the state machine
+ void changeState(std::unique_ptr<SpotMicroState> sms);
+
+ // Publishes body state as a float array for plotting by another node
+ void publishBodyState();
+
+ // Publish LCD monitor messages
+ void publishLcdMonitorData();
+
+ // Broadcast static tf2 coordinate frame transformation to /tf_static
+ // Should only be called once at initalization, as it's only for static
+ // transformations of the robot model that do not change over time
+ void publishStaticTransforms();
+
+ // Broadcast dynamic tf2 coordinate frame transformations to /tf
+ // Will broadcast dynamic robot and leg joint transformations
+ void publishDynamicTransforms();
+
+ // Integrate robot odometry. The robot doesn't actually have any
+ // sensed odometry, but an open loop estimate derived from velocity
+ // commands should still be useful
+ void integrateOdometry();
+
+ // Calculates the robot odometry coordinate frame
+ Eigen::Affine3d getOdometryTransform();
+
+};
+#endif
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..87fb6f310971d3f76f58d6a19a1b8177aff26be7
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h
@@ -0,0 +1,29 @@
+#pragma once //designed to include the current source file only once in a single compilation.
+
+#include <eigen3/Eigen/Geometry>
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+// Utility functions for spot mico motion command node
+
+
+// Convert a Eigen Matrix4f to an Affine3d
+Eigen::Affine3d matrix4fToAffine3d(const Eigen::Matrix4f& in);
+
+
+// Create a ROS tf2 TransformStamped from a Eigen Affine3d,
+// parent frame id and child frame id. Stamped with current time,
+// so should be broadcast ASAP
+geometry_msgs::TransformStamped eigAndFramesToTrans(
+ const Eigen::Affine3d& transform,
+ std::string parent_frame_id, std::string child_frame_id);
+
+
+// Create a transform from a translation, rotation, and parent and
+// child frame IDs. Will stamp the transform with ros::Time::now(),
+// so the returned transform should be broadcast asap
+ geometry_msgs::TransformStamped createTransform(
+ std::string parent_frame_id, std::string child_frame_id,
+ double x, double y, double z,
+ double roll, double pitch, double yaw);
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/launch/motion_cmd.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/launch/motion_cmd.launch
new file mode 100644
index 0000000000000000000000000000000000000000..10ce9f0360f8393753d47d9bdd68609b6aa09895
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/launch/motion_cmd.launch
@@ -0,0 +1,43 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Launch file for launching spot micro motion command node and, optionally, other nodes-->
+<!-- via command line arguments -->
+
+<launch>
+ <!-- Optional command line argument to run this node standalone (without i2c_pwmboard) -->
+ <!-- Useful for running or debugging program on a PC instead of a rpi -->
+ <arg name="run_standalone" default="false"/>
+
+ <!-- Run debug mode -->
+ <arg name="debug_mode" default="false"/>
+
+ <!-- Optional command line argument to also run lcd monitor node on rpi --> -->
+ <arg name="run_lcd" default="false"/>
+
+
+
+
+ <!-- If run_standalone is false, run i2c_pwmboard (the unless argument below)-->
+ <node unless="$(arg run_standalone)" name="i2cpwm_board_node" pkg="i2cpwm_board" type="i2cpwm_board" output="screen"> │walk: Start walk mode and keyboard motion control
+ </node>
+
+
+ <!-- Run the spot micro motion command node-->
+ <node name="spot_micro_motion_cmd_node" pkg="spot_micro_motion_cmd" type="spot_micro_motion_cmd_node" output="screen">
+ <!-- loading the parameters from yaml file during th runtime -->
+ <rosparam command="load" file="$(find spot_micro_motion_cmd)/config/spot_micro_motion_cmd.yaml"/>
+
+ <!-- Override debug_mode parameter to true IF debug_mode is true -->
+ <param if="$(arg debug_mode)" name="debug_mode" value="true" />
+
+ <!-- Override standalone_mode parameter to true IF standalone_mode is true -->
+ <param if="$(arg run_standalone)" name="run_standalone" value="true" />
+ </node>
+
+
+ <!-- If run_lcd is true, also run the lcd monitor node on the rpi by including it's launch file -->
+ <group if="$(arg run_lcd)">
+ <include file="$(find lcd_monitor)/launch/lcd_monitor.launch"/>
+ </group>
+
+
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..ae793b6b282b1bafdb195c1fb38bc94b545e5fb7
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore
@@ -0,0 +1,3 @@
+build/
+*.swp
+*.vim
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py
new file mode 100644
index 0000000000000000000000000000000000000000..594c731c628fa0f2d06c588ee2476e50ce237859
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py
@@ -0,0 +1,195 @@
+# This file is NOT licensed under the GPLv3, which is the license for the rest
+# of YouCompleteMe.
+#
+# Here's the license text for this file:
+#
+# This is free and unencumbered software released into the public domain.
+#
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
+#
+# In jurisdictions that recognize copyright laws, the author or authors
+# of this software dedicate any and all copyright interest in the
+# software to the public domain. We make this dedication for the benefit
+# of the public at large and to the detriment of our heirs and
+# successors. We intend this dedication to be an overt act of
+# relinquishment in perpetuity of all present and future rights to this
+# software under copyright law.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+# For more information, please refer to <http://unlicense.org/>
+
+from distutils.sysconfig import get_python_inc
+import platform
+import os
+import subprocess
+import ycm_core
+
+DIR_OF_THIS_SCRIPT = os.path.abspath( os.path.dirname( __file__ ) )
+DIR_OF_THIRD_PARTY = os.path.join( DIR_OF_THIS_SCRIPT, 'third_party' )
+SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+'-Wall',
+'-Wextra',
+'-Werror',
+'-Wno-long-long',
+'-Wno-variadic-macros',
+'-fexceptions',
+'-DNDEBUG',
+# You 100% do NOT need -DUSE_CLANG_COMPLETER and/or -DYCM_EXPORT in your flags;
+# only the YCM source code needs it.
+'-DUSE_CLANG_COMPLETER',
+'-DYCM_EXPORT=',
+# THIS IS IMPORTANT! Without the '-x' flag, Clang won't know which language to
+# use when compiling headers. So it will guess. Badly. So C++ headers will be
+# compiled as C headers. You don't want that so ALWAYS specify the '-x' flag.
+# For a C project, you would set this to 'c' instead of 'c++'.
+'-x',
+'c++',
+'-isystem',
+'cpp/pybind11',
+'-isystem',
+'cpp/BoostParts',
+'-isystem',
+get_python_inc(),
+'-isystem',
+'cpp/llvm/include',
+'-isystem',
+'cpp/llvm/tools/clang/include',
+'-I',
+'cpp/ycm',
+'-I',
+'cpp/ycm/ClangCompleter',
+'-isystem',
+'cpp/ycm/tests/gmock/gtest',
+'-isystem',
+'cpp/ycm/tests/gmock/gtest/include',
+'-isystem',
+'cpp/ycm/tests/gmock',
+'-isystem',
+'cpp/ycm/tests/gmock/include',
+'-isystem',
+'cpp/ycm/benchmarks/benchmark/include',
+]
+
+# Clang automatically sets the '-std=' flag to 'c++14' for MSVC 2015 or later,
+# which is required for compiling the standard library, and to 'c++11' for older
+# versions.
+if platform.system() != 'Windows':
+ flags.append( '-std=c++11' )
+
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# You can get CMake to generate this file for you by adding:
+# set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
+# to your CMakeLists.txt file.
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = '/home/mike/repos/spot_micro_kinematics_cpp/build'
+
+if os.path.exists( compilation_database_folder ):
+ database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+ database = None
+
+
+def IsHeaderFile( filename ):
+ extension = os.path.splitext( filename )[ 1 ]
+ return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
+
+
+def FindCorrespondingSourceFile( filename ):
+ if IsHeaderFile( filename ):
+ basename = os.path.splitext( filename )[ 0 ]
+ for extension in SOURCE_EXTENSIONS:
+ replacement_file = basename + extension
+ if os.path.exists( replacement_file ):
+ return replacement_file
+ return filename
+
+
+def Settings( **kwargs ):
+ if kwargs[ 'language' ] == 'cfamily':
+ # If the file is a header, try to find the corresponding source file and
+ # retrieve its flags from the compilation database if using one. This is
+ # necessary since compilation databases don't have entries for header files.
+ # In addition, use this source file as the translation unit. This makes it
+ # possible to jump from a declaration in the header file to its definition
+ # in the corresponding source file.
+ filename = FindCorrespondingSourceFile( kwargs[ 'filename' ] )
+
+ if not database:
+ return {
+ 'flags': flags,
+ 'include_paths_relative_to_dir': DIR_OF_THIS_SCRIPT,
+ 'override_filename': filename
+ }
+
+ compilation_info = database.GetCompilationInfoForFile( filename )
+ if not compilation_info.compiler_flags_:
+ return {}
+
+ # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+ # python list, but a "list-like" StringVec object.
+ final_flags = list( compilation_info.compiler_flags_ )
+
+ # NOTE: This is just for YouCompleteMe; it's highly likely that your project
+ # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR
+ # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT.
+ try:
+ final_flags.remove( '-stdlib=libc++' )
+ except ValueError:
+ pass
+
+ return {
+ 'flags': final_flags,
+ 'include_paths_relative_to_dir': compilation_info.compiler_working_dir_,
+ 'override_filename': filename
+ }
+ return {}
+
+
+def GetStandardLibraryIndexInSysPath( sys_path ):
+ for path in sys_path:
+ if os.path.isfile( os.path.join( path, 'os.py' ) ):
+ return sys_path.index( path )
+ raise RuntimeError( 'Could not find standard library path in Python path.' )
+
+
+def PythonSysPath( **kwargs ):
+ sys_path = kwargs[ 'sys_path' ]
+ for folder in os.listdir( DIR_OF_THIRD_PARTY ):
+ if folder == 'python-future':
+ folder = os.path.join( folder, 'src' )
+ sys_path.insert( GetStandardLibraryIndexInSysPath( sys_path ) + 1,
+ os.path.realpath( os.path.join( DIR_OF_THIRD_PARTY,
+ folder ) ) )
+ continue
+
+ if folder == 'cregex':
+ interpreter_path = kwargs[ 'interpreter_path' ]
+ major_version = subprocess.check_output( [
+ interpreter_path, '-c', 'import sys; print( sys.version_info[ 0 ] )' ]
+ ).rstrip().decode( 'utf8' )
+ folder = os.path.join( folder, 'regex_{}'.format( major_version ) )
+
+ sys_path.insert( 0, os.path.realpath( os.path.join( DIR_OF_THIRD_PARTY,
+ folder ) ) )
+ return sys_path
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9bc0bc45410da998040e7df68f8f1c260476f5cf
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt
@@ -0,0 +1,70 @@
+# Download and unpack googletest at configure time
+# See: http://crascit.com/2015/07/25/cmake-gtest/
+configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt)
+
+# Call CMake to download and Google Test
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download )
+if(result)
+ message(FATAL_ERROR "CMake step for googletest failed: ${result}")
+endif()
+# Build the downloaded google test
+execute_process(COMMAND ${CMAKE_COMMAND} --build .
+ RESULT_VARIABLE result
+ WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download )
+if(result)
+ message(FATAL_ERROR "Build step for googletest failed: ${result}")
+endif()
+
+# Prevent overriding the parent project's compiler/linker
+# settings on Windows
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+# Prevent installation of GTest with your project
+set(INSTALL_GTEST OFF CACHE BOOL "" FORCE)
+set(INSTALL_GMOCK OFF CACHE BOOL "" FORCE)
+
+# Add googletest directly to our build. This defines
+# the gtest and gtest_main targets.
+add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src
+ ${CMAKE_CURRENT_BINARY_DIR}/googletest-build)
+
+
+# This is a bit of a hack that can be used to get gtest libraries to build in C++11 if you aren't using CMAKE_CXX_STANDARD
+#
+#set(CXX11_FEATURES
+# cxx_nullptr
+# cxx_auto_type
+# cxx_delegating_constructors
+#)
+#target_compile_features(gtest
+# PRIVATE
+# ${CXX11_FEATURES}
+#)
+#
+#target_compile_features(gmock_main
+# PRIVATE
+# ${CXX11_FEATURES}
+#)
+#
+#target_compile_features(gmock
+# PRIVATE
+# ${CXX11_FEATURES}
+#)
+#
+#target_compile_features(gmock_main
+# PRIVATE
+# ${CXX11_FEATURES}
+#)
+
+# Add aliases for GTest and GMock libraries
+if(NOT TARGET GTest::GTest)
+ add_library(GTest::GTest ALIAS gtest)
+ add_library(GTest::Main ALIAS gtest_main)
+endif()
+
+if(NOT TARGET GTest::GMock)
+ add_library(GMock::GMock ALIAS gmock)
+ add_library(GMock::Main ALIAS gmock_main)
+endif()
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in
new file mode 100644
index 0000000000000000000000000000000000000000..b486b594256c1feded79b5f880d30380eaaddaa8
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(googletest-download NONE)
+
+include(ExternalProject)
+
+# Version bfc0ffc8a698072c794ae7299db9cb6866f4c0bc happens to be master when I set this up.
+# To prevent an issue with accidentally installing GTest / GMock with your project you should use a
+# commit after 9469fb687d040b60c8749b7617fee4e77c7f6409
+# Note: This is after the release of v1.8
+ExternalProject_Add(googletest
+ URL https://github.com/google/googletest/archive/703bd9caab50b139428cea1aaff9974ebee5742e.tar.gz
+ SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
+ BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
+ CONFIGURE_COMMAND ""
+ BUILD_COMMAND ""
+ INSTALL_COMMAND ""
+ TEST_COMMAND ""
+)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d3cf81a3d055993a8b94da35d4bcd2968c704619
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt
@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.5)
+
+# Set the project name
+project (spot_micro_kinematics)
+
+set(CMAKE_CXX_STANDARD 14)
+
+set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
+
+find_package(Eigen3 REQUIRED NO_MODULE)
+
+# Add an library for the example classes
+add_library(spot_micro_kinematics
+ src/spot_micro_kinematics.cpp
+ src/utils.cpp
+ src/spot_micro_leg.cpp
+)
+
+target_include_directories(spot_micro_kinematics
+ PUBLIC
+ ${PROJECT_SOURCE_DIR}/include
+)
+
+#target_link_libraries(spot_micro_kinematics
+ #Eigen3::Eigen
+#)
+
+#############################################
+# Unit tests
+# Only add 3rd party directory if gtest doesnt already exist
+if (NOT TARGET gtest)
+ add_subdirectory(3rd_party/google-test)
+endif ()
+# enable CTest testing
+enable_testing()
+
+# Add a testing executable
+add_executable(test_utils tests/test_utils.cpp)
+
+target_link_libraries(test_utils
+ spot_micro_kinematics
+ gtest
+ gtest_main
+)
+
+add_test(test_utils test_utils)
+
+
+# Add a testing executable
+add_executable(test_spot_micro_leg tests/test_spot_micro_leg.cpp)
+
+target_link_libraries(test_spot_micro_leg
+ spot_micro_kinematics
+ gtest
+ gtest_main
+)
+
+add_test(test_spot_micro_leg test_spot_micro_leg)
+
+# Add a testing executable
+add_executable(test_spot_micro_kinematics tests/test_spot_micro_kinematics.cpp)
+
+target_link_libraries(test_spot_micro_kinematics
+ spot_micro_kinematics
+ gtest
+ gtest_main
+)
+
+add_test(test_spot_micro_kinematics test_spot_micro_kinematics)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..8ef4fa971109be35b2f8ab2c9683fb20370b3a96
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 mike4192
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..fa719cf8360e49f4543b5a36eee0cfa7e3ea766c
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md
@@ -0,0 +1,2 @@
+# spot_micro_kinematics_cpp
+A c++ library for kinematic operations on a spot micro quadruped
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h
new file mode 100644
index 0000000000000000000000000000000000000000..b89033149ffc708df09588faf4d03b62dfa851b5
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h
@@ -0,0 +1,132 @@
+#pragma once
+
+#include <eigen3/Eigen/Geometry>
+
+#include "spot_micro_kinematics/utils.h"
+#include "spot_micro_kinematics/spot_micro_leg.h"
+
+
+namespace smk { // Start smk namespace
+
+// Struct to hold joint angles for four legs
+struct LegsJointAngles {
+ JointAngles right_back;
+ JointAngles right_front;
+ JointAngles left_front;
+ JointAngles left_back;
+};
+
+struct LegsFootPos {
+ Point right_back;
+ Point right_front;
+ Point left_front;
+ Point left_back;
+};
+
+// Struct to hold various configuration values of a spot micro robot frame
+struct SpotMicroConfig {
+ float hip_link_length;
+ float upper_leg_link_length;
+ float lower_leg_link_length;
+ float body_width;
+ float body_length;
+};
+
+// Struct to hold euler angles
+struct EulerAngs {
+ float phi;
+ float theta;
+ float psi;
+};
+
+// Struct to hold body position and euler angles
+struct BodyState {
+ EulerAngs euler_angs;
+ Point xyz_pos;
+ LegsFootPos leg_feet_pos;
+};
+
+struct AllRobotRelativeTransforms {
+ Eigen::Matrix4f bodyCenter;
+ Eigen::Matrix4f centerToRightBack;
+ Eigen::Matrix4f centerToRightFront;
+ Eigen::Matrix4f centerToLeftFront;
+ Eigen::Matrix4f centerToLeftBack;
+ LegRelativeTransforms rightBackLeg;
+ LegRelativeTransforms rightFrontLeg;
+ LegRelativeTransforms leftFrontLeg;
+ LegRelativeTransforms leftBackLeg;
+};
+
+class SpotMicroKinematics {
+
+ public:
+ // Constructor, sets up a spot micro kinematics object
+ SpotMicroKinematics(float x, float y, float z, const SpotMicroConfig& smc);
+
+ // Default Constructor
+ SpotMicroKinematics() = default;
+
+ // Returns the body center homogenous transformation matrix
+ Eigen::Matrix4f getBodyHt();
+
+ // Sets the joint angles for the legs in the robot
+ void setLegJointAngles(const LegsJointAngles& four_legs_joint_angs);
+
+ // Sets the foot for each leg to the commanded position in a global coordinate
+ // system
+ void setFeetPosGlobalCoordinates(const LegsFootPos& four_legs_foot_pos);
+
+ // Sets a body rotation without translating the body or moving the feet
+ void setBodyAngles(float phi, float theta, float psi);
+
+ // Sets a body position without rotation of the body, or moving the feet
+ void setBodyPosition(float x, float y, float z);
+
+ // Sets the body state: feet position, body position and angles
+ void setBodyState(const BodyState& body_state);
+
+ // Returns the joint angles of the four legs
+ LegsJointAngles getLegsJointAngles();
+
+ // Return the position of the four feet
+ LegsFootPos getLegsFootPos();
+
+ // Returns body state: feet positio, body position and angles
+ BodyState getBodyState();
+
+ // Returns a structure of the relative homogenous transforms for
+ // all of the robot links
+ AllRobotRelativeTransforms getRobotTransforms();
+
+ private:
+
+ SpotMicroConfig smc_; // Spot micro config struct
+
+ // x, y, z position of body center in global coordinate system
+ float x_;
+ float y_;
+ float z_;
+
+ // Euler angles of body in global coordinate system
+ float phi_;
+ float theta_;
+ float psi_;
+
+ // Leg objects of the robot
+ SpotMicroLeg right_back_leg_;
+ SpotMicroLeg right_front_leg_;
+ SpotMicroLeg left_front_leg_;
+ SpotMicroLeg left_back_leg_;
+
+};
+
+} // End smk namespace
+
+
+
+
+
+
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h
new file mode 100644
index 0000000000000000000000000000000000000000..15ee64678976dc3ef3f6ba05030485ac068e0eb7
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h
@@ -0,0 +1,61 @@
+#pragma once
+
+#include <eigen3/Eigen/Geometry>
+
+#include "spot_micro_kinematics/utils.h"
+
+
+namespace smk {
+
+class SpotMicroLeg {
+
+ public:
+ // Constructor, sets a leg up with initial joint angles and link lengths
+ SpotMicroLeg(const JointAngles& joint_angles,
+ const LinkLengths& link_lengths,
+ bool is_leg_12);
+
+ // Also use default constructor
+ SpotMicroLeg() = default;
+
+ // Sets the three leg angles and updates homogeneous transformation
+ // matrices
+ void setAngles(const JointAngles& joint_angles);
+
+ // Set the foot position in the leg's local coordinate system
+ void setFootPosLocalCoordinates(const Point& point);
+
+ // Set the foot position in global coordinate system given a desired point, at
+ // the ht representing the start of the leg's coordinate system
+ void setFootPosGlobalCoordinates(const Point& point,
+ const Eigen::Matrix4f& ht_leg_start);
+
+ // Returns the foot position in the global coordinate system given the ht
+ // representing the start of the leg's coordinate system
+ Point getFootPosGlobalCoordinates(const Eigen::Matrix4f& ht_leg_start);
+
+ // Returns the three joint angles, ang1, ang2, ang3
+ JointAngles getLegJointAngles();
+
+ // Get homogenous transform for leg base to link 1
+ Eigen::Matrix4f getTransform0To1();
+
+ // Get homogenous transform for leg base to link 3
+ Eigen::Matrix4f getTransform1To3();
+
+ // Get homogenous transform for leg base to link 4
+ Eigen::Matrix4f getTransform3To4();
+
+
+ private:
+ JointAngles joint_angles_; // Joint angles of the leg
+
+ LinkLengths link_lengths_; // Lengths of the leg links
+
+ bool is_leg_12_; // Boolean representing whether leg is 1 or 2,
+ // (as opposed to 3 or 4)
+
+};
+
+
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd6c6720fea0ff64f0e975054376d354f7df3056
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h
@@ -0,0 +1,104 @@
+#pragma once // So header is only included once
+
+#include <eigen3/Eigen/Geometry>
+
+namespace smk
+{
+
+struct Point {
+ float x;
+ float y;
+ float z;
+};
+
+struct LinkLengths {
+ float l1;
+ float l2;
+ float l3;
+};
+
+struct JointAngles {
+ float ang1;
+ float ang2;
+ float ang3;
+};
+
+struct LegRelativeTransforms {
+ Eigen::Matrix4f t01;
+ Eigen::Matrix4f t13;
+ Eigen::Matrix4f t34;
+};
+
+
+// Returns a 4x4 Matrix that represents a homogeneous rotation matrix
+// in the order x, y, z. Input angles are in units radians.
+// A convenience wrapper around Eigen::Transform build up
+Eigen::Matrix4f homogRotXyz(float x_ang, float y_ang, float z_ang);
+
+// Returns a 4x4 matrix that represents a homogeneous translation matrix.
+// Input distances of x, y, z
+// A convenience wrapper around Eigen::Transform
+Eigen::Matrix4f homogTransXyz(float x, float y, float z);
+
+
+// Returns the inverse of the inputted homogeneous transform. Note that the
+// inverse is not just the inverse of the matrix, but a reversed rotation, and a
+// reversed linear translation. See definition for more details
+Eigen::Matrix4f homogInverse(const Eigen::Matrix4f& ht);
+
+
+// Returns the homogeneous transformation matrix representing the transformation
+// from robot center to a coordinate system of the right back leg of a quadruped.
+// Assumes legs are positioned in the corner of a rectangular plane defined by a
+// length and height.
+Eigen::Matrix4f htLegRightBack(float body_length, float body_width);
+
+
+// Returns the homogeneous transformation matrix representing the transformation
+// from robot center to a coordinate system of the right front leg of a quadruped.
+// Assumes legs are positioned in the corner of a rectangular plane defined by a
+// length and height.
+Eigen::Matrix4f htLegRightFront(float body_length, float body_width);
+
+// Returns the homogeneous transformation matrix representing the transformation
+// from robot center to a coordinate system of the left front leg of a quadruped.
+// Assumes legs are positioned in the corner of a rectangular plane defined by a
+// length and height.
+Eigen::Matrix4f htLegLeftFront(float body_length, float body_width);
+
+// Returns the homogeneous transformation matrix representing the transformation
+// from robot center to a coordinate system of the left back leg of a quadruped.
+// Assumes legs are positioned in the corner of a rectangular plane defined by a
+// length and height.
+Eigen::Matrix4f htLegLeftBack(float body_length, float body_width);
+
+
+// Returns the homogeneous transformation matrix for joint 0 to 1 for a
+// quadruped leg
+Eigen::Matrix4f ht0To1(float rot_ang, float link_length);
+
+
+// Returns the homogeneous transformation matrix for joint 1 to 2 for a
+// quadruped leg
+Eigen::Matrix4f ht1To2();
+
+// Returns the homogeneous transformation matrix for joint 2 to 3 for a
+// quadruped leg
+Eigen::Matrix4f ht2To3(float rot_ang, float link_length);
+
+// Returns the homogeneous transformation matrix for joint 3 to 4 for a
+// quadruped leg
+Eigen::Matrix4f ht3To4(float rot_ang, float link_length);
+
+// Returns the homogeneous transformation matrix from joint 0 to 1 for a
+// quadruped leg
+Eigen::Matrix4f ht0To4(const JointAngles& joint_angles,
+ const LinkLengths& link_lengths);
+
+// Returns the joint angles of a leg after running the inverse kinematics on a
+// quadruped leg.
+// Last argument, an optional boolean input, specifies whether the equations for
+// legs 1 and 2 are used (as opposed for legs 3 and 4)
+ JointAngles ikine(const Point& point, const LinkLengths& link_lengths,
+ bool is_leg_12 = true);
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dd215b9a0f5c4ea1e8623bb7d0bd949fafbab22d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp
@@ -0,0 +1,214 @@
+#include <eigen3/Eigen/Geometry>
+
+#include "spot_micro_kinematics/utils.h"
+#include "spot_micro_kinematics/spot_micro_leg.h"
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+
+using namespace Eigen;
+
+namespace smk { // Start smk namespace
+
+SpotMicroKinematics::SpotMicroKinematics(float x, float y, float z,
+ const SpotMicroConfig& smc)
+ : x_(x),
+ y_(y),
+ z_(z),
+ smc_(smc) {
+
+ // Initialize other class attributes
+ phi_ = 0.0f;
+ theta_ = 0.0f;
+ psi_ = 0.0f;
+
+ // Create temporary structs for initializing leg's joint angles and lengths
+ JointAngles joint_angles_temp = {0.0f, 0.0f, 0.0f};
+ LinkLengths link_lengths_temp = {smc.hip_link_length,
+ smc.upper_leg_link_length,
+ smc.lower_leg_link_length};
+
+ // Create legs
+ right_back_leg_ = SpotMicroLeg(joint_angles_temp, link_lengths_temp, true);
+ right_front_leg_ = SpotMicroLeg(joint_angles_temp, link_lengths_temp, true);
+ left_front_leg_ = SpotMicroLeg(joint_angles_temp, link_lengths_temp, false);
+ left_back_leg_ = SpotMicroLeg(joint_angles_temp, link_lengths_temp, false);
+}
+
+
+Matrix4f SpotMicroKinematics::getBodyHt() {
+ // Euler angle order is phi, psi, theta because the axes of the robot are x
+ // pointing forward, y pointing up, z pointing right
+ return(homogTransXyz(x_, y_, z_) * homogRotXyz(phi_, psi_, theta_));
+}
+
+
+void SpotMicroKinematics::setLegJointAngles(
+ const LegsJointAngles& four_legs_joint_angs) {
+ // Call each leg's method to set joint angles
+ right_back_leg_.setAngles(four_legs_joint_angs.right_back);
+ right_front_leg_.setAngles(four_legs_joint_angs.right_front);
+ left_front_leg_.setAngles(four_legs_joint_angs.left_front);
+ left_back_leg_.setAngles(four_legs_joint_angs.left_back);
+}
+
+
+void SpotMicroKinematics::setFeetPosGlobalCoordinates(
+ const LegsFootPos& four_legs_foot_pos) {
+ // Get the body center homogeneous transform matrix
+ Matrix4f ht_body = getBodyHt();
+
+ // Create each leg's starting ht matrix. Made in order of right back, right
+ // front, left front, left back
+ Matrix4f ht_rb = ht_body * htLegRightBack(smc_.body_length, smc_.body_width);
+ Matrix4f ht_rf = ht_body * htLegRightFront(smc_.body_length, smc_.body_width);
+ Matrix4f ht_lf = ht_body * htLegLeftFront(smc_.body_length, smc_.body_width);
+ Matrix4f ht_lb = ht_body * htLegLeftBack(smc_.body_length, smc_.body_width);
+
+
+ // Call each leg's method to set foot position in global coordinates
+ right_back_leg_.setFootPosGlobalCoordinates(
+ four_legs_foot_pos.right_back, ht_rb);
+
+ right_front_leg_.setFootPosGlobalCoordinates(
+ four_legs_foot_pos.right_front, ht_rf);
+
+ left_front_leg_.setFootPosGlobalCoordinates(
+ four_legs_foot_pos.left_front, ht_lf);
+
+ left_back_leg_.setFootPosGlobalCoordinates(
+ four_legs_foot_pos.left_back, ht_lb);
+}
+
+
+void SpotMicroKinematics::setBodyAngles(float phi, float theta, float psi) {
+ // Save the current feet position
+ LegsFootPos saved_foot_pos = getLegsFootPos();
+
+ // Update body angles
+ phi_ = phi;
+ theta_ = theta;
+ psi_ = psi;
+
+ // Call method to set absolute feet position to the saved values
+ setFeetPosGlobalCoordinates(saved_foot_pos);
+}
+
+
+void SpotMicroKinematics::setBodyPosition(float x, float y, float z) {
+ // Save the current feet position
+ LegsFootPos saved_foot_pos = getLegsFootPos();
+
+ // Update body angles
+ x_ = x;
+ y_ = y;
+ z_ = z;
+
+ // Call method to set absolute feet position to the saved values
+ setFeetPosGlobalCoordinates(saved_foot_pos);
+}
+
+
+void SpotMicroKinematics::setBodyState(const BodyState& body_state) {
+ // Set x,y,z position
+ x_ = body_state.xyz_pos.x;
+ y_ = body_state.xyz_pos.y;
+ z_ = body_state.xyz_pos.z;
+
+ // set euler angles
+ phi_ = body_state.euler_angs.phi;
+ theta_ = body_state.euler_angs.theta;
+ psi_ = body_state.euler_angs.psi;
+
+ // set feet position
+ setFeetPosGlobalCoordinates(body_state.leg_feet_pos);
+}
+
+LegsJointAngles SpotMicroKinematics::getLegsJointAngles() {
+ // Return the leg joint angles
+ LegsJointAngles ret_val;
+
+ ret_val.right_back = right_back_leg_.getLegJointAngles();
+ ret_val.right_front = right_front_leg_.getLegJointAngles();
+ ret_val.left_front = left_front_leg_.getLegJointAngles();
+ ret_val.left_back = left_back_leg_.getLegJointAngles();
+
+ return ret_val;
+}
+
+
+LegsFootPos SpotMicroKinematics::getLegsFootPos() {
+
+ // Get the body center homogeneous transform matrix
+ Matrix4f ht_body = getBodyHt();
+
+ // Return the leg joint angles
+ LegsFootPos ret_val;
+
+ // Create each leg's starting ht matrix. Made in order of right back, right
+ // front, left front, left back
+ Matrix4f ht_rb = ht_body * htLegRightBack(smc_.body_length, smc_.body_width);
+ Matrix4f ht_rf = ht_body * htLegRightFront(smc_.body_length, smc_.body_width);
+ Matrix4f ht_lf = ht_body * htLegLeftFront(smc_.body_length, smc_.body_width);
+ Matrix4f ht_lb = ht_body * htLegLeftBack(smc_.body_length, smc_.body_width);
+
+ ret_val.right_back = right_back_leg_.getFootPosGlobalCoordinates(ht_rb);
+ ret_val.right_front = right_front_leg_.getFootPosGlobalCoordinates(ht_rf);
+ ret_val.left_front = left_front_leg_.getFootPosGlobalCoordinates(ht_lf);
+ ret_val.left_back = left_back_leg_.getFootPosGlobalCoordinates(ht_lb);
+
+ return ret_val;
+}
+
+BodyState SpotMicroKinematics::getBodyState() {
+ BodyState body_state = {EulerAngs{phi_, theta_, psi_},
+ Point{x_, y_, z_},
+ getLegsFootPos()};
+ return body_state;
+}
+
+
+AllRobotRelativeTransforms SpotMicroKinematics::getRobotTransforms() {
+
+ // Initialize structure
+ AllRobotRelativeTransforms allTransforms;
+
+ // Fill the structure in
+ // Body center
+ allTransforms.bodyCenter = getBodyHt();
+
+ // Center to four leg corners
+ allTransforms.centerToRightBack = htLegRightBack(smc_.body_length,
+ smc_.body_width);
+
+ allTransforms.centerToRightFront = htLegRightFront(smc_.body_length,
+ smc_.body_width);
+
+ allTransforms.centerToLeftFront = htLegLeftFront(smc_.body_length,
+ smc_.body_width);
+
+ allTransforms.centerToLeftBack = htLegLeftBack(smc_.body_length,
+ smc_.body_width);
+
+ // Right back leg
+ allTransforms.rightBackLeg.t01 = right_back_leg_.getTransform0To1();
+ allTransforms.rightBackLeg.t13 = right_back_leg_.getTransform1To3();
+ allTransforms.rightBackLeg.t34 = right_back_leg_.getTransform3To4();
+
+ // Right front leg
+ allTransforms.rightFrontLeg.t01 = right_front_leg_.getTransform0To1();
+ allTransforms.rightFrontLeg.t13 = right_front_leg_.getTransform1To3();
+ allTransforms.rightFrontLeg.t34 = right_front_leg_.getTransform3To4();
+
+ // Left front leg
+ allTransforms.leftFrontLeg.t01 = left_front_leg_.getTransform0To1();
+ allTransforms.leftFrontLeg.t13 = left_front_leg_.getTransform1To3();
+ allTransforms.leftFrontLeg.t34 = left_front_leg_.getTransform3To4();
+
+ // Left back leg
+ allTransforms.leftBackLeg.t01 = left_back_leg_.getTransform0To1();
+ allTransforms.leftBackLeg.t13 = left_back_leg_.getTransform1To3();
+ allTransforms.leftBackLeg.t34 = left_back_leg_.getTransform3To4();
+
+ return allTransforms;
+}
+
+} // End smk namespace
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d7dbccab71b51f7e1114aed64dbfd29d8966cef
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp
@@ -0,0 +1,92 @@
+#include <eigen3/Eigen/Geometry>
+
+#include "spot_micro_kinematics/utils.h"
+#include "spot_micro_kinematics/spot_micro_leg.h"
+
+using namespace Eigen;
+
+namespace smk { // Start smk namespace
+
+
+// Constructor
+SpotMicroLeg::SpotMicroLeg(const JointAngles& joint_angles,
+ const LinkLengths& link_lengths,
+ bool is_leg_12)
+ : joint_angles_(joint_angles),
+ link_lengths_(link_lengths),
+ is_leg_12_(is_leg_12) {
+}
+
+
+void SpotMicroLeg::setAngles(const JointAngles& joint_angles) {
+ // Update object's joint angles
+
+ joint_angles_ = joint_angles;
+}
+
+
+void SpotMicroLeg::setFootPosLocalCoordinates(const Point& point) {
+
+ // Run inverse kinematics to find joint angles
+ JointAngles joint_angles = ikine(point, link_lengths_, is_leg_12_);
+
+ // Call method to set joint angles of the leg
+ setAngles(joint_angles);
+}
+
+
+void SpotMicroLeg::setFootPosGlobalCoordinates(const Point& point,
+ const Matrix4f& ht_leg_start) {
+
+ // Need to express the point in the leg's coordinate system, can do so by
+ // transforming a vector of the points in global coordinate by the inverse of
+ // the leg's starting homogeneous transform
+
+ // Make a homogeneous vector, and store the point in global coords in it
+ Eigen::Vector4f p4_ht_vec(point.x, point.y, point.z, 1.0f);
+
+ // Multiply it by the inverse of the homgeneous transform of the leg start.
+ // This operation yields a foot position in the foot's local coordinates
+ p4_ht_vec = homogInverse(ht_leg_start) * p4_ht_vec;
+
+ Point point_local{.x = p4_ht_vec(0), .y = p4_ht_vec(1), .z = p4_ht_vec(2)};
+
+ // Call this leg's method for setting foot position in local cordinates
+ setFootPosLocalCoordinates(point_local);
+}
+
+
+Point SpotMicroLeg::getFootPosGlobalCoordinates(const Matrix4f& ht_leg_start) {
+ // Get homogeneous transform of foot
+ Matrix4f ht_foot = ht_leg_start *
+ ht0To4(joint_angles_, link_lengths_);
+
+ // Construct return point structure
+ Point return_point = {.x = ht_foot(0,3),
+ .y = ht_foot(1,3),
+ .z = ht_foot(2,3) };
+ return return_point;
+}
+
+
+JointAngles SpotMicroLeg::getLegJointAngles() {
+ return joint_angles_;
+}
+
+
+Matrix4f SpotMicroLeg::getTransform0To1() {
+ return ht0To1(joint_angles_.ang1, link_lengths_.l1);
+}
+
+
+Matrix4f SpotMicroLeg::getTransform1To3() {
+ return (ht1To2() *
+ ht2To3(joint_angles_.ang2, link_lengths_.l2));
+}
+
+
+Matrix4f SpotMicroLeg::getTransform3To4() {
+ return ht3To4(joint_angles_.ang3, link_lengths_.l3);
+}
+
+} // End smk namespace
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5222b0a1a6ed25fa5af02fcc511c800866db8f3
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp
@@ -0,0 +1,218 @@
+#include "spot_micro_kinematics/utils.h"
+
+#include <iostream>
+#include <cmath>
+
+#include <eigen3/Eigen/Geometry>
+
+using namespace Eigen;
+
+namespace smk
+{ // Start smk namespace
+
+Matrix4f homogRotXyz(float x_ang, float y_ang, float z_ang)
+{
+ // Create 3d transformation, and execute x, y, and z rotations
+ Transform<float, 3, Affine> t = Transform<float,3,Affine>::Identity();
+ t.rotate(AngleAxisf(x_ang, Vector3f::UnitX()));
+ t.rotate(AngleAxisf(y_ang, Vector3f::UnitY()));
+ t.rotate(AngleAxisf(z_ang, Vector3f::UnitZ()));
+
+ return t.matrix();
+}
+
+
+Matrix4f homogTransXyz(float x, float y, float z)
+{
+ // Create a linear translation homogenous transformation matrix
+ Transform<float, 3, Affine> t;
+ t = Translation<float, 3> (Vector3f(x,y,z));
+
+ return t.matrix();
+}
+
+
+Matrix4f homogInverse(const Matrix4f& ht)
+{
+//The inverse of a homogeneous transformation matrix can be represented as a
+// a matrix product of the following:
+//
+// ------------------- -------------------
+// | | 0 | | 1 0 0 -x_t |
+// ht_inv = | R^-1 | 0 | * | 0 1 0 -y_t |
+// |___________| 0 | | 0 0 1 -z_t |
+// | 0 0 0 | 1 | | 0 0 0 1 |
+// ------------------- -------------------
+//
+// Where R^-1 is the inverse of the rotation matrix portion of the homogeneous
+// transform (the first three rows and columns). Note that the inverse
+// of a rotation matrix is equal to its transpose. And x_t, y_t, z_t are the
+// linear trasnformation portions of the original transform.
+
+ Matrix3f temp_rot = ht.block<3,3>(0,0); // Get rotation matrix portion from homogeneous transform via block
+ temp_rot.transposeInPlace(); // Transpose, equivalent to inverse for rotation matrix
+
+ // Store linear translation portion and negate directions
+ Vector3f temp_translate = ht.block<3,1>(0,3);
+ temp_translate = temp_translate * -1.0f;
+
+ // Create left hand portion of ht_inv from comment block above
+ Matrix4f ht_inverted1 = Matrix4f::Identity();
+ ht_inverted1.block<3,3>(0,0) = temp_rot;
+
+ // Create right hand portion of ht_in from comment block above
+ Matrix4f ht_inverted2 = Matrix4f::Identity();
+ ht_inverted2.block<3,1>(0,3) = temp_translate;
+
+ // Return product of matrices, the homogeneous transform inverse
+ return (ht_inverted1 * ht_inverted2);
+}
+
+
+Matrix4f htLegRightBack(float body_length, float body_width) {
+
+ // Build up matrix representing right back leg ht. First, a pi/2 rotation in y
+ Matrix4f htLegRightBack = homogRotXyz(0.0f, M_PI/2.0f, 0.0);
+
+ // Next, add the linear translation portion
+ htLegRightBack.block<3,1>(0,3) = Vector3f(-body_length/2.0f, 0.0f, body_width/2.0f);
+
+ return htLegRightBack;
+}
+
+
+Matrix4f htLegRightFront(float body_length, float body_width) {
+
+ // Build up matrix representing right front leg ht. First, a pi/2 rotation in y
+ Matrix4f htLegRightFront = homogRotXyz(0.0f, M_PI/2.0f, 0.0);
+
+ // Next, add the linear translation portion
+ htLegRightFront.block<3,1>(0,3) = Vector3f(body_length/2.0f, 0.0f, body_width/2.0f);
+
+ return htLegRightFront;
+}
+
+Matrix4f htLegLeftFront(float body_length, float body_width) {
+
+ // Build up matrix representing right front leg ht. First, a pi/2 rotation in y
+ Matrix4f htLegLeftFront = homogRotXyz(0.0f, -M_PI/2.0f, 0.0);
+
+ // Next, add the linear translation portion
+ htLegLeftFront.block<3,1>(0,3) = Vector3f(body_length/2.0f, 0.0f, -body_width/2.0f);
+
+ return htLegLeftFront;
+}
+
+
+Matrix4f htLegLeftBack(float body_length, float body_width) {
+
+ // Build up matrix representing right back leg ht. First, a pi/2 rotation in y
+ Matrix4f htLegLeftBack = homogRotXyz(0.0f, -M_PI/2.0f, 0.0);
+
+ // Next, add the linear translation portion
+ htLegLeftBack.block<3,1>(0,3) = Vector3f(-body_length/2.0f, 0.0f, -body_width/2.0f);
+
+ return htLegLeftBack;
+}
+
+
+Matrix4f ht0To1(float rot_ang, float link_length) {
+
+ // Build up the matrix as from the paper
+ Matrix4f ht_0_to_1 = homogRotXyz(0.0f, 0.0f, rot_ang);
+
+ // Add in remaining terms
+ ht_0_to_1(0,3) = -link_length*cos(rot_ang);
+ ht_0_to_1(1,3) = -link_length*sin(rot_ang);
+
+ return ht_0_to_1;
+}
+
+Matrix4f ht1To2() {
+ // Build up the matrix as from the paper
+ Matrix4f ht_1_to_2;
+
+ ht_1_to_2 <<
+ 0.0f, 0.0f, -1.0f, 0.0f,
+ -1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ return ht_1_to_2;
+}
+
+
+Matrix4f ht2To3(float rot_ang, float link_length) {
+
+ // Build up the matrix as from the paper
+ Matrix4f ht_2_to_3 = homogRotXyz(0.0f, 0.0f, rot_ang);
+
+ // Add in remaining terms
+ ht_2_to_3(0,3) = link_length*cos(rot_ang);
+ ht_2_to_3(1,3) = link_length*sin(rot_ang);
+
+ return ht_2_to_3;
+}
+
+Matrix4f ht3To4(float rot_ang, float link_length) {
+ // Same as the 2 to 3 transformation, so just call that function
+
+ return ht2To3(rot_ang, link_length);
+}
+
+Matrix4f ht0To4(const JointAngles& joint_angles,
+ const LinkLengths& link_lengths) {
+ // Result is a sequential multiplication of all 4 transform matrices
+ return (ht0To1(joint_angles.ang1, link_lengths.l1) *
+ ht1To2() *
+ ht2To3(joint_angles.ang2, link_lengths.l2) *
+ ht3To4(joint_angles.ang3, link_lengths.l3));
+}
+
+
+JointAngles ikine(const Point& point, const LinkLengths& link_lengths, bool is_leg_12) {
+ using namespace std;
+
+ // Initialize return struct
+ JointAngles joint_angles;
+
+ // Convenience variables for math
+ float x4 = point.x;
+ float y4 = point.y;
+ float z4 = point.z;
+ float l1 = link_lengths.l1;
+ float l2 = link_lengths.l2;
+ float l3 = link_lengths.l3;
+
+ // Supporting variable D
+ float D = (x4*x4 + y4*y4 + z4*z4 - l1*l1 - l2*l2 - l3*l3) /
+ (2*l2*l3);
+
+ // Poor man's inverse kinematics reachability protection:
+ // Limit D to a maximum value of 1, otherwise the square root functions
+ // below (sqrt(1 - D^2)) will attempt a square root of a negative number
+ if (D > 1.0f) {
+ D = 1.0f;
+ } else if (D < -1.0f) {
+ D = -1.0f;
+ }
+
+ if (is_leg_12) {
+ joint_angles.ang3 = atan2(sqrt(1 - D*D), D);
+ } else {
+ joint_angles.ang3 = atan2(-sqrt(1 - D*D), D);
+ }
+
+ // Another poor mans reachability sqrt protection
+ float protected_sqrt_val = x4*x4 + y4*y4 - l1*l1;
+ if (protected_sqrt_val < 0.0f) { protected_sqrt_val = 0.0f;}
+
+ joint_angles.ang2 = atan2(z4, sqrt(protected_sqrt_val)) -
+ atan2(l3*sin(joint_angles.ang3), l2 + l3*cos(joint_angles.ang3));
+
+ joint_angles.ang1 = atan2(y4, x4) + atan2(sqrt(protected_sqrt_val), -l1);
+
+ return joint_angles;
+}
+
+} // End smk namespace
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf43a074e39c6e69af2970ec0abe4ff69f8a7157
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp
@@ -0,0 +1,288 @@
+#include <iostream>
+
+#include <eigen3/Eigen/Geometry>
+#include <gtest/gtest.h>
+
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+
+using namespace std;
+using namespace Eigen;
+using namespace smk;
+
+TEST(testSetJointAngles, basic_test)
+{
+
+ float d2r = M_PI/180.0f;
+
+ // Create a spot micro kinematics object, set the joint angles, make sure they
+ // are set for the legs
+ SpotMicroConfig smc = {0.1, 0.2, 0.3, 0.4, 0.5};
+
+ SpotMicroKinematics sm = SpotMicroKinematics(0.0f, 0.0f, 0.0f, smc);
+
+ JointAngles desired = {10*d2r, 20*d2r, -15*d2r};
+
+ LegsJointAngles four_legs_desired_angs = {desired, desired, desired, desired};
+
+ sm.setLegJointAngles(four_legs_desired_angs);
+
+ LegsJointAngles test_legs_joint_angs = sm.getLegsJointAngles();
+
+ // Test equality
+ EXPECT_EQ(four_legs_desired_angs.right_back.ang1, test_legs_joint_angs.right_back.ang1);
+ EXPECT_EQ(four_legs_desired_angs.right_back.ang2, test_legs_joint_angs.right_back.ang2);
+ EXPECT_EQ(four_legs_desired_angs.right_back.ang3, test_legs_joint_angs.right_back.ang3);
+
+ EXPECT_EQ(four_legs_desired_angs.right_front.ang1, test_legs_joint_angs.right_front.ang1);
+ EXPECT_EQ(four_legs_desired_angs.right_front.ang2, test_legs_joint_angs.right_front.ang2);
+ EXPECT_EQ(four_legs_desired_angs.right_front.ang3, test_legs_joint_angs.right_front.ang3);
+
+ EXPECT_EQ(four_legs_desired_angs.left_front.ang1, test_legs_joint_angs.left_front.ang1);
+ EXPECT_EQ(four_legs_desired_angs.left_front.ang2, test_legs_joint_angs.left_front.ang2);
+ EXPECT_EQ(four_legs_desired_angs.left_front.ang3, test_legs_joint_angs.left_front.ang3);
+
+ EXPECT_EQ(four_legs_desired_angs.left_back.ang1, test_legs_joint_angs.left_back.ang1);
+ EXPECT_EQ(four_legs_desired_angs.left_back.ang2, test_legs_joint_angs.left_back.ang2);
+ EXPECT_EQ(four_legs_desired_angs.left_back.ang3, test_legs_joint_angs.left_back.ang3);
+}
+
+
+TEST(testSetFootPos, basic_test)
+{
+
+ float d2r = M_PI/180.0f;
+
+ // Create a spot micro kinematics object, set reachable foot positions given
+ // the leg and body geometry
+ SpotMicroConfig smc = {0.1, 0.4, 0.4, 0.4, 0.5};
+ SpotMicroKinematics sm = SpotMicroKinematics(0.05f, 0.03f, 0.01f, smc);
+
+ LegsFootPos four_legs_desired_pos = {.right_back = {-0.22, -0.22, 0.32},
+ .right_front = {0.22, -0.22, 0.22},
+ .left_front = {0.22, -0.22, -0.22},
+ .left_back = {-0.22, -0.22, -0.22}};
+
+ sm.setFeetPosGlobalCoordinates(four_legs_desired_pos);
+
+ LegsFootPos test_legs_foot_pos = sm.getLegsFootPos();
+
+ // Test value nearness to 0.0001f
+ float tol = 0.0001f;
+ EXPECT_NEAR(four_legs_desired_pos.right_back.x, test_legs_foot_pos.right_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.y, test_legs_foot_pos.right_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.z, test_legs_foot_pos.right_back.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.right_front.x, test_legs_foot_pos.right_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.y, test_legs_foot_pos.right_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.z, test_legs_foot_pos.right_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_front.x, test_legs_foot_pos.left_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.y, test_legs_foot_pos.left_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.z, test_legs_foot_pos.left_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_back.x, test_legs_foot_pos.left_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.y, test_legs_foot_pos.left_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.z, test_legs_foot_pos.left_back.z, tol);
+}
+
+
+TEST(testSetBodyAngle, basic_test)
+{
+
+ float d2r = M_PI/180.0f;
+
+ // Create a spot micro kinematics object, set reachable foot positions given
+ // the leg and body geometry.
+ //
+ // Get the joint angles.
+ //
+ // Set body angles
+ //
+ // Get the feet position and joint angles
+ //
+ // Verify the feet position still match after all operations, but that the
+ // joint angles are different (verifying the legs actually changes but the
+ // feet are in the same spot)
+
+
+ SpotMicroConfig smc = {0.1, 0.4, 0.4, 0.4, 0.5};
+ SpotMicroKinematics sm = SpotMicroKinematics(0.05f, 0.03f, 0.01f, smc);
+
+ LegsFootPos four_legs_desired_pos = {.right_back = {-0.22, -0.22, 0.32},
+ .right_front = {0.22, -0.22, 0.22},
+ .left_front = {0.22, -0.22, -0.22},
+ .left_back = {-0.22, -0.22, -0.22}};
+
+ sm.setFeetPosGlobalCoordinates(four_legs_desired_pos);
+
+ LegsJointAngles truth_joint_angles = sm.getLegsJointAngles();
+
+ sm.setBodyAngles(5.0*d2r, 3.0*d2r, -2.0*d2r);
+
+ LegsFootPos test_legs_foot_pos = sm.getLegsFootPos();
+ LegsJointAngles test_legs_joint_angs = sm.getLegsJointAngles();
+
+ // Test value nearness to 0.0001f
+ float tol = 0.0001f;
+ EXPECT_NEAR(four_legs_desired_pos.right_back.x, test_legs_foot_pos.right_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.y, test_legs_foot_pos.right_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.z, test_legs_foot_pos.right_back.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.right_front.x, test_legs_foot_pos.right_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.y, test_legs_foot_pos.right_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.z, test_legs_foot_pos.right_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_front.x, test_legs_foot_pos.left_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.y, test_legs_foot_pos.left_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.z, test_legs_foot_pos.left_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_back.x, test_legs_foot_pos.left_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.y, test_legs_foot_pos.left_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.z, test_legs_foot_pos.left_back.z, tol);
+
+ // Test that joint angles are different
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang1 -
+ test_legs_joint_angs.right_back.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang2 -
+ test_legs_joint_angs.right_back.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang3 -
+ test_legs_joint_angs.right_back.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang1 -
+ test_legs_joint_angs.right_front.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang2 -
+ test_legs_joint_angs.right_front.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang3 -
+ test_legs_joint_angs.right_front.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang1 -
+ test_legs_joint_angs.left_front.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang2 -
+ test_legs_joint_angs.left_front.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang3 -
+ test_legs_joint_angs.left_front.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang1 -
+ test_legs_joint_angs.left_back.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang2 -
+ test_legs_joint_angs.left_back.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang3 -
+ test_legs_joint_angs.left_back.ang3) > tol);
+}
+
+
+TEST(testSetBodyPos, basic_test)
+{
+
+ float d2r = M_PI/180.0f;
+
+ // Create a spot micro kinematics object, set reachable foot positions given
+ // the leg and body geometry.
+ //
+ // Get the joint angles.
+ //
+ // Set body position
+ //
+ // Get the feet position and joint angles
+ //
+ // Verify the feet position still match after all operations, but that the
+ // joint angles are different (verifying the legs actually changes but the
+ // feet are in the same spot)
+
+
+ SpotMicroConfig smc = {0.1, 0.4, 0.4, 0.4, 0.5};
+ SpotMicroKinematics sm = SpotMicroKinematics(0.05f, 0.03f, 0.01f, smc);
+
+ LegsFootPos four_legs_desired_pos = {.right_back = {-0.22, -0.22, 0.32},
+ .right_front = {0.22, -0.22, 0.22},
+ .left_front = {0.22, -0.22, -0.22},
+ .left_back = {-0.22, -0.22, -0.22}};
+
+ sm.setFeetPosGlobalCoordinates(four_legs_desired_pos);
+
+ LegsJointAngles truth_joint_angles = sm.getLegsJointAngles();
+
+ sm.setBodyPosition(0.052f, -0.053f, 0.05f);
+
+ LegsFootPos test_legs_foot_pos = sm.getLegsFootPos();
+ LegsJointAngles test_legs_joint_angs = sm.getLegsJointAngles();
+
+ // Test value nearness to 0.0001f
+ float tol = 0.0001f;
+ EXPECT_NEAR(four_legs_desired_pos.right_back.x, test_legs_foot_pos.right_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.y, test_legs_foot_pos.right_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_back.z, test_legs_foot_pos.right_back.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.right_front.x, test_legs_foot_pos.right_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.y, test_legs_foot_pos.right_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.right_front.z, test_legs_foot_pos.right_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_front.x, test_legs_foot_pos.left_front.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.y, test_legs_foot_pos.left_front.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_front.z, test_legs_foot_pos.left_front.z, tol);
+
+ EXPECT_NEAR(four_legs_desired_pos.left_back.x, test_legs_foot_pos.left_back.x, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.y, test_legs_foot_pos.left_back.y, tol);
+ EXPECT_NEAR(four_legs_desired_pos.left_back.z, test_legs_foot_pos.left_back.z, tol);
+
+ // Test that joint angles are different
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang1 -
+ test_legs_joint_angs.right_back.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang2 -
+ test_legs_joint_angs.right_back.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_back.ang3 -
+ test_legs_joint_angs.right_back.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang1 -
+ test_legs_joint_angs.right_front.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang2 -
+ test_legs_joint_angs.right_front.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.right_front.ang3 -
+ test_legs_joint_angs.right_front.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang1 -
+ test_legs_joint_angs.left_front.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang2 -
+ test_legs_joint_angs.left_front.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_front.ang3 -
+ test_legs_joint_angs.left_front.ang3) > tol);
+
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang1 -
+ test_legs_joint_angs.left_back.ang1) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang2 -
+ test_legs_joint_angs.left_back.ang2) > tol);
+ EXPECT_TRUE(abs(truth_joint_angles.left_back.ang3 -
+ test_legs_joint_angs.left_back.ang3) > tol);
+}
+
+
+TEST(testReturnBodyState, basic_test)
+{
+ float d2r = M_PI/180.0;
+ float truth_x = 0.1; float truth_y = 0.12; float truth_z = 0.13;
+ float truth_phi = 0.1; float truth_theta = 0.05f; float truth_psi = -0.02f;
+
+ // Create a spot micro kinematics object
+ SpotMicroConfig smc = {0.1, 0.2, 0.3, 0.4, 0.5};
+
+ SpotMicroKinematics sm = SpotMicroKinematics(truth_x, truth_y, truth_z, smc);
+
+ JointAngles desired = {10*d2r, 20*d2r, -15*d2r};
+
+ LegsJointAngles four_legs_desired_angs = {desired, desired, desired, desired};
+
+ sm.setLegJointAngles(four_legs_desired_angs);
+
+ sm.setBodyAngles(truth_phi, truth_theta, truth_psi);
+
+ BodyState test_body_state = sm.getBodyState();
+
+ // Test equality
+ EXPECT_FLOAT_EQ(truth_x, test_body_state.xyz_pos.x);
+ EXPECT_FLOAT_EQ(truth_y, test_body_state.xyz_pos.y);
+ EXPECT_FLOAT_EQ(truth_z, test_body_state.xyz_pos.z);
+
+ EXPECT_FLOAT_EQ(truth_phi, test_body_state.euler_angs.phi);
+ EXPECT_FLOAT_EQ(truth_theta, test_body_state.euler_angs.theta);
+ EXPECT_FLOAT_EQ(truth_psi, test_body_state.euler_angs.psi);
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..18275a37be81802362124a7eab6a1894c9181855
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp
@@ -0,0 +1,91 @@
+#include <iostream>
+
+#include <eigen3/Eigen/Geometry>
+#include <gtest/gtest.h>
+
+#include "spot_micro_kinematics/spot_micro_leg.h"
+
+using namespace std;
+using namespace Eigen;
+using namespace smk;
+
+TEST(setFootPos, local_coordinates)
+{
+ // Create a leg object with a homogeneous transform that is
+ // far translated. Call method to set a reachable point within the legs own
+ // coordinate system (but which would not be a point reachable if it were in a
+ // global coordinate system).
+ // Verify foot placed at that point.
+ float d2r = M_PI/180.0f;
+
+ Point desired_point = {0.02f, -0.2f, 0.01f};
+ JointAngles joint_angles = {0.0f, 0.0f, 0.0f};
+ LinkLengths link_lengths = {0.1f, 0.4f, 0.4f};
+
+ // Sample body ht that is far translated
+ Matrix4f body_ht = homogTransXyz(10.0f, 20.0f, 30.0f) *
+ homogRotXyz(5*d2r, 5*d2r, 5*d2r);
+
+ // Make the leg start ht, put in zero boyd width and length
+ Matrix4f ht_leg_start = body_ht * htLegRightFront(0.0f, 0.0f);
+
+ // Create leg object
+ SpotMicroLeg sml = SpotMicroLeg(joint_angles, link_lengths, true);
+
+ // Call method to set foot position in local coordinates
+ sml.setFootPosLocalCoordinates(desired_point);
+
+ // Get foot position in global coordinates
+ Point foot_point_global = sml.getFootPosGlobalCoordinates(ht_leg_start);
+
+ // Create a homogeneous vector with that point
+ Vector4f test_point_vec(foot_point_global.x,
+ foot_point_global.y,
+ foot_point_global.z,
+ 1.0f);
+
+ // Express that position in leg's local coordinate frame
+ auto leg_local_foot_pt = homogInverse(ht_leg_start) * test_point_vec;
+
+ // Test equality to 0.0001
+ EXPECT_NEAR(desired_point.x, leg_local_foot_pt(0), 0.0001f);
+ EXPECT_NEAR(desired_point.y, leg_local_foot_pt(1), 0.0001f);
+ EXPECT_NEAR(desired_point.z, leg_local_foot_pt(2), 0.0001f);
+}
+
+
+TEST(setFootPos, global_coordinates)
+{
+ // Create a leg object with some non zero starting homogeneous trasnform, and
+ // leg links. Call method to set a reachable point. Verify foot placed at that
+ // point.
+ float d2r = M_PI/180.0f;
+
+ Point desired_point = {0.02f, 0.01f, 0.01f};
+ JointAngles joint_angles = {0.0f, 0.0f, 0.0f};
+ LinkLengths link_lengths = {0.1f, 0.4f, 0.4f};
+
+ // Sample body ht that is translated and rotated slightly
+ Matrix4f body_ht = homogTransXyz(0.05f, 0.14f, 0.05f) *
+ homogRotXyz(5*d2r, 5*d2r, 5*d2r);
+
+ // Make the leg start ht, put in zero boyd width and length
+ Matrix4f ht_leg_start = body_ht * htLegRightFront(0.0f, 0.0f);
+
+ // Create leg object
+ SpotMicroLeg sml = SpotMicroLeg(joint_angles, link_lengths, true);
+
+ // Call method to set foot position in global coordinates
+ sml.setFootPosGlobalCoordinates(desired_point, ht_leg_start);
+
+ // Get foot position
+ Point test_point = sml.getFootPosGlobalCoordinates(ht_leg_start);
+
+ // Test equality to 0.0001
+ EXPECT_NEAR(desired_point.x, test_point.x, 0.0001f);
+ EXPECT_NEAR(desired_point.y, test_point.y, 0.0001f);
+ EXPECT_NEAR(desired_point.z, test_point.z, 0.0001f);
+}
+
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd8db2391359f885c7482e86247dc714309ef7c7
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp
@@ -0,0 +1,443 @@
+#include <iostream>
+
+#include <eigen3/Eigen/Geometry>
+#include <gtest/gtest.h>
+
+#include "spot_micro_kinematics/utils.h"
+
+using namespace std;
+using namespace Eigen;
+
+TEST(homogeneousRotXyz, x_rotation)
+{
+
+ // Manually create a rotation matrix with a x rotation
+ float ang = 10*M_PI/180.0;
+ Matrix4f truth;
+ truth << 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, cos(ang), -sin(ang), 0.0f,
+ 0.0f, sin(ang), cos(ang), 0.0f,
+ 0.00, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f test_val = smk::homogRotXyz(ang,0.0f,0.0f);
+
+ EXPECT_TRUE(truth.isApprox(test_val));
+
+}
+
+
+
+TEST(homogeneousRotXyz, y_rotation)
+{
+
+ // Manually create a rotation matrix with a y rotation
+ float ang = 10*M_PI/180.0;
+ Matrix4f truth;
+ truth << cos(ang), 0.0f, sin(ang), 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ -sin(ang), 0.0f, cos(ang), 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f test_val = smk::homogRotXyz(0.0f, ang, 0.0f);
+
+ EXPECT_TRUE(truth.isApprox(test_val));
+
+}
+
+
+TEST(homogeneousRotXyz, z_rotation)
+{
+
+ // Manually create a rotation matrix with a z rotation
+ float ang = 10*M_PI/180.0;
+ Matrix4f truth;
+ truth << cos(ang), -sin(ang), 0.0f, 0.0f,
+ sin(ang), cos(ang), 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f test_val = smk::homogRotXyz(0.0f, 0.0f, ang);
+
+ EXPECT_TRUE(truth.isApprox(test_val));
+
+}
+
+TEST(homogeneousRotXyz, combo_rotation)
+{
+
+ // Manually create three rotation matrices
+ float ang = 10*M_PI/180.0;
+
+ Matrix4f truthx;
+ truthx<< 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, cos(ang), -sin(ang), 0.0f,
+ 0.0f, sin(ang), cos(ang), 0.0f,
+ 0.00, 0.0f, 0.0f, 1.0f;
+
+
+ Matrix4f truthy;
+ truthy<< cos(ang), 0.0f, sin(ang), 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ -sin(ang), 0.0f, cos(ang), 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f truthz;
+ truthz<< cos(ang), -sin(ang), 0.0f, 0.0f,
+ sin(ang), cos(ang), 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f truth = truthx * truthy * truthz;
+
+ Matrix4f test_val = smk::homogRotXyz(ang, ang, ang);
+
+ EXPECT_TRUE(truth.isApprox(test_val));
+
+}
+
+
+TEST(homogeneousTranslate, xyz_translate)
+{
+
+ // Manually create a homogeneous translation matrix
+ Matrix4f truth;
+ truth << 1.0f, 0.0f, 0.0f, 1.4f,
+ 0.0f, 1.0f, 0.0f, -3.2f,
+ 0.0f, 0.0f, 1.0f, 10.8f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ Matrix4f test_val = smk::homogTransXyz(1.4,-3.2,10.8);
+
+ EXPECT_TRUE(truth.isApprox(test_val));
+
+}
+
+
+TEST(homogeneousInverse, ht_inverse)
+{
+
+ // Test by running a forward transformation on a set of coordinates, then
+ // computing the homogeneous transformation inverse, and multiplying on the
+ // transformed coordinates, and verifying the result matches the beggining
+ // coordinates
+
+ Vector3f truth_point = Vector3f(0.23, 10.0, 2.56);
+
+ Matrix4f ht_rot = smk::homogRotXyz(0.3, 0.12, -1.2);
+
+ Matrix4f ht_trans = smk::homogTransXyz(20.1, -100.21, 2.3);
+
+ auto ht = ht_rot * ht_trans;
+
+ auto transformed_point = ht * truth_point.homogeneous();
+
+ // Now calculate the inverse homogeneous transform matrix
+ auto ht_inv = smk::homogInverse(ht);
+
+ // Multiply transformed point by this matrix to get the original point back
+ auto test_point = ht_inv * transformed_point;
+
+ EXPECT_TRUE(truth_point.isApprox(test_point.block<3,1>(0,0)));
+
+}
+
+
+TEST(htLeg, rightback)
+{
+ // Create a body homogeneous transform, and verify the leg ht matrix comes out
+ // as expected
+
+ float len = 2.0f;
+ float width = 4.0f;
+ float height = 0.14f;
+
+ Matrix4f ht_rightback_test = smk::htLegRightBack(len, width);
+
+ Matrix4f ht_rightback_truth = Matrix4f::Identity();
+
+ ht_rightback_truth <<
+ cos(M_PI/2.0f), 0.0f, sin(M_PI/2.0f), -len/2.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ -sin(M_PI/2.0f), 0.0f, cos(M_PI/2.0f), width/2.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_rightback_truth.isApprox(ht_rightback_test));
+
+}
+
+
+TEST(htLeg, rightfront)
+{
+ // Create a body homogeneous transform, and verify the leg ht matrix comes out
+ // as expected
+
+ float len = 2.0f;
+ float width = 4.0f;
+ float height = 0.14f;
+
+ Matrix4f ht_rightfront_test = smk::htLegRightFront(len, width);
+
+ Matrix4f ht_rightfront_truth = Matrix4f::Identity();
+
+ ht_rightfront_truth <<
+ cos(M_PI/2.0f), 0.0f, sin(M_PI/2.0f), len/2.0f,
+ 0.0f, 1.0f, 0.0f, 0.0,
+ -sin(M_PI/2.0f), 0.0f, cos(M_PI/2.0f), width/2.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_rightfront_truth.isApprox(ht_rightfront_test));
+
+}
+
+
+TEST(htLeg, leftfront)
+{
+ // Create a body homogeneous transform, and verify the leg ht matrix comes out
+ // as expected
+
+ float len = 2.0f;
+ float width = 4.0f;
+ float height = 0.14f;
+
+ Matrix4f body_ht = smk::homogRotXyz(0.0f, 0.0f, 0.0f) *
+ smk::homogTransXyz(0.0f, -height, 0.0f);
+
+ Matrix4f ht_leftfront_test = smk::htLegLeftFront(len, width);
+
+ Matrix4f ht_leftfront_truth = Matrix4f::Identity();
+
+ ht_leftfront_truth <<
+ cos(-M_PI/2.0f), 0.0f, sin(-M_PI/2.0f), len/2.0f,
+ 0.0f, 1.0f, 0.0f, 0.0,
+ -sin(-M_PI/2.0f), 0.0f, cos(-M_PI/2.0f), -width/2.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_leftfront_truth.isApprox(ht_leftfront_test));
+
+}
+
+TEST(htLeg, leftback)
+{
+ // Create a body homogeneous transform, and verify the leg ht matrix comes out
+ // as expected
+
+ float len = 2.0f;
+ float width = 4.0f;
+ float height = 0.14f;
+
+ Matrix4f ht_leftback_test = smk::htLegLeftBack(len, width);
+
+ Matrix4f ht_leftback_truth = Matrix4f::Identity();
+
+ ht_leftback_truth <<
+ cos(-M_PI/2.0f), 0.0f, sin(-M_PI/2.0f), -len/2.0f,
+ 0.0f, 1.0f, 0.0f, 0.0,
+ -sin(-M_PI/2.0f), 0.0f, cos(-M_PI/2.0f), -width/2.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_leftback_truth.isApprox(ht_leftback_test));
+
+}
+
+
+TEST(htLegLinkTransforms, ht0To1)
+{
+
+ // Test the homogenous transform from link 0 to 1 via a contrived example
+
+ float rot_ang = 35.0f;
+ float link_length = 0.4;
+
+ Matrix4f ht_0_to_1_test = smk::ht0To1(rot_ang, link_length);
+
+ Matrix4f ht_0_to_1_truth = Matrix4f::Identity();
+
+ ht_0_to_1_truth <<
+ cos(rot_ang), -sin(rot_ang), 0.0f, -link_length*cos(rot_ang),
+ sin(rot_ang), cos(rot_ang), 0.0f, -link_length*sin(rot_ang),
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_0_to_1_truth.isApprox(ht_0_to_1_test));
+
+}
+
+
+TEST(htLegLinkTransforms, ht1To2)
+{
+
+ // Test the homogenous transform from link 1 to 2 via a contrived example
+
+ Matrix4f ht_1_to_2_test = smk::ht1To2();
+
+ Matrix4f ht_1_to_2_truth = Matrix4f::Identity();
+
+ ht_1_to_2_truth <<
+ 0.0f, 0.0f, -1.0f, 0.0f,
+ -1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_1_to_2_truth.isApprox(ht_1_to_2_test));
+
+}
+
+
+TEST(htLegLinkTransforms, ht2To3)
+{
+ // Test the homogenous transform from link 2 to 3 via a contrived example
+
+ float rot_ang = 35.0f;
+ float link_length = 0.4;
+
+ Matrix4f ht_2_to_3_test = smk::ht2To3(rot_ang, link_length);
+
+ Matrix4f ht_2_to_3_truth = Matrix4f::Identity();
+
+ ht_2_to_3_truth <<
+ cos(rot_ang), -sin(rot_ang), 0.0f, link_length*cos(rot_ang),
+ sin(rot_ang), cos(rot_ang), 0.0f, link_length*sin(rot_ang),
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f;
+
+ EXPECT_TRUE(ht_2_to_3_truth.isApprox(ht_2_to_3_test));
+
+}
+
+// Not testing ht3To4 because it executes the same exact function as ht2To3
+
+TEST(htLegLinkTransforms, ht0To4)
+{
+ // Test the homogenous transform from link 0 to 4 via a contrived example
+
+ float ang1 = 35.0f;
+ float ang2 = 22.1f;
+ float ang3 = -18.23f;
+ float link1 = 0.4;
+ float link2 = 0.11;
+ float link3 = 0.68;
+
+ smk::JointAngles joint_angles = {ang1, ang2, ang3};
+ smk::LinkLengths link_lengths = {link1, link2, link3};
+
+ Matrix4f ht_0_to_1 = smk::ht0To1(ang1, link1);
+ Matrix4f ht_1_to_2 = smk::ht1To2();
+ Matrix4f ht_2_to_3 = smk::ht2To3(ang2, link2);
+ Matrix4f ht_3_to_4 = smk::ht3To4(ang3, link3);
+
+ Matrix4f ht_0_to_4_truth = ht_0_to_1 * ht_1_to_2 * ht_2_to_3 * ht_3_to_4;
+
+ Matrix4f ht_0_to_4_test = smk::ht0To4(joint_angles, link_lengths);
+
+ EXPECT_TRUE(ht_0_to_4_truth.isApprox(ht_0_to_4_test));
+
+}
+
+
+TEST(inverseKinematics, simpleFootPlacement)
+{
+ // Command a reachable foot position and verify the foot position ends up
+ // there
+
+ float x4_desired = 0.1f;
+ float y4_desired = -0.4f;
+ float z4_desired = 0.1f;
+ float x4_test = 0.0f;
+ float y4_test = 0.0f;
+ float z4_test = 0.0f;
+
+ float link1 = 0.1f;
+ float link2 = 0.4f;
+ float link3 = 0.5f;
+
+ smk::JointAngles joint_angles;
+ smk::LinkLengths link_lengths = {link1, link2, link3};
+ smk::Point point = {x4_desired, y4_desired, z4_desired};
+
+ joint_angles = smk::ikine(point, link_lengths, true);
+
+ // Now run forward kinematics and get position of foot
+ Matrix4f ht_0_to_4 = smk::ht0To4(joint_angles, link_lengths);
+
+ // Get foot positions from matrix, the linear portion
+ x4_test = ht_0_to_4(0,3);
+ y4_test = ht_0_to_4(1,3);
+ z4_test = ht_0_to_4(2,3);
+
+ EXPECT_FLOAT_EQ(x4_desired, x4_test);
+ EXPECT_FLOAT_EQ(y4_desired, y4_test);
+ EXPECT_FLOAT_EQ(z4_desired, z4_test);
+}
+
+
+TEST(inverseKinematics, simpleFootPlacementLeg34)
+{
+ // Command a reachable foot position and verify the foot position ends up
+ // there, commanding as leg 34
+
+ float x4_desired = 0.1f;
+ float y4_desired = -0.4f;
+ float z4_desired = 0.1f;
+ float x4_test = 0.0f;
+ float y4_test = 0.0f;
+ float z4_test = 0.0f;
+
+ float link1 = 0.1f;
+ float link2 = 0.4f;
+ float link3 = 0.5f;
+
+ smk::JointAngles joint_angles;
+ smk::LinkLengths link_lengths = {link1, link2, link3};
+ smk::Point point = {x4_desired, y4_desired, z4_desired};
+
+ // Leg 34 signified by passing a boolean of false at the end
+ joint_angles = smk::ikine(point, link_lengths, true);
+
+ // Now run forward kinematics and get position of foot
+ Matrix4f ht_0_to_4 = smk::ht0To4(joint_angles, link_lengths);
+
+ // Get foot positions from matrix, the linear portion
+ x4_test = ht_0_to_4(0,3);
+ y4_test = ht_0_to_4(1,3);
+ z4_test = ht_0_to_4(2,3);
+
+ EXPECT_FLOAT_EQ(x4_desired, x4_test);
+ EXPECT_FLOAT_EQ(y4_desired, y4_test);
+ EXPECT_FLOAT_EQ(z4_desired, z4_test);
+}
+
+TEST(inverseKinematics, unreachableFootPlacement)
+{
+ // Command a unreachable foot position and verify the foot position doesn't
+ // end up there, and that the program doesnt crash (by nature of unit test
+ // completing)
+
+ float x4_desired = 10.0f;
+ float y4_desired = -10.0f;
+ float z4_desired = 10.0f;
+ float x4_test = 0.0f;
+ float y4_test = 0.0f;
+ float z4_test = 0.0f;
+
+ float link1 = 0.1f;
+ float link2 = 0.4f;
+ float link3 = 0.5f;
+
+ smk::JointAngles joint_angles;
+ smk::LinkLengths link_lengths = {link1, link2, link3};
+ smk::Point point = {x4_desired, y4_desired, z4_desired};
+
+ joint_angles = smk::ikine(point, link_lengths, true);
+
+ // Now run forward kinematics and get position of foot
+ Matrix4f ht_0_to_4 = smk::ht0To4(joint_angles, link_lengths);
+
+ // Get foot positions from matrix, the linear portion
+ x4_test = ht_0_to_4(0,3);
+ y4_test = ht_0_to_4(1,3);
+ z4_test = ht_0_to_4(2,3);
+
+ EXPECT_NE(x4_desired, x4_test);
+ EXPECT_NE(y4_desired, y4_test);
+ EXPECT_NE(z4_desired, z4_test);
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..50e7de514a0ce49051dfad4698944e2f7395eda1
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/package.xml
@@ -0,0 +1,80 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_motion_cmd</name>
+ <version>0.0.0</version>
+ <description>The spot_micro_motion_cmd package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/spot_micro_motion_cmd</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>i2cpwm_board</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>tf2</build_depend>
+ <build_depend>tf2_ros</build_depend>
+ <build_depend>tf2_geometry_msgs</build_depend>
+ <build_export_depend>i2cpwm_board</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <build_export_depend>geometry_msgs</build_export_depend>
+ <build_export_depend>tf2</build_export_depend>
+ <build_export_depend>tf2_ros</build_export_depend>
+ <build_export_depend>tf2_geometry_msgs</build_export_depend>
+ <exec_depend>i2cpwm_board</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>tf2</exec_depend>
+ <exec_depend>tf2_ros</exec_depend>
+ <exec_depend>tf2_geometry_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..933dcb8426f207851efb4d80b2e7b8fe748aaa97
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h
@@ -0,0 +1,90 @@
+#pragma once
+
+#include <cmath>
+
+// Encapsulates a first order filter with rate limiting
+// Can be used to extract a first order time history response.
+// Major assumption of use is that the object is called at a fixed sample time
+//
+// Atributes:
+// dt_: sample time in seconds
+// tau_: time constant in seconds
+// state_: internal state of the filter
+// cmd_: Command value
+// alpha_: derived constant from sample time and time constant
+//
+// This filter assumes the following equations:
+// - y[i] is the current output, current state
+// - y[i-1] is the previous output, previous state
+// - u[i] is the current command
+//
+// y[0], u[0] = x0
+//
+// y[i] = (1 - alpha) * y[i-1] + alpha * u[i]
+//
+// alpha = dt / (tau + dt)
+//
+// If y[i] - y[0] exeeds the rate limit, then y[i] is limited to an
+// increment cooresponding to the rate limit
+class RateLmtdFirstOrderFilter {
+ private:
+ float dt_{0.001f};
+ float tau_;
+ float state_;
+ float cmd_;
+ float alpha_;
+ float rate_limit_;
+
+ public:
+ // Constructor
+ RateLmtdFirstOrderFilter(float dt, float tau, float x0, float rate_limit)
+ : dt_(dt),
+ tau_(tau),
+ state_(x0),
+ cmd_(x0),
+ alpha_(dt/(tau+dt)),
+ rate_limit_(rate_limit) {}
+
+ RateLmtdFirstOrderFilter() = default;
+
+ void setCommand(float cmd) {
+ cmd_ = cmd;
+ }
+
+ float runTimestepAndGetOutput() {
+ runTimestep();
+ return state_;
+ }
+
+void runTimestep() {
+ // Convenience variables
+ float a = alpha_;
+ float y_prev = state_;
+ float u = cmd_;
+
+ // Update equation
+ float y = (1-a)*y_prev + a*u;
+
+ // Enforce rate limit
+ float rate = (y-y_prev)/dt_;
+ if (std::fabs(rate) > rate_limit_) {
+ if (rate > 0.0f) {
+ y = y_prev + rate_limit_*dt_;
+ } else {
+ y = y_prev - rate_limit_*dt_;
+ }
+ }
+ state_ = y;
+}
+
+
+float getOutput() const {
+ return state_;
+}
+
+
+void resetState(float x0) {
+ state_ = x0;
+}
+
+};
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/command.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/command.h
new file mode 100644
index 0000000000000000000000000000000000000000..0086743d50db99f80927f69b24407bb326635773
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/command.h
@@ -0,0 +1,94 @@
+#pragma once
+
+#include "std_msgs/Float32.h"
+#include "std_msgs/Bool.h"
+
+
+class Command {
+ public:
+
+ float x_vel_cmd_mps_;
+ float y_vel_cmd_mps_;
+ float yaw_rate_cmd_rps_;
+ float phi_cmd_;
+ float theta_cmd_;
+ float psi_cmd_;
+
+ bool idle_cmd_;
+ bool walk_cmd_;
+ bool stand_cmd_;
+
+ //New object detected bool
+ bool obj_det_;
+
+ // Constructor
+ Command()
+ : x_vel_cmd_mps_(0.0)
+ , y_vel_cmd_mps_(0.0)
+ , yaw_rate_cmd_rps_(0.0)
+ , phi_cmd_(0.0f)
+ , theta_cmd_(0.0f)
+ , psi_cmd_(0.0f)
+ , idle_cmd_(false)
+ , walk_cmd_(false)
+ , stand_cmd_(false)
+ { }
+
+ bool getObjectDetected() const {
+ // Object detected getter
+ return obj_det_;
+ }
+
+ bool getStandCmd() const {
+ // stand cmd status getter.
+ return stand_cmd_;
+ }
+
+ bool getIdleCmd() const {
+ return idle_cmd_;
+ }
+
+ bool getWalkCmd() const {
+ return walk_cmd_;
+ }
+
+ float getXSpeedCmd() const {
+ return x_vel_cmd_mps_;
+ }
+
+ float getYSpeedCmd() const {
+ return y_vel_cmd_mps_;
+ }
+
+ float getYawRateCmd() const {
+ return yaw_rate_cmd_rps_;
+ }
+
+ float getPhiCmd() const {
+ return phi_cmd_;
+ }
+
+ float getThetaCmd() const {
+ return theta_cmd_;
+ }
+
+ float getPsiCmd() const {
+ return psi_cmd_;
+ }
+
+ void resetAllCommands() {
+ x_vel_cmd_mps_ = 0;
+ y_vel_cmd_mps_ = 0;
+ yaw_rate_cmd_rps_ = 0;
+ phi_cmd_ = 0;
+ theta_cmd_ = 0;
+ psi_cmd_ = 0;
+ }
+
+ void resetEventCmds() {
+ // Reset all event commands to false
+ idle_cmd_ = false;
+ walk_cmd_ = false;
+ stand_cmd_ = false;
+ }
+};
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1f9597c8c9ee492699c686cc5dff32ab1413db64
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp
@@ -0,0 +1,33 @@
+#include "spot_micro_idle.h"
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_transition_stand.h"
+
+SpotMicroIdleState::SpotMicroIdleState() {
+ // Construcotr, doesn't need to do anything, for now...
+ //std::cout << "SpotMicroIdleState Ctor" << std::endl;
+}
+
+SpotMicroIdleState::~SpotMicroIdleState() {
+ //std::cout << "SpotMicroIdleState Dtor" << std::endl;
+}
+
+void SpotMicroIdleState::handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd_) {
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Idle State" << std::endl;
+ }
+
+ // Check if stand command issued, if so, transition to stand state
+ if (cmd.getStandCmd() == true) {
+ changeState(smmc, std::make_unique<SpotMicroTransitionStandState>());
+
+ } else {
+ // Otherwise, just command idle servo commands
+ smmc->publishZeroServoAbsoluteCommand();
+ }
+
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h
new file mode 100644
index 0000000000000000000000000000000000000000..aee34c25d41f4249ec9d6dafee22816201ee2fed
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <iostream>
+
+#include "spot_micro_state.h"
+#include "command.h"
+
+
+class SpotMicroIdleState : public SpotMicroState {
+ public:
+ SpotMicroIdleState(); // Constructor
+ ~SpotMicroIdleState(); // Destructor
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual std::string getCurrentStateName() {
+ return "Idle";
+ }
+};
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58a9a01eb272a53705f9e5d8a0dbf14e7f63dfc9
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp
@@ -0,0 +1,33 @@
+#include "spot_micro_object.h"
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_transition_stand.h"
+
+SpotMicroObjectDetectedState::SpotMicroObjectDetectedState() {
+ // Construcotr, doesn't need to do anything, for now...
+ //std::cout << "SpotMicroIdleState Ctor" << std::endl;
+}
+
+SpotMicroObjectDetectedState::~SpotMicroObjectDetectedState() {
+ //std::cout << "SpotMicroIdleState Dtor" << std::endl;
+}
+
+void SpotMicroObjectDetectedState::handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd_) {
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Object Detected State" << std::endl;
+ }
+
+ // Check if object is detected anymore, if not transition to the stand
+ if (!(cmd.getObjectDetected())) {
+ //changeState(smmc, std::make_unique<SpotMicroTransitionStandState>());
+
+ } else {
+ // Otherwise, just carry on being in object detection state
+ //smmc->publishZeroServoAbsoluteCommand();
+ }
+
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h
new file mode 100644
index 0000000000000000000000000000000000000000..042e5b8a5077937b9edf9b2215ebad9f5efe81c3
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <iostream>
+
+#include "spot_micro_state.h"
+#include "command.h"
+
+
+class SpotMicroObjectDetectedState : public SpotMicroState {
+ public:
+ SpotMicroObjectDetectedState(); // Constructor
+ ~SpotMicroObjectDetectedState(); // Destructor
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual std::string getCurrentStateName() {
+ return "Dodging";
+ }
+};
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..396afa0795beaf9a493fd24b358af77c86fcc759
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp
@@ -0,0 +1,97 @@
+#include "spot_micro_stand.h"
+
+#include "spot_micro_transition_idle.h"
+#include "spot_micro_walk.h"
+#include "spot_micro_motion_cmd.h"
+#include "rate_limited_first_order_filter.h"
+
+SpotMicroStandState::SpotMicroStandState() {
+ // Construcotr, doesn't need to do anything, for now...
+ //std::cout << "SpotMicroStandState Ctor" << std::endl;
+}
+
+SpotMicroStandState::~SpotMicroStandState() {
+ //std::cout << "SpotMicroStandState Dtor" << std::endl;
+}
+
+void SpotMicroStandState::handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd) {
+
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Stand State" << std::endl;
+ }
+
+
+
+ if (cmd.getIdleCmd() == true) {
+ // Call parent class's change state method
+ changeState(smmc, std::make_unique<SpotMicroTransitionIdleState>());
+
+ } else if (cmd.getWalkCmd() == true) {
+ changeState(smmc, std::make_unique<SpotMicroWalkState>());
+
+ } else {
+ // Get command values
+ cmd_state_.euler_angs.phi = cmd.getPhiCmd();
+ cmd_state_.euler_angs.theta = cmd.getThetaCmd();
+ cmd_state_.euler_angs.psi = cmd.getPsiCmd();
+
+ // Set command to filters
+ angle_cmd_filters_.x.setCommand(cmd_state_.euler_angs.phi);
+ angle_cmd_filters_.y.setCommand(cmd_state_.euler_angs.theta);
+ angle_cmd_filters_.z.setCommand(cmd_state_.euler_angs.psi);
+
+ // Run Filters and get command values
+ body_state_cmd->euler_angs.phi =
+ angle_cmd_filters_.x.runTimestepAndGetOutput();
+ body_state_cmd->euler_angs.theta =
+ angle_cmd_filters_.y.runTimestepAndGetOutput();
+ body_state_cmd->euler_angs.psi =
+ angle_cmd_filters_.z.runTimestepAndGetOutput();
+
+ body_state_cmd->xyz_pos = cmd_state_.xyz_pos;
+
+ body_state_cmd->leg_feet_pos = cmd_state_.leg_feet_pos;
+
+ // Set and publish command
+ smmc->setServoCommandMessageData();
+ smmc->publishServoProportionalCommand();
+ }
+}
+
+
+
+void SpotMicroStandState::init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc) {
+ // Set default stance
+ cmd_state_.leg_feet_pos = smmc->getNeutralStance();
+
+ // End body state position and angles
+ cmd_state_.euler_angs.phi = 0.0f;
+ cmd_state_.euler_angs.theta = 0.0f;
+ cmd_state_.euler_angs.psi = 0.0f;
+
+ cmd_state_.xyz_pos.x = 0.0f;
+ cmd_state_.xyz_pos.y = smnc.default_stand_height;
+ cmd_state_.xyz_pos.z = 0.0f;
+
+ float dt = smnc.dt;
+ float tau = smnc.transit_tau;
+ float rate_limit = smnc.transit_angle_rl;
+
+ typedef RateLmtdFirstOrderFilter rlof;
+
+ angle_cmd_filters_.x =
+ rlof(dt, tau, cmd_state_.euler_angs.phi, rate_limit);
+ angle_cmd_filters_.y =
+ rlof(dt, tau, cmd_state_.euler_angs.theta, rate_limit);
+ angle_cmd_filters_.z =
+ rlof(dt, tau, cmd_state_.euler_angs.psi, rate_limit);
+
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c7157bb1762e41c31cec8340526db9c70af1d5d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "spot_micro_state.h"
+#include "command.h"
+#include <iostream>
+
+
+class SpotMicroStandState : public SpotMicroState {
+ public:
+ SpotMicroStandState(); // Constructor
+ ~SpotMicroStandState(); // Destructor
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual void init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc);
+
+ // Returns current state name as a string
+ virtual std::string getCurrentStateName() {
+ return "Stand";
+ }
+
+ private:
+ smk::BodyState cmd_state_;
+
+ // Three filters for angle commands
+ XyzFilters angle_cmd_filters_;
+};
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e23f259bd77e34169ac94d449ca1856e1dc652d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp
@@ -0,0 +1,275 @@
+#include "spot_micro_state.h"
+
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+#include "spot_micro_motion_cmd.h"
+
+using namespace smk;
+
+// Constructor
+SpotMicroState::SpotMicroState() {
+ // Nothing needs to be done
+ //std::cout << "SpotMicroState Ctor" << std::endl;
+}
+
+
+SpotMicroState::~SpotMicroState() {
+ //std::cout << "SpotMicroState Dtor" << std::endl;
+}
+
+
+void SpotMicroState::changeState(SpotMicroMotionCmd* smmc, std::unique_ptr<SpotMicroState> sms) {
+ smmc->changeState(std::move(sms));
+}
+
+
+void SpotMicroState::initBodyStateFilters(
+ float dt, float tau, float rl, float rl_ang,
+ const smk::BodyState& body_state,
+ BodyStateFilters* body_state_filters) {
+
+ // typedef for convenience
+ typedef RateLmtdFirstOrderFilter rlfof;
+
+ // Create and initialize filter objects
+ body_state_filters->leg_right_front =
+ {.x = rlfof(dt, tau, body_state.leg_feet_pos.right_front.x, rl),
+ .y = rlfof(dt, tau, body_state.leg_feet_pos.right_front.y, rl),
+ .z = rlfof(dt, tau, body_state.leg_feet_pos.right_front.z, rl)};
+
+ body_state_filters->leg_right_back =
+ {.x = rlfof(dt, tau, body_state.leg_feet_pos.right_back.x, rl),
+ .y = rlfof(dt, tau, body_state.leg_feet_pos.right_back.y, rl),
+ .z = rlfof(dt, tau, body_state.leg_feet_pos.right_back.z, rl)};
+
+ body_state_filters->leg_left_back =
+ {.x = rlfof(dt, tau, body_state.leg_feet_pos.left_back.x, rl),
+ .y = rlfof(dt, tau, body_state.leg_feet_pos.left_back.y, rl),
+ .z = rlfof(dt, tau, body_state.leg_feet_pos.left_back.z, rl)};
+
+ body_state_filters->leg_left_front =
+ {.x = rlfof(dt, tau, body_state.leg_feet_pos.left_front.x, rl),
+ .y = rlfof(dt, tau, body_state.leg_feet_pos.left_front.y, rl),
+ .z = rlfof(dt, tau, body_state.leg_feet_pos.left_front.z, rl)};
+
+ body_state_filters->body_pos =
+ {.x = rlfof(dt, tau, body_state.xyz_pos.x, rl),
+ .y = rlfof(dt, tau, body_state.xyz_pos.y, rl),
+ .z = rlfof(dt, tau, body_state.xyz_pos.z, rl)};
+
+ body_state_filters->body_angs =
+ {.x = rlfof(dt, tau, body_state.euler_angs.phi, rl_ang),
+ .y = rlfof(dt, tau, body_state.euler_angs.theta, rl_ang),
+ .z = rlfof(dt, tau, body_state.euler_angs.psi, rl_ang)};
+}
+
+
+void SpotMicroState::setBodyStateFilterCommands(
+ const smk::BodyState& body_state,
+ BodyStateFilters* body_state_filters) {
+
+ // Set commands for all filters
+ // Right front leg
+ body_state_filters->leg_right_front.x.setCommand(
+ body_state.leg_feet_pos.right_front.x);
+
+ body_state_filters->leg_right_front.y.setCommand(
+ body_state.leg_feet_pos.right_front.y);
+
+ body_state_filters->leg_right_front.z.setCommand(
+ body_state.leg_feet_pos.right_front.z);
+
+ // Right Back leg
+ body_state_filters->leg_right_back.x.setCommand(
+ body_state.leg_feet_pos.right_back.x);
+
+ body_state_filters->leg_right_back.y.setCommand(
+ body_state.leg_feet_pos.right_back.y);
+
+ body_state_filters->leg_right_back.z.setCommand(
+ body_state.leg_feet_pos.right_back.z);
+
+ // Left Back leg
+ body_state_filters->leg_left_back.x.setCommand(
+ body_state.leg_feet_pos.left_back.x);
+
+ body_state_filters->leg_left_back.y.setCommand(
+ body_state.leg_feet_pos.left_back.y);
+
+ body_state_filters->leg_left_back.z.setCommand(
+ body_state.leg_feet_pos.left_back.z);
+
+ // Left front leg
+ body_state_filters->leg_left_front.x.setCommand(
+ body_state.leg_feet_pos.left_front.x);
+
+ body_state_filters->leg_left_front.y.setCommand(
+ body_state.leg_feet_pos.left_front.y);
+
+ body_state_filters->leg_left_front.z.setCommand(
+ body_state.leg_feet_pos.left_front.z);
+
+ // Body pos
+ body_state_filters->body_pos.x.setCommand(body_state.xyz_pos.x);
+ body_state_filters->body_pos.y.setCommand(body_state.xyz_pos.y);
+ body_state_filters->body_pos.z.setCommand(body_state.xyz_pos.z);
+
+ body_state_filters->body_angs.x.setCommand(body_state.euler_angs.phi);
+ body_state_filters->body_angs.y.setCommand(body_state.euler_angs.theta);
+ body_state_filters->body_angs.z.setCommand(body_state.euler_angs.psi);
+}
+
+
+void SpotMicroState::runFilters(BodyStateFilters* body_state_filters) {
+
+ // xyz body position
+ body_state_filters->body_pos.x.runTimestep();
+ body_state_filters->body_pos.y.runTimestep();
+ body_state_filters->body_pos.z.runTimestep();
+
+ // Phi, theta psi body angles
+ body_state_filters->body_angs.x.runTimestep();
+ body_state_filters->body_angs.y.runTimestep();
+ body_state_filters->body_angs.z.runTimestep();
+
+ // right back leg
+ body_state_filters->leg_right_back.x.runTimestep();
+ body_state_filters->leg_right_back.y.runTimestep();
+ body_state_filters->leg_right_back.z.runTimestep();
+
+ // right front leg
+ body_state_filters->leg_right_front.x.runTimestep();
+ body_state_filters->leg_right_front.y.runTimestep();
+ body_state_filters->leg_right_front.z.runTimestep();
+
+ // right front leg
+ body_state_filters->leg_left_front.x.runTimestep();
+ body_state_filters->leg_left_front.y.runTimestep();
+ body_state_filters->leg_left_front.z.runTimestep();
+
+ // right front leg
+ body_state_filters->leg_left_back.x.runTimestep();
+ body_state_filters->leg_left_back.y.runTimestep();
+ body_state_filters->leg_left_back.z.runTimestep();
+}
+
+void SpotMicroState::assignFilterValuesToBodyState(
+ const BodyStateFilters& body_state_filters,
+ smk::BodyState* body_state) {
+
+ body_state->xyz_pos.x = body_state_filters.body_pos.x.getOutput();
+ body_state->xyz_pos.y = body_state_filters.body_pos.y.getOutput();
+ body_state->xyz_pos.z = body_state_filters.body_pos.z.getOutput();
+
+ body_state->euler_angs.phi = body_state_filters.body_angs.x.getOutput();
+ body_state->euler_angs.theta = body_state_filters.body_angs.y.getOutput();
+ body_state->euler_angs.psi = body_state_filters.body_angs.z.getOutput();
+
+ // right back leg
+ body_state->leg_feet_pos.right_back.x =
+ body_state_filters.leg_right_back.x.getOutput();
+ body_state->leg_feet_pos.right_back.y =
+ body_state_filters.leg_right_back.y.getOutput();
+ body_state->leg_feet_pos.right_back.z =
+ body_state_filters.leg_right_back.z.getOutput();
+
+ // right front leg
+ body_state->leg_feet_pos.right_front.x =
+ body_state_filters.leg_right_front.x.getOutput();
+ body_state->leg_feet_pos.right_front.y =
+ body_state_filters.leg_right_front.y.getOutput();
+ body_state->leg_feet_pos.right_front.z =
+ body_state_filters.leg_right_front.z.getOutput();
+
+ // right front leg
+ body_state->leg_feet_pos.left_front.x =
+ body_state_filters.leg_left_front.x.getOutput();
+ body_state->leg_feet_pos.left_front.y =
+ body_state_filters.leg_left_front.y.getOutput();
+ body_state->leg_feet_pos.left_front.z =
+ body_state_filters.leg_left_front.z.getOutput();
+
+ // right front leg
+ body_state->leg_feet_pos.left_back.x =
+ body_state_filters.leg_left_back.x.getOutput();
+ body_state->leg_feet_pos.left_back.y =
+ body_state_filters.leg_left_back.y.getOutput();
+ body_state->leg_feet_pos.left_back.z =
+ body_state_filters.leg_left_back.z.getOutput();
+}
+
+bool SpotMicroState::checkBodyStateEquality(
+ const smk::BodyState& body_state1,
+ const smk::BodyState& body_state2,
+ float tol) {
+ // Initialize return value true, run checks and set false if any fail
+ bool ret_val = true;
+
+ // right back leg
+ if (std::fabs(body_state1.leg_feet_pos.right_back.x -
+ body_state2.leg_feet_pos.right_back.x) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.right_back.y -
+ body_state2.leg_feet_pos.right_back.y) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.right_back.z -
+ body_state2.leg_feet_pos.right_back.z) > tol) {
+ ret_val = false;
+
+ // right front leg
+ } else if (std::fabs(body_state1.leg_feet_pos.right_front.x -
+ body_state2.leg_feet_pos.right_front.x) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.right_front.y -
+ body_state2.leg_feet_pos.right_front.y) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.right_front.z -
+ body_state2.leg_feet_pos.right_front.z) > tol) {
+ ret_val = false;
+
+ // left front leg
+ } else if (std::fabs(body_state1.leg_feet_pos.left_front.x -
+ body_state2.leg_feet_pos.left_front.x) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.left_front.y -
+ body_state2.leg_feet_pos.left_front.y) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.left_front.z -
+ body_state2.leg_feet_pos.left_front.z) > tol) {
+ ret_val = false;
+
+ // left back leg
+ } else if (std::fabs(body_state1.leg_feet_pos.left_back.x -
+ body_state2.leg_feet_pos.left_back.x) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.left_back.y -
+ body_state2.leg_feet_pos.left_back.y) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.leg_feet_pos.left_back.z -
+ body_state2.leg_feet_pos.left_back.z) > tol) {
+ ret_val = false;
+
+ // body xyz pos
+ } else if (std::fabs(body_state1.xyz_pos.x -
+ body_state2.xyz_pos.x) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.xyz_pos.y -
+ body_state2.xyz_pos.y) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.xyz_pos.z -
+ body_state2.xyz_pos.z) > tol) {
+ ret_val = false;
+
+ // body angles
+ } else if (std::fabs(body_state1.euler_angs.phi -
+ body_state2.euler_angs.phi) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.euler_angs.theta -
+ body_state2.euler_angs.theta) > tol) {
+ ret_val = false;
+ } else if (std::fabs(body_state1.euler_angs.psi -
+ body_state2.euler_angs.psi) > tol) {
+ ret_val = false;
+ }
+
+ return ret_val;
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8ad0759f9837557c33d2fa51d03cb14e9843adb
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h
@@ -0,0 +1,88 @@
+#pragma once
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+
+#include "rate_limited_first_order_filter.h"
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+#include "command.h"
+
+// Forward declaration of classes and structs. Can't include
+// spot_micro_motion_cmd.h here otherwise get circular dependence compile errors
+class SpotMicroMotionCmd;
+struct SpotMicroNodeConfig;
+
+
+// Struct holding a filter for three things relative to axes
+struct XyzFilters {
+ RateLmtdFirstOrderFilter x;
+ RateLmtdFirstOrderFilter y;
+ RateLmtdFirstOrderFilter z;
+};
+
+// Convenience structure for hold filters for all possible transitory states
+struct BodyStateFilters {
+ XyzFilters leg_right_back;
+ XyzFilters leg_right_front;
+ XyzFilters leg_left_front;
+ XyzFilters leg_left_back;
+ XyzFilters body_pos;
+ XyzFilters body_angs;
+};
+
+class SpotMicroState {
+ public:
+
+ // Constructor
+ SpotMicroState();
+
+ // Destructor
+ virtual ~SpotMicroState();
+
+ // virtual method to handle input commands, may change SpotMicroMotionCmd object
+ // passed by reference.
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd) {}
+
+ virtual void init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc) {}
+
+ virtual std::string getCurrentStateName() {
+ return "None";
+ }
+
+ protected:
+
+ // Calls SpotMicroMotionCmd's method to change the currently active state
+ void changeState(SpotMicroMotionCmd* smmc, std::unique_ptr<SpotMicroState> sms);
+
+ // Initializes set of filters controlling body state values to values
+ // contained in body_state
+ virtual void initBodyStateFilters(float dt, float tau, float rl, float rl_ang,
+ const smk::BodyState& body_state,
+ BodyStateFilters* body_state_filters);
+
+ // Sets body state filter commands to the values contained in body_state
+ virtual void setBodyStateFilterCommands(const smk::BodyState& body_state,
+ BodyStateFilters* body_state_filters);
+
+ // Calls run timestep method for all body state filters
+ virtual void runFilters(BodyStateFilters* body_state_filters);
+
+ // Assigns current filter value to body state
+ virtual void assignFilterValuesToBodyState(
+ const BodyStateFilters& body_state_filters,
+ smk::BodyState* body_state);
+
+ // Checks equality of body state structs to an absolute tolerance. Returns
+ // true if all absolute value differences of body state values are within
+ // tolernace
+ virtual bool checkBodyStateEquality(const smk::BodyState& body_state1,
+ const smk::BodyState& body_state2,
+ float tol);
+
+};
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2c3f2d92e6ae1ad41894aa2d4acf73fa6e26173
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp
@@ -0,0 +1,83 @@
+#include "spot_micro_transition_idle.h"
+
+#include "spot_micro_idle.h"
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_state.h"
+
+SpotMicroTransitionIdleState::SpotMicroTransitionIdleState() {
+ // Construcotr, doesn't need to do anything, for now...
+ //std::cout << "SpotMicroTransitionIdleState Ctor" << std::endl;
+}
+
+SpotMicroTransitionIdleState::~SpotMicroTransitionIdleState() {
+ //std::cout << "SpotMicroTransitionIdleState Dtor" << std::endl;
+}
+
+
+void SpotMicroTransitionIdleState::init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc) {
+ // Set initial state and end state
+ // Get starting body state
+ start_body_state_ = body_state;
+
+ // Create end state
+ // Create end state feet positions, a default foot stance
+ end_body_state_.leg_feet_pos = smmc->getLieDownStance();
+
+ // End body state position and angles
+ end_body_state_.euler_angs.phi = 0.0f;
+ end_body_state_.euler_angs.theta = 0.0f;
+ end_body_state_.euler_angs.psi = 0.0f;
+
+ end_body_state_.xyz_pos.x = 0.0f;
+ end_body_state_.xyz_pos.y = smnc.lie_down_height;
+ end_body_state_.xyz_pos.z = 0.0f;
+
+ // Initialize filters
+ float dt = smnc.dt;
+ float tau = smnc.transit_tau;
+ float rl = smnc.transit_rl;
+ float rl_ang = smnc.transit_angle_rl;
+
+ initBodyStateFilters(dt, tau, rl, rl_ang,
+ body_state, &body_state_filters_);
+
+ // Set destination commands for all filters
+ setBodyStateFilterCommands(end_body_state_, &body_state_filters_);
+}
+
+
+void SpotMicroTransitionIdleState::handleInputCommands(
+ const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk:: BodyState* body_state_cmd) {
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Transition Idle State" << std::endl;
+ }
+
+ // Check if desired end state reached, if so, change to stand state
+ if (checkBodyStateEquality(body_state, end_body_state_, 0.001f)) {
+ changeState(smmc, std::make_unique<SpotMicroIdleState>());
+
+ } else {
+ // Otherwise, rise filters and assign output values to body state command
+ runFilters(&body_state_filters_);
+
+ // Assing filter values to cmd
+ assignFilterValuesToBodyState(body_state_filters_,
+ body_state_cmd);
+
+ // Send command
+ smmc->setServoCommandMessageData();
+ smmc->publishServoProportionalCommand();
+
+ }
+
+}
+
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h
new file mode 100644
index 0000000000000000000000000000000000000000..2fbf6047a1bfd64f3cbf4588f7cd19ef8f4fa54f
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include "spot_micro_state.h"
+#include "command.h"
+#include "rate_limited_first_order_filter.h"
+
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+
+class SpotMicroTransitionIdleState : public SpotMicroState {
+ public:
+ SpotMicroTransitionIdleState(); // Constructor
+ ~SpotMicroTransitionIdleState(); // Destructor
+
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual void init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc);
+
+ // Returns current state name as a string
+ virtual std::string getCurrentStateName() {
+ return "Transit Idle";
+ }
+ private:
+ smk::BodyState start_body_state_;
+ smk::BodyState end_body_state_;
+ RateLmtdFirstOrderFilter rlfof;
+ BodyStateFilters body_state_filters_;
+};
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8d6cb5c27cfbd0e4619f11eb22fbede172b5ed34
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp
@@ -0,0 +1,82 @@
+#include "spot_micro_transition_stand.h"
+
+#include "spot_micro_stand.h"
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_state.h"
+
+SpotMicroTransitionStandState::SpotMicroTransitionStandState() {
+ // Construcotr, doesn't need to do anything, for now...
+ //std::cout << "SpotMicroTransitionStandState Ctor" << std::endl;
+}
+
+SpotMicroTransitionStandState::~SpotMicroTransitionStandState() {
+ //std::cout << "SpotMicroTransitionStandState Dtor" << std::endl;
+}
+
+
+void SpotMicroTransitionStandState::init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc) {
+ // Set initial state and end state
+ // Get starting body state
+ start_body_state_ = body_state;
+
+ // Create end state
+ // Create end state feet positions, a default foot stance
+ end_body_state_.leg_feet_pos = smmc->getNeutralStance();
+
+ // End body state position and angles
+ end_body_state_.euler_angs.phi = 0.0f;
+ end_body_state_.euler_angs.theta = 0.0f;
+ end_body_state_.euler_angs.psi = 0.0f;
+
+ end_body_state_.xyz_pos.x = 0.0f;
+ end_body_state_.xyz_pos.y = smnc.default_stand_height;
+ end_body_state_.xyz_pos.z = 0.0f;
+
+ // Initialize filters
+ float dt = smnc.dt;
+ float tau = smnc.transit_tau;
+ float rl = smnc.transit_rl;
+ float rl_ang = smnc.transit_angle_rl;
+
+ initBodyStateFilters(dt, tau, rl, rl_ang,
+ body_state, &body_state_filters_);
+
+ // Set destination commands for all filters
+ setBodyStateFilterCommands(end_body_state_, &body_state_filters_);
+}
+
+
+void SpotMicroTransitionStandState::handleInputCommands(
+ const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk:: BodyState* body_state_cmd) {
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Transition Stand State" << std::endl;
+ }
+
+ // Check if desired end state reached, if so, change to stand state
+ if (checkBodyStateEquality(body_state, end_body_state_, 0.001f)) {
+ changeState(smmc, std::make_unique<SpotMicroStandState>());
+
+ } else {
+ // Otherwise, rise filters and assign output values to body state command
+ runFilters(&body_state_filters_);
+
+ // Assing filter values to cmd
+ assignFilterValuesToBodyState(body_state_filters_,
+ body_state_cmd);
+
+ // Send command
+ smmc->setServoCommandMessageData();
+ smmc->publishServoProportionalCommand();
+
+ }
+
+}
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h
new file mode 100644
index 0000000000000000000000000000000000000000..76b3fbacdc38125b3395024068ebd817402871d6
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <iostream>
+
+#include "spot_micro_state.h"
+#include "command.h"
+#include "rate_limited_first_order_filter.h"
+
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+
+class SpotMicroTransitionStandState : public SpotMicroState {
+ public:
+ SpotMicroTransitionStandState(); // Constructor
+ ~SpotMicroTransitionStandState(); // Destructor
+
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual void init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc);
+
+ // Returns current state name as a string
+ virtual std::string getCurrentStateName() {
+ return "Transit Stand";
+ }
+ private:
+ smk::BodyState start_body_state_;
+ smk::BodyState end_body_state_;
+ RateLmtdFirstOrderFilter rlfof;
+ BodyStateFilters body_state_filters_;
+};
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..060653394d900ff6a3a5426c8e2efd75d4245466
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp
@@ -0,0 +1,360 @@
+#include "spot_micro_walk.h"
+
+#include <eigen3/Eigen/Geometry>
+
+#include "spot_micro_transition_stand.h"
+#include "spot_micro_motion_cmd.h"
+#include "rate_limited_first_order_filter.h"
+
+SpotMicroWalkState::SpotMicroWalkState() {
+ contact_feet_states_.right_back_in_swing = false;
+ contact_feet_states_.right_front_in_swing = false;
+ contact_feet_states_.left_front_in_swing = false;
+ contact_feet_states_.left_back_in_swing = false;
+
+ ticks_ = 0;
+ phase_index_ = 0;
+ subphase_ticks_ = 0;
+}
+
+SpotMicroWalkState::~SpotMicroWalkState() {
+ //std::cout << "SpotMicroWalkState Dtor" << std::endl;
+}
+
+void SpotMicroWalkState::handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd) {
+ // Debug output
+ if (smnc.debug_mode) {
+ std::cout << "In Spot Micro Walk State" << std::endl;
+ }
+
+
+ // If stand command received, change to transition to stand state
+ if (cmd.getStandCmd() == true) {
+ // Call parent class's change state method
+ changeState(smmc, std::make_unique<SpotMicroTransitionStandState>());
+
+ } else {
+ // Update gate phasing data
+ updatePhaseData();
+
+ // Step the gait controller
+ body_state_cmd->leg_feet_pos = stepGait(body_state, cmd, smnc, smmc->getNeutralStance());
+
+ if (smnc_.num_phases == 8) {
+ // Step body shift controller, only for 8 phase gait
+ body_state_cmd->xyz_pos = stepBodyShift(body_state, cmd, smnc);
+ }
+
+ // Set servo data and publish command
+ smmc->setServoCommandMessageData();
+ smmc->publishServoProportionalCommand();
+
+ // Increment ticks
+ ticks_ += 1;
+ }
+}
+
+
+
+void SpotMicroWalkState::init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc) {
+
+ // Save off config struct for convenience
+ smnc_ = smnc;
+
+ // Set default stance
+ cmd_state_.leg_feet_pos = smmc->getNeutralStance();
+
+ // End body state position and angles
+ cmd_state_.euler_angs.phi = 0.0f;
+ cmd_state_.euler_angs.theta = 0.0f;
+ cmd_state_.euler_angs.psi = 0.0f;
+
+ cmd_state_.xyz_pos.x = 0.0f;
+ cmd_state_.xyz_pos.y = smnc.default_stand_height;
+ cmd_state_.xyz_pos.z = 0.0f;
+
+}
+
+
+void SpotMicroWalkState::updatePhaseData() {
+ int phase_time = ticks_ % smnc_.phase_length;
+ int phase_sum = 0;
+
+ // Update phase index and subphase ticks
+ for (int i = 0; i < smnc_.num_phases; i++) {
+ phase_sum += smnc_.phase_ticks[i];
+ if (phase_time < phase_sum) {
+ phase_index_ = i;
+ subphase_ticks_ = phase_time - phase_sum + smnc_.phase_ticks[i];
+ break;
+ }
+ }
+
+ // Update contact feet states
+ if (smnc_.rb_contact_phases[phase_index_] == 0) {
+ contact_feet_states_.right_back_in_swing = true;
+ } else {
+ contact_feet_states_.right_back_in_swing = false;
+ }
+
+ if (smnc_.rf_contact_phases[phase_index_] == 0) {
+ contact_feet_states_.right_front_in_swing = true;
+ } else {
+ contact_feet_states_.right_front_in_swing = false;
+ }
+
+ if (smnc_.lf_contact_phases[phase_index_] == 0) {
+ contact_feet_states_.left_front_in_swing = true;
+ } else {
+ contact_feet_states_.left_front_in_swing = false;
+ }
+
+ if (smnc_.lb_contact_phases[phase_index_] == 0) {
+ contact_feet_states_.left_back_in_swing = true;
+ } else {
+ contact_feet_states_.left_back_in_swing = false;
+ }
+}
+
+
+smk::LegsFootPos SpotMicroWalkState::stepGait(
+ const smk::BodyState& body_state,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc,
+ const smk::LegsFootPos& default_stance_feet_pos) {
+
+ bool contact_mode;
+ float swing_proportion;
+ smk::Point foot_pos;
+ smk::LegsFootPos new_feet_pos;
+ smk::Point default_stance_foot_pos;
+
+ for (int i = 0; i < 4; i++) {
+ if (i == 0) { // right back
+ contact_mode = contact_feet_states_.right_back_in_swing;
+ foot_pos = body_state.leg_feet_pos.right_back;
+ default_stance_foot_pos = default_stance_feet_pos.right_back;
+
+ } else if (i == 1) { // right front
+ contact_mode = contact_feet_states_.right_front_in_swing;
+ foot_pos = body_state.leg_feet_pos.right_front;
+ default_stance_foot_pos = default_stance_feet_pos.right_front;
+
+ } else if (i == 2) { // left front
+ contact_mode = contact_feet_states_.left_front_in_swing;
+ foot_pos = body_state.leg_feet_pos.left_front;
+ default_stance_foot_pos = default_stance_feet_pos.left_front;
+
+ } else { // left back
+ contact_mode = contact_feet_states_.left_back_in_swing;
+ foot_pos = body_state.leg_feet_pos.left_back;
+ default_stance_foot_pos = default_stance_feet_pos.left_back;
+ }
+
+ if (contact_mode == false) { // Stance controller
+ foot_pos = stanceController(foot_pos, cmd, smnc);
+
+ } else { // swing leg controller
+ swing_proportion = (float)subphase_ticks_ / (float)smnc.swing_ticks;
+ foot_pos = swingLegController(foot_pos, cmd, smnc, swing_proportion, default_stance_foot_pos);
+ }
+
+ if (i == 0) { // right back
+ new_feet_pos.right_back = foot_pos;
+
+ } else if (i == 1) { // right front
+ new_feet_pos.right_front = foot_pos;
+
+ } else if (i == 2) { // left front
+ new_feet_pos.left_front = foot_pos;
+
+ } else { // left back
+ new_feet_pos.left_back = foot_pos;
+ }
+ }
+
+ // Return new feet positions
+ return new_feet_pos;
+}
+
+
+smk::Point SpotMicroWalkState::stanceController(
+ const smk::Point& foot_pos,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc) {
+
+ using namespace Eigen;
+
+ // Declare return value
+ smk::Point new_foot_pos;
+
+ // Convenience variables
+ float dt = smnc.dt;
+ float h_tau = smnc.foot_height_time_constant;
+
+ // Calculate position deltas due to speed and rotation commands
+ // Create vector to hold current foot position
+ Vector3f foot_pos_vec(foot_pos.x, foot_pos.y, foot_pos.z);
+ Vector3f new_foot_pos_vec;
+
+ // Create delta position vector, which is the commanded velocity times the
+ // timestep. Note that y speed command is really sideways velocity command, which
+ // is in the z direction of robot coordinate system. Stance foot position hard
+ // coded to 0 height here in second equation
+ Vector3f delta_pos(-cmd.getXSpeedCmd() * dt,
+ (1.0f/h_tau)*(0.0f - foot_pos.y) * dt,
+ -cmd.getYSpeedCmd() * dt);
+
+ // Create rotation matrix for yaw rate
+ Matrix3f rot_delta;
+ rot_delta = AngleAxisf(cmd.getYawRateCmd() * dt, Vector3f::UnitY());
+
+ // Move foot by rotation and linear translation deltas
+ new_foot_pos_vec = (rot_delta * foot_pos_vec) + delta_pos;
+
+ // Assign values to return structure
+ new_foot_pos.x = new_foot_pos_vec[0];
+ new_foot_pos.y = new_foot_pos_vec[1];
+ new_foot_pos.z = new_foot_pos_vec[2];
+
+ // Return value
+ return new_foot_pos;
+}
+
+smk::Point SpotMicroWalkState::swingLegController(
+ const smk::Point& foot_pos,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc,
+ float swing_proportion,
+ const smk::Point& default_stance_foot_pos) {
+
+ using namespace Eigen;
+
+ // declare return value
+ smk::Point new_foot_pos;
+
+ // Convenience variables
+ float dt = smnc.dt;
+ //float h_tau = smnc.foot_height_time_constant;
+ float swing_height;
+ float alpha = smnc.alpha;
+ float beta = smnc.beta;
+ float stance_ticks = smnc.stance_ticks;
+ Vector3f default_stance_foot_pos_vec(default_stance_foot_pos.x,
+ default_stance_foot_pos.y,
+ default_stance_foot_pos.z);
+
+ // Calculate swing height based on triangular profile
+ if (swing_proportion < 0.5f) {
+ swing_height = (swing_proportion/ 0.5f) * smnc.z_clearance;
+ } else {
+ swing_height = smnc.z_clearance * (1.0f - (swing_proportion - 0.5f) / 0.5f);
+ }
+
+
+ // Calculate position deltas due to speed and rotation commands
+ // Create vector to hold current foot position
+ Vector3f foot_pos_vec(foot_pos.x, foot_pos.y, foot_pos.z);
+ Vector3f new_foot_pos_vec;
+
+ // Create delta position vector for touchdown location
+ Vector3f delta_pos(alpha * stance_ticks * dt * cmd.getXSpeedCmd(),
+ 0.0f,
+ alpha * stance_ticks * dt * cmd.getYSpeedCmd());
+
+ // Create rotation matrix for yaw rate
+ float theta = beta * stance_ticks * dt * -cmd.getYawRateCmd();
+ Matrix3f rot_delta;
+ rot_delta = AngleAxisf(theta, Vector3f::UnitY());
+
+ // Calculate touchdown location
+ Vector3f touchdown_location = (rot_delta * default_stance_foot_pos_vec) + delta_pos;
+
+ float time_left = dt * smnc.swing_ticks * (1.0f - swing_proportion);
+
+ Vector3f delta_pos2 = ((touchdown_location - foot_pos_vec) / time_left) * dt;
+
+ new_foot_pos_vec = foot_pos_vec + delta_pos2;
+ new_foot_pos_vec[1] = swing_height;
+
+ // Assign values to return structure
+ new_foot_pos.x = new_foot_pos_vec[0];
+ new_foot_pos.y = new_foot_pos_vec[1];
+ new_foot_pos.z = new_foot_pos_vec[2];
+
+ // Return value
+ return new_foot_pos;
+
+}
+
+smk::Point SpotMicroWalkState::stepBodyShift(
+ const smk::BodyState& body_state,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc) {
+
+ // Convenience variables
+ float dt = smnc.dt;
+
+ int shift_phase = smnc.body_shift_phases[phase_index_];
+ float shift_proportion = (float)subphase_ticks_ / (float)smnc.swing_ticks;
+ float time_left = dt * smnc.swing_ticks * (1.0f - shift_proportion);
+ float end_x_pos;
+ float end_z_pos;
+ smk::Point return_point;
+ return_point.y = smnc.default_stand_height;
+
+ if (shift_phase == 2) { // Hold front left shift pos
+ return_point.x = smnc.fwd_body_balance_shift;
+ return_point.z = -smnc.side_body_balance_shift;
+
+ } else if (shift_phase == 4) { // Hold back left shift pos
+ return_point.x = -smnc.back_body_balance_shift;
+ return_point.z = -smnc.side_body_balance_shift;
+
+ } else if (shift_phase == 6) { // Hold front right shift pos
+ return_point.x = smnc.fwd_body_balance_shift;
+ return_point.z = smnc.side_body_balance_shift;
+
+ } else if (shift_phase == 8) { // Hold back right shift pos
+ return_point.x = -smnc.back_body_balance_shift;
+ return_point.z = smnc.side_body_balance_shift;
+
+ } else {
+ // Shift body to front left
+ if (shift_phase == 1) {
+ end_x_pos = smnc.fwd_body_balance_shift;
+ end_z_pos = -smnc.side_body_balance_shift;
+
+ // Shift body to back left
+ } else if (shift_phase == 3) {
+ end_x_pos = -smnc.back_body_balance_shift;
+ end_z_pos = -smnc.side_body_balance_shift;
+
+ // Shift body to front right
+ } else if (shift_phase == 5) {
+ end_x_pos = smnc.fwd_body_balance_shift;
+ end_z_pos = smnc.side_body_balance_shift;
+
+ // Shift body to back right
+ } else {
+ end_x_pos = -smnc.back_body_balance_shift;
+ end_z_pos = smnc.side_body_balance_shift;
+ }
+
+ float delta_x = ((end_x_pos - body_state.xyz_pos.x) / time_left) * dt;
+ float delta_z = ((end_z_pos - body_state.xyz_pos.z) / time_left) * dt;
+
+ return_point.x = body_state.xyz_pos.x + delta_x;
+ return_point.z = body_state.xyz_pos.z + delta_z;
+ }
+
+ return return_point;
+
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h
new file mode 100644
index 0000000000000000000000000000000000000000..d656d75fba963d470876695d87e4094f8446e494
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h
@@ -0,0 +1,79 @@
+#pragma once
+
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_state.h"
+#include "command.h"
+
+struct ContactFeet {
+ bool right_back_in_swing;
+ bool right_front_in_swing;
+ bool left_front_in_swing;
+ bool left_back_in_swing;\
+};
+
+class SpotMicroWalkState : public SpotMicroState {
+ public:
+ SpotMicroWalkState(); // Constructor
+ ~SpotMicroWalkState(); // Destructor
+
+ virtual void handleInputCommands(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc,
+ smk::BodyState* body_state_cmd);
+
+ virtual void init(const smk::BodyState& body_state,
+ const SpotMicroNodeConfig& smnc,
+ const Command& cmd,
+ SpotMicroMotionCmd* smmc);
+
+ // Returns current state name as a string
+ virtual std::string getCurrentStateName() {
+ return "Walk";
+ }
+
+ private:
+
+ SpotMicroNodeConfig smnc_;
+ smk::BodyState cmd_state_;
+
+ int ticks_;
+ int phase_index_;
+ int subphase_ticks_;
+ ContactFeet contact_feet_states_;
+
+ // Updates the integer phase index cooresponding to the current phase the
+ // robot gait should be in. Updates the subphase ticks, the ticks since the
+ // start of the current phase. And updates the contact feet states
+ // representing which feet are in swing and stance phases
+ void updatePhaseData();
+
+
+ // Steps the gait controller one timestep, sets the feet command state, and
+ // possibly other command states (such as xyz position, euler angles) if
+ // necessary
+ smk::LegsFootPos stepGait(const smk::BodyState& body_state,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc,
+ const smk::LegsFootPos& default_stance_feet_pos);
+
+ // Returns new foot position incremented by stance controller
+ smk::Point stanceController(const smk::Point& foot_pos,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc);
+
+ // Returns new foot position incremented by swing leg controller
+ smk::Point swingLegController(const smk::Point& foot_pos,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc,
+ float swing_proportion,
+ const smk::Point& default_stance_foot_pos);
+
+ // Steps the body shift controller that shifts the body xyz position
+ // to maintain balance during the gait cycle
+ smk::Point stepBodyShift(const smk::BodyState& body_state,
+ const Command& cmd,
+ const SpotMicroNodeConfig& smnc);
+};
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8985202a59d91ad88a5b325018f91dbc8ed47a61
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp
@@ -0,0 +1,752 @@
+#include "spot_micro_motion_cmd.h"
+
+#include <eigen3/Eigen/Geometry>
+#include "std_msgs/Float32.h"
+#include "std_msgs/Bool.h"
+#include "std_msgs/String.h"
+#include "std_msgs/Float32MultiArray.h"
+#include "geometry_msgs/Vector3.h"
+#include "geometry_msgs/Twist.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+#include "spot_micro_motion_cmd.h"
+#include "spot_micro_kinematics/spot_micro_kinematics.h"
+#include "i2cpwm_board/Servo.h"
+#include "i2cpwm_board/ServoArray.h"
+#include "i2cpwm_board/ServoConfig.h"
+#include "i2cpwm_board/ServosConfig.h"
+#include "spot_micro_idle.h"
+#include "utils.h"
+
+
+using namespace smk;
+using namespace Eigen;
+using namespace geometry_msgs;
+typedef std::vector<std::pair<std::string,std::string>> VectorStringPairs;
+
+// Constructor
+SpotMicroMotionCmd::SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh) {
+
+ nh_ = nh;
+ pnh_ = pnh;
+ if (smnc_.debug_mode) {
+ std::cout<<"from Constructor \n";
+ }
+
+ // Initialize Command
+ cmd_ = Command();
+
+ // Initialize state to Idle state
+ state_ = std::make_unique<SpotMicroIdleState>();
+
+ // Read in config parameters into smnc_
+ readInConfigParameters();
+
+ // Initialize spot micro kinematics object of this class
+ sm_ = smk::SpotMicroKinematics(0.0f, 0.0f, 0.0f, smnc_.smc);
+
+ // Set an initial body height and stance cmd for idle mode
+ body_state_cmd_.euler_angs = {.phi = 0.0f, .theta = 0.0f, .psi = 0.0f};
+ body_state_cmd_.xyz_pos = {.x = 0.0f, .y = smnc_.lie_down_height, .z = 0.0f};
+ body_state_cmd_.leg_feet_pos = getLieDownStance();
+
+ // Set the spot micro kinematics object to this initial command
+ sm_.setBodyState(body_state_cmd_);
+
+ // Set initial odometry state to zero
+ robot_odometry_.euler_angs = {.phi = 0.0f, .theta = 0.0f, .psi = 0.0f};
+ robot_odometry_.xyz_pos = {.x = 0.0f, .y = 0.0f, .z = 0.0f};
+
+ // Initialize servo array message with 12 servo objects
+ for (int i = 1; i <= smnc_.num_servos; i++) {
+ i2cpwm_board::Servo temp_servo;
+ temp_servo.servo = i;
+ temp_servo.value = 0;
+ servo_array_.servos.push_back(temp_servo);
+ }
+
+ // Initialize servo array absolute message with 12 servo object with a value of
+ // zero, just copy servo_array_msg since it's already correct
+ servo_array_absolute_.servos = servo_array_.servos;
+
+ // Initialize publishers and subscribers
+ // object detected state subscriber
+ obj_det_sub_ = nh.subscribe("/obj_det", 1, &SpotMicroMotionCmd::standCommandCallback, this);
+
+ // stand cmd event subscriber
+ stand_sub_ = nh.subscribe("/stand_cmd", 1, &SpotMicroMotionCmd::standCommandCallback, this);
+
+ // idle cmd event subscriber
+ idle_sub_ = nh.subscribe("/idle_cmd", 1, &SpotMicroMotionCmd::idleCommandCallback, this);
+
+ // walk cmd event subscriber
+ walk_sub_ = nh.subscribe("/walk_cmd", 1, &SpotMicroMotionCmd::walkCommandCallback, this);
+
+ // body angle command subscriber
+ body_angle_cmd_sub_ = nh.subscribe("/angle_cmd", 1, &SpotMicroMotionCmd::angleCommandCallback, this);
+
+ // velocity command subscriber
+ vel_cmd_sub_ = nh.subscribe("/cmd_vel", 1, &SpotMicroMotionCmd::velCommandCallback, this);
+
+ // servos_absolute publisher
+ servos_absolute_pub_ = nh.advertise<i2cpwm_board::ServoArray>("servos_absolute", 1);
+
+ // Servos proportional publisher
+ servos_proportional_pub_ = nh.advertise<i2cpwm_board::ServoArray>("servos_proportional",1);
+
+ // Servos configuration publisher
+ servos_config_client_ = nh.serviceClient<i2cpwm_board::ServosConfig>("config_servos");
+
+ // Body state publisher for plotting
+ body_state_pub_ = nh.advertise<std_msgs::Float32MultiArray>("body_state",1);
+
+ // State string publisher for lcd monitor
+ lcd_state_pub_ = nh.advertise<std_msgs::String>("lcd_state",1);
+
+ // Velocity command state publisher for lcd monitor
+ lcd_vel_cmd_pub_ = nh.advertise<geometry_msgs::Twist>("lcd_vel_cmd",1);
+
+ // Angle command state publisher for lcd monitor
+ lcd_angle_cmd_pub_ = nh.advertise<geometry_msgs::Vector3>("lcd_angle_cmd",1);
+
+
+
+ // Initialize lcd monitor messages
+ lcd_state_string_msg_.data = "Idle";
+
+ lcd_vel_cmd_msg_.linear.x = 0.0f;
+ lcd_vel_cmd_msg_.linear.y = 0.0f;
+ lcd_vel_cmd_msg_.linear.z = 0.0f;
+ lcd_vel_cmd_msg_.angular.x = 0.0f;
+ lcd_vel_cmd_msg_.angular.y = 0.0f;
+ lcd_vel_cmd_msg_.angular.z = 0.0f;
+
+ lcd_angle_cmd_msg_.x = 0.0f;
+ lcd_angle_cmd_msg_.y = 0.0f;
+ lcd_angle_cmd_msg_.z = 0.0f;
+
+
+ // Only do if plot mode
+ // Initialize body state message for plot debug only
+ // Initialize 18 values to hold xyz positions of the four legs (12) +
+ // the body x,y,z positions (3), and the body angles (3) for a total of 18
+ if (smnc_.plot_mode) {
+ for (int i = 0; i < 18; i++) {
+ body_state_msg_.data.push_back(0.0f);
+ }
+ }
+
+ // Publish static transforms
+ publishStaticTransforms();
+}
+
+
+// Destructor method
+SpotMicroMotionCmd::~SpotMicroMotionCmd() {
+
+ if (smnc_.debug_mode) {
+ std::cout<<"from Destructor \n";
+ }
+ // Free up the memory assigned from heap
+}
+
+
+void SpotMicroMotionCmd::runOnce() {
+ if (smnc_.debug_mode) {
+ std::cout<<"from Runonce \n";
+ }
+
+ // Call method to handle input commands
+ handleInputCommands();
+
+ // Consume all event commands.
+ // This resets all event commands if they were true. Doing this enforces a rising edge detection
+ resetEventCommands();
+
+ // Only publish body state message in debug mode
+ if (smnc_.plot_mode) {
+ publishBodyState();
+ }
+
+ // Publish lcd monitor data
+ publishLcdMonitorData();
+
+ // Broadcast dynamic transforms
+ publishDynamicTransforms();
+
+ if (smnc_.publish_odom) {
+ // Integrate robot odometry
+ integrateOdometry();
+ }
+}
+
+
+bool SpotMicroMotionCmd::publishServoConfiguration() {
+ // Create a temporary servo config
+ i2cpwm_board::ServoConfig temp_servo_config;
+ i2cpwm_board::ServosConfig temp_servo_config_array;
+
+ // Loop through servo configuration dictionary in smnc_, append servo to
+ for (std::map<std::string, std::map<std::string, float>>::iterator
+ iter = smnc_.servo_config.begin();
+ iter != smnc_.servo_config.end();
+ ++iter) {
+
+ std::map<std::string, float> servo_config_params = iter->second;
+ temp_servo_config.center = servo_config_params["center"];
+ temp_servo_config.range = servo_config_params["range"];
+ temp_servo_config.servo = servo_config_params["num"];
+ temp_servo_config.direction = servo_config_params["direction"];
+
+ // Append to temp_servo_config_array
+ temp_servo_config_array.request.servos.push_back(temp_servo_config);
+ }
+
+ // call the client service, return true if succesfull, false if not
+ if (!servos_config_client_.call(temp_servo_config_array)) {
+ if (!smnc_.debug_mode && !smnc_.run_standalone) {
+ // Only error out if not in debug mode or standalone mode
+ ROS_ERROR("Failed to call service servo_config");
+ return false;
+ }
+ }
+
+ return true;
+}
+
+
+void SpotMicroMotionCmd::setServoCommandMessageData() {
+ // Set the state of the spot micro kinematics object by setting the foot
+ // positions, body position, and body orientation. Retrieve joint angles and
+ // set the servo cmd message data
+ sm_.setBodyState(body_state_cmd_);
+ LegsJointAngles joint_angs = sm_.getLegsJointAngles();
+
+ // Set the angles for the servo command message
+ servo_cmds_rad_["RF_1"] = joint_angs.right_front.ang1;
+ servo_cmds_rad_["RF_2"] = joint_angs.right_front.ang2;
+ servo_cmds_rad_["RF_3"] = joint_angs.right_front.ang3;
+
+ servo_cmds_rad_["RB_1"] = joint_angs.right_back.ang1;
+ servo_cmds_rad_["RB_2"] = joint_angs.right_back.ang2;
+ servo_cmds_rad_["RB_3"] = joint_angs.right_back.ang3;
+
+ servo_cmds_rad_["LF_1"] = joint_angs.left_front.ang1;
+ servo_cmds_rad_["LF_2"] = joint_angs.left_front.ang2;
+ servo_cmds_rad_["LF_3"] = joint_angs.left_front.ang3;
+
+ servo_cmds_rad_["LB_1"] = joint_angs.left_back.ang1;
+ servo_cmds_rad_["LB_2"] = joint_angs.left_back.ang2;
+ servo_cmds_rad_["LB_3"] = joint_angs.left_back.ang3;
+}
+
+
+void SpotMicroMotionCmd::publishServoProportionalCommand() {
+ for (std::map<std::string, std::map<std::string, float>>::iterator
+ iter = smnc_.servo_config.begin();
+ iter != smnc_.servo_config.end();
+ ++iter) {
+
+ std::string servo_name = iter->first;
+ std::map<std::string, float> servo_config_params = iter->second;
+
+ int servo_num = servo_config_params["num"];
+ float cmd_ang_rad = servo_cmds_rad_[servo_name];
+ float center_ang_rad = servo_config_params["center_angle_deg"]*M_PI/180.0f;
+ float servo_proportional_cmd = (cmd_ang_rad - center_ang_rad) /
+ (smnc_.servo_max_angle_deg*M_PI/180.0f);
+
+ if (servo_proportional_cmd > 1.0f) {
+ servo_proportional_cmd = 1.0f;
+ ROS_WARN("Proportional Command above +1.0 was computed, clipped to 1.0");
+ ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI);
+
+ } else if (servo_proportional_cmd < -1.0f) {
+ servo_proportional_cmd = -1.0f;
+ ROS_WARN("Proportional Command below -1.0 was computed, clipped to -1.0");
+ ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI);
+ }
+
+ servo_array_.servos[servo_num-1].servo = servo_num;
+ servo_array_.servos[servo_num-1].value = servo_proportional_cmd;
+ }
+
+ // Publish message
+ servos_proportional_pub_.publish(servo_array_);
+}
+
+
+void SpotMicroMotionCmd::publishZeroServoAbsoluteCommand() {
+ // Publish the servo absolute message
+ servos_absolute_pub_.publish(servo_array_absolute_);
+}
+
+
+SpotMicroNodeConfig SpotMicroMotionCmd::getNodeConfig() {
+ return smnc_;
+}
+
+
+LegsFootPos SpotMicroMotionCmd::getNeutralStance() {
+ float len = smnc_.smc.body_length; // body length
+ float width = smnc_.smc.body_width; // body width
+ float l1 = smnc_.smc.hip_link_length; // liength of the hip link
+ float f_offset = smnc_.stand_front_x_offset; // x offset for front feet in neutral stance
+ float b_offset = smnc_.stand_back_x_offset; // y offset for back feet in neutral stance
+
+ LegsFootPos neutral_stance;
+ neutral_stance.right_back = {.x = -len/2 + b_offset, .y = 0.0f, .z = width/2 + l1};
+ neutral_stance.right_front = {.x = len/2 + f_offset, .y = 0.0f, .z = width/2 + l1};
+ neutral_stance.left_front = {.x = len/2 + f_offset, .y = 0.0f, .z = -width/2 - l1};
+ neutral_stance.left_back = {.x = -len/2 + b_offset, .y = 0.0f, .z = -width/2 - l1};
+
+ return neutral_stance;
+}
+
+
+LegsFootPos SpotMicroMotionCmd::getLieDownStance() {
+ float len = smnc_.smc.body_length; // body length
+ float width = smnc_.smc.body_width; // body width
+ float l1 = smnc_.smc.hip_link_length; // length of the hip link
+ float x_off = smnc_.lie_down_feet_x_offset;
+
+ LegsFootPos lie_down_stance;
+ lie_down_stance.right_back = {.x = -len/2 + x_off, .y = 0.0f, .z = width/2 + l1};
+ lie_down_stance.right_front = {.x = len/2 + x_off, .y = 0.0f, .z = width/2 + l1};
+ lie_down_stance.left_front = {.x = len/2 + x_off, .y = 0.0f, .z = -width/2 - l1};
+ lie_down_stance.left_back = {.x = -len/2 + x_off, .y = 0.0f, .z = -width/2 - l1};
+
+ return lie_down_stance;
+}
+
+
+void SpotMicroMotionCmd::commandIdle() {
+ cmd_.idle_cmd_ = true;
+}
+
+
+std::string SpotMicroMotionCmd::getCurrentStateName() {
+ return state_->getCurrentStateName();
+}
+
+
+void SpotMicroMotionCmd::readInConfigParameters() {
+ // Read in and save parameters
+ // Use private node handle for getting params so just the relative
+ // parameter name can be used (as opposed to the global name, e.g.:
+ // /spot_micro_motion_cmd/param1
+ pnh_.getParam("hip_link_length", smnc_.smc.hip_link_length);
+ pnh_.getParam("upper_leg_link_length", smnc_.smc.upper_leg_link_length);
+ pnh_.getParam("lower_leg_link_length", smnc_.smc.lower_leg_link_length);
+ pnh_.getParam("body_width", smnc_.smc.body_width);
+ pnh_.getParam("body_length", smnc_.smc.body_length);
+ pnh_.getParam("default_stand_height", smnc_.default_stand_height);
+ pnh_.getParam("stand_front_x_offset", smnc_.stand_front_x_offset);
+ pnh_.getParam("stand_back_x_offset", smnc_.stand_back_x_offset);
+ pnh_.getParam("lie_down_height", smnc_.lie_down_height);
+ pnh_.getParam("lie_down_foot_x_offset", smnc_.lie_down_feet_x_offset);
+ pnh_.getParam("num_servos", smnc_.num_servos);
+ pnh_.getParam("servo_max_angle_deg", smnc_.servo_max_angle_deg);
+ pnh_.getParam("transit_tau", smnc_.transit_tau);
+ pnh_.getParam("transit_rl", smnc_.transit_rl);
+ pnh_.getParam("transit_angle_rl", smnc_.transit_angle_rl);
+ pnh_.getParam("dt", smnc_.dt);
+ pnh_.getParam("debug_mode", smnc_.debug_mode);
+ pnh_.getParam("run_standalone", smnc_.run_standalone);
+ pnh_.getParam("plot_mode", smnc_.plot_mode);
+ pnh_.getParam("max_fwd_velocity", smnc_.max_fwd_velocity);
+ pnh_.getParam("max_side_velocity", smnc_.max_side_velocity);
+ pnh_.getParam("max_yaw_rate", smnc_.max_yaw_rate);
+ pnh_.getParam("z_clearance", smnc_.z_clearance);
+ pnh_.getParam("alpha", smnc_.alpha);
+ pnh_.getParam("beta", smnc_.beta);
+ pnh_.getParam("num_phases", smnc_.num_phases);
+ pnh_.getParam("rb_contact_phases", smnc_.rb_contact_phases);
+ pnh_.getParam("rf_contact_phases", smnc_.rf_contact_phases);
+ pnh_.getParam("lf_contact_phases", smnc_.lf_contact_phases);
+ pnh_.getParam("lb_contact_phases", smnc_.lb_contact_phases);
+ pnh_.getParam("overlap_time", smnc_.overlap_time);
+ pnh_.getParam("swing_time", smnc_.swing_time);
+ pnh_.getParam("foot_height_time_constant", smnc_.foot_height_time_constant);
+ pnh_.getParam("body_shift_phases", smnc_.body_shift_phases);
+ pnh_.getParam("fwd_body_balance_shift", smnc_.fwd_body_balance_shift);
+ pnh_.getParam("back_body_balance_shift", smnc_.back_body_balance_shift);
+ pnh_.getParam("side_body_balance_shift", smnc_.side_body_balance_shift);
+ pnh_.getParam("publish_odom", smnc_.publish_odom);
+ pnh_.getParam("lidar_x_pos", smnc_.lidar_x_pos);
+ pnh_.getParam("lidar_y_pos", smnc_.lidar_y_pos);
+ pnh_.getParam("lidar_z_pos", smnc_.lidar_z_pos);
+ pnh_.getParam("lidar_yaw_angle", smnc_.lidar_yaw_angle);
+
+ // Derived parameters, round result of division of floats
+ smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt);
+ smnc_.swing_ticks = round(smnc_.swing_time / smnc_.dt);
+
+ // 8 Phase gait specific
+ if (smnc_.num_phases == 8) {
+ smnc_.stance_ticks = 7 * smnc_.swing_ticks;
+ smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt);
+ smnc_.phase_ticks = std::vector<int>
+ {smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks,
+ smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks};
+ smnc_.phase_length = smnc_.num_phases * smnc_.swing_ticks;
+
+ } else {
+ // 4 phase gait specific
+ smnc_.stance_ticks = 2 * smnc_.overlap_ticks + smnc_.swing_ticks;
+ smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt);
+ smnc_.phase_ticks = std::vector<int>
+ {smnc_.overlap_ticks, smnc_.swing_ticks, smnc_.overlap_ticks, smnc_.swing_ticks};
+ smnc_.phase_length = 2 * smnc_.swing_ticks + 2 * smnc_.overlap_ticks;
+ }
+
+ // Temporary map for populating map in smnc_
+ std::map<std::string, float> temp_map;
+
+ // Iterate over servo names, as defined in the map servo_cmds_rad, to populate
+ // the servo config map in smnc_
+ for(std::map<std::string, float>::iterator
+ iter = servo_cmds_rad_.begin();
+ iter != servo_cmds_rad_.end();
+ ++iter) {
+
+ std::string servo_name = iter->first; // Get key, string of the servo name
+
+ pnh_.getParam(servo_name, temp_map); // Read the parameter. Parameter name must match servo name
+ smnc_.servo_config[servo_name] = temp_map; // Assing in servo config to map in the struct
+ }
+
+}
+
+/*void SpotMicroMotionCmd::objectCommandcallback(
+ const std_msgs::Bool::ConstPtr& msg) {
+ if (msg->data == true) {cmd_.obj_det = true;}
+)*/
+
+void SpotMicroMotionCmd::standCommandCallback(
+ const std_msgs::Bool::ConstPtr& msg) {
+ if (msg->data == true) {cmd_.stand_cmd_ = true;}
+}
+
+
+void SpotMicroMotionCmd::idleCommandCallback(
+ const std_msgs::Bool::ConstPtr& msg) {
+ if (msg->data == true) {cmd_.idle_cmd_ = true;}
+}
+
+
+void SpotMicroMotionCmd::walkCommandCallback(
+ const std_msgs::Bool::ConstPtr& msg) {
+ if (msg->data == true) {cmd_.walk_cmd_ = true;}
+}
+
+
+void SpotMicroMotionCmd::angleCommandCallback(
+ const geometry_msgs::Vector3ConstPtr& msg) {
+ cmd_.phi_cmd_ = msg->x;
+ cmd_.theta_cmd_ = msg->y;
+ cmd_.psi_cmd_ = msg->z;
+}
+
+
+void SpotMicroMotionCmd::velCommandCallback(
+ const geometry_msgs::TwistConstPtr& msg) {
+ cmd_.x_vel_cmd_mps_ = msg->linear.x;
+ cmd_.y_vel_cmd_mps_ = msg->linear.y;
+ cmd_.yaw_rate_cmd_rps_ = msg->angular.z;
+}
+
+
+void SpotMicroMotionCmd::resetEventCommands() {
+ // Reset all event commands, setting all command states false if they were true
+ cmd_.resetEventCmds();
+}
+
+
+void SpotMicroMotionCmd::handleInputCommands() {
+ // Delegate input handling to state
+ state_->handleInputCommands(sm_.getBodyState(), smnc_, cmd_, this, &body_state_cmd_);
+}
+
+
+void SpotMicroMotionCmd::changeState(std::unique_ptr<SpotMicroState> sms) {
+ // Change the active state
+ state_ = std::move(sms);
+
+ // Call init method of new state
+ state_->init(sm_.getBodyState(), smnc_, cmd_, this);
+
+ // Reset all command values
+ cmd_.resetAllCommands();
+}
+
+
+void SpotMicroMotionCmd::publishBodyState() {
+ // Order of the float array:
+ // 3 floats xyz for rightback leg foot pos
+ // 3 floats xyz for rightfront leg foot pos
+ // 3 floats xyz for leftfront leg foot pos
+ // 3 floats xyz for leftback leg foot pos
+ // 3 floats for xyz body position
+ // 3 floats for phi, theta, psi body angles
+
+ body_state_msg_.data[0] = body_state_cmd_.leg_feet_pos.right_back.x;
+ body_state_msg_.data[1] = body_state_cmd_.leg_feet_pos.right_back.y;
+ body_state_msg_.data[2] = body_state_cmd_.leg_feet_pos.right_back.z;
+
+ body_state_msg_.data[3] = body_state_cmd_.leg_feet_pos.right_front.x;
+ body_state_msg_.data[4] = body_state_cmd_.leg_feet_pos.right_front.y;
+ body_state_msg_.data[5] = body_state_cmd_.leg_feet_pos.right_front.z;
+
+ body_state_msg_.data[6] = body_state_cmd_.leg_feet_pos.left_front.x;
+ body_state_msg_.data[7] = body_state_cmd_.leg_feet_pos.left_front.y;
+ body_state_msg_.data[8] = body_state_cmd_.leg_feet_pos.left_front.z;
+
+ body_state_msg_.data[9] = body_state_cmd_.leg_feet_pos.left_back.x;
+ body_state_msg_.data[10] = body_state_cmd_.leg_feet_pos.left_back.y;
+ body_state_msg_.data[11] = body_state_cmd_.leg_feet_pos.left_back.z;
+
+ body_state_msg_.data[12] = body_state_cmd_.xyz_pos.x;
+ body_state_msg_.data[13] = body_state_cmd_.xyz_pos.y;
+ body_state_msg_.data[14] = body_state_cmd_.xyz_pos.z;
+
+ body_state_msg_.data[15] = body_state_cmd_.euler_angs.phi;
+ body_state_msg_.data[16] = body_state_cmd_.euler_angs.theta;
+ body_state_msg_.data[17] = body_state_cmd_.euler_angs.psi;
+
+ body_state_pub_.publish(body_state_msg_);
+}
+
+
+void SpotMicroMotionCmd::publishLcdMonitorData() {
+ lcd_state_string_msg_.data = getCurrentStateName();
+
+ lcd_vel_cmd_msg_.linear.x = cmd_.getXSpeedCmd();
+ lcd_vel_cmd_msg_.linear.y = cmd_.getYSpeedCmd();
+ lcd_vel_cmd_msg_.angular.z = cmd_.getYawRateCmd();
+
+ lcd_angle_cmd_msg_.x = cmd_.getPhiCmd();
+ lcd_angle_cmd_msg_.y = cmd_.getThetaCmd();
+ lcd_angle_cmd_msg_.z = cmd_.getPsiCmd();
+
+ lcd_state_pub_.publish(lcd_state_string_msg_);
+ lcd_vel_cmd_pub_.publish(lcd_vel_cmd_msg_);
+ lcd_angle_cmd_pub_.publish(lcd_angle_cmd_msg_);
+}
+
+
+void SpotMicroMotionCmd::publishStaticTransforms() {
+
+ TransformStamped tr_stamped;
+
+ // base_link to front_link transform
+ tr_stamped = createTransform("base_link", "front_link",
+ 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0);
+ static_transform_br_.sendTransform(tr_stamped);
+
+ // base_link to rear_link transform
+ tr_stamped = createTransform("base_link", "rear_link",
+ 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0);
+ static_transform_br_.sendTransform(tr_stamped);
+
+ // base_link to lidar_link transform
+ float x_offset = smnc_.lidar_x_pos;
+ float y_offset = smnc_.lidar_y_pos;
+ float z_offset = smnc_.lidar_z_pos;
+ float yaw_angle = smnc_.lidar_yaw_angle*M_PI/180.0; // Converted to radians
+ tr_stamped = createTransform("base_link", "lidar_link",
+ x_offset, y_offset, z_offset,
+ 0.0, 0.0, yaw_angle);
+ static_transform_br_.sendTransform(tr_stamped);
+
+ // legs to leg cover transforms
+ const VectorStringPairs leg_cover_pairs {
+ { "front_left_leg_link", "front_left_leg_link_cover" },
+ { "front_right_leg_link", "front_right_leg_link_cover"},
+ { "rear_right_leg_link", "rear_right_leg_link_cover" },
+ { "rear_left_leg_link", "rear_left_leg_link_cover" }};
+
+ // Loop over all leg to leg cover name pairs, publish a 0 dist/rot transform
+ for (auto it = leg_cover_pairs.begin(); it != leg_cover_pairs.end(); it++) {
+ tr_stamped = createTransform(it->first, it->second,
+ 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0);
+ static_transform_br_.sendTransform(tr_stamped);
+ }
+
+ // foot to toe link transforms
+ const VectorStringPairs foot_toe_pairs {
+ { "front_left_foot_link", "front_left_toe_link" },
+ { "front_right_foot_link", "front_right_toe_link"},
+ { "rear_right_foot_link", "rear_right_toe_link" },
+ { "rear_left_foot_link", "rear_left_toe_link" }};
+
+ // Loop over all name pairs, publish the same transform
+ for (auto it = foot_toe_pairs.begin(); it != foot_toe_pairs.end(); it++) {
+ tr_stamped = createTransform(it->first, it->second,
+ 0.0, 0.0, -0.13, // TODO: Change to a parameter
+ 0.0, 0.0, 0.0);
+ static_transform_br_.sendTransform(tr_stamped);
+ }
+}
+
+
+void SpotMicroMotionCmd::publishDynamicTransforms() {
+
+ // Get joint angles
+ LegsJointAngles joint_angs = sm_.getLegsJointAngles();
+
+ // Declare utility variables
+ TransformStamped transform_stamped;
+ Affine3d temp_trans;
+
+ /////////////////
+ // ODOMETRY /////
+ /////////////////
+ if (smnc_.publish_odom) {
+ transform_stamped = eigAndFramesToTrans(getOdometryTransform(), "odom", "base_footprint");
+ transform_br_.sendTransform(transform_stamped);
+ }
+
+ /////////////////
+ // BODY CENTER //
+ /////////////////
+
+ temp_trans = matrix4fToAffine3d(sm_.getBodyHt());
+
+ // Rotate body center transform to desired coordinate system
+ // Original, kinematics, coordinate frame: x forward, y up, z right
+ // Desired orientation: x forward, y left, z up
+ // Rotate the robot frame +90 deg about the global +X axis (pre-multiply),
+ // then rotate the local coordinate system by -90 (post multiply)
+ temp_trans = AngleAxisd(M_PI/2.0, Vector3d::UnitX()) *
+ temp_trans *
+ AngleAxisd(-M_PI/2.0, Vector3d::UnitX());
+
+ // Create and broadcast the transform
+ transform_stamped = eigAndFramesToTrans(temp_trans, "base_footprint", "base_link");
+ transform_br_.sendTransform(transform_stamped);
+
+
+ /////////////////////
+ // FRONT RIGHT LEG //
+ /////////////////////
+ // Shoulder
+ transform_stamped = createTransform("base_link", "front_right_shoulder_link",
+ smnc_.smc.body_length/2.0, -smnc_.smc.body_width/2.0, 0.0,
+ joint_angs.right_front.ang1, 0.0, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // leg
+ transform_stamped = createTransform("front_right_shoulder_link","front_right_leg_link",
+ 0.0, -smnc_.smc.hip_link_length, 0.0,
+ 0.0, -joint_angs.right_front.ang2, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // foot
+ transform_stamped = createTransform("front_right_leg_link","front_right_foot_link",
+ 0.0, 0.0, -smnc_.smc.upper_leg_link_length,
+ 0.0, -joint_angs.right_front.ang3, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+
+ ////////////////////
+ // REAR RIGHT LEG //
+ ////////////////////
+ // shoulder
+ transform_stamped = createTransform("base_link", "rear_right_shoulder_link",
+ -smnc_.smc.body_length/2.0, -smnc_.smc.body_width/2.0, 0.0,
+ joint_angs.right_back.ang1, 0.0, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // leg
+ transform_stamped = createTransform("rear_right_shoulder_link","rear_right_leg_link",
+ 0.0, -smnc_.smc.hip_link_length, 0.0,
+ 0.0, -joint_angs.right_back.ang2, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // foot
+ transform_stamped = createTransform("rear_right_leg_link","rear_right_foot_link",
+ 0.0, 0.0, -smnc_.smc.upper_leg_link_length,
+ 0.0, -joint_angs.right_back.ang3, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+
+ ////////////////////
+ // FRONT LEFT LEG //
+ ////////////////////
+ // Shoulder
+ transform_stamped = createTransform("base_link", "front_left_shoulder_link",
+ smnc_.smc.body_length/2.0, smnc_.smc.body_width/2.0, 0.0,
+ -joint_angs.left_front.ang1, 0.0, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // leg
+ transform_stamped = createTransform("front_left_shoulder_link","front_left_leg_link",
+ 0.0, smnc_.smc.hip_link_length, 0.0,
+ 0.0, joint_angs.left_front.ang2, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // foot
+ transform_stamped = createTransform("front_left_leg_link","front_left_foot_link",
+ 0.0, 0.0, -smnc_.smc.upper_leg_link_length,
+ 0.0, joint_angs.left_front.ang3, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+
+ ///////////////////
+ // REAR LEFT LEG //
+ ///////////////////
+ // shoulder
+ transform_stamped = createTransform("base_link", "rear_left_shoulder_link",
+ -smnc_.smc.body_length/2.0, smnc_.smc.body_width/2.0, 0.0,
+ -joint_angs.left_back.ang1, 0.0, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // leg
+ transform_stamped = createTransform("rear_left_shoulder_link","rear_left_leg_link",
+ 0.0, smnc_.smc.hip_link_length, 0.0,
+ 0.0, joint_angs.left_back.ang2, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+
+ // foot
+ transform_stamped = createTransform("rear_left_leg_link","rear_left_foot_link",
+ 0.0, 0.0, -smnc_.smc.upper_leg_link_length,
+ 0.0, joint_angs.left_back.ang3, 0.0);
+ transform_br_.sendTransform(transform_stamped);
+}
+
+
+void SpotMicroMotionCmd::integrateOdometry() {
+ // Get loop time, heading, and rate commands
+ float dt = smnc_.dt;
+ float psi = robot_odometry_.euler_angs.psi;
+ float x_spd = cmd_.getXSpeedCmd();
+ float y_spd = -cmd_.getYSpeedCmd();
+ float yaw_rate = -cmd_.getYawRateCmd();
+
+ // This is the odometry coordinate frame (not the robot kinematic frame)
+ float x_dot = x_spd*cos(psi) - y_spd*sin(psi);
+ float y_dot = x_spd*sin(psi) + y_spd*cos(psi);
+ float yaw_dot = yaw_rate;
+
+ // Integrate x and y position, and yaw angle, from commanded values
+ // y speed and yaw rate are reversed due to mismatch between command
+ // coordinate frame and world coordinate frame
+ robot_odometry_.xyz_pos.x += x_dot*dt;
+ robot_odometry_.xyz_pos.y += y_dot*dt;
+ robot_odometry_.euler_angs.psi += yaw_dot*dt;
+}
+
+
+Affine3d SpotMicroMotionCmd::getOdometryTransform() {
+ // Create odemtry translation and rotation, and combine together
+ Translation3d translation(robot_odometry_.xyz_pos.x, robot_odometry_.xyz_pos.y, 0.0);
+ AngleAxisd rotation(robot_odometry_.euler_angs.psi, Vector3d::UnitZ());
+
+ return (translation * rotation);
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c9602261cbe07f62bf1a0c312b12f7c760ac70d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp
@@ -0,0 +1,43 @@
+// Node file to create object and initialising the ROS node
+#include "spot_micro_motion_cmd.h"
+#include <iostream>
+
+
+int main(int argc, char** argv) {
+ /* initialising the ROS node creating node handle
+ for regestring it to the master and then private node handle to
+ handle the parameters */
+ ros::init(argc, argv, "spot_micro_motion_cmd");
+ ros::NodeHandle nh;
+ ros::NodeHandle pnh("~");
+
+ SpotMicroMotionCmd node(nh,pnh); // Creating the object
+
+ ros::Rate rate(1.0/node.getNodeConfig().dt); // Defing the looping rate
+
+
+ // Only proceed if servo configuration publishing succeeds
+ if (node.publishServoConfiguration()) {
+
+ bool debug_mode = node.getNodeConfig().debug_mode;
+ ros::Time begin;
+ /* Looking for any interupt else it will continue looping */
+ // Main loop runs indefinitely unless there is an interupt call
+ while (ros::ok())
+ {
+ if (debug_mode) {
+ begin = ros::Time::now();
+ }
+
+ node.runOnce();
+ ros::spinOnce();
+ rate.sleep();
+
+ if (debug_mode) {
+ std::cout << (ros::Time::now() - begin) << std::endl;
+ }
+ }
+
+ }
+ return 0;
+}
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/utils.cpp b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da129ff32aa31544052c72ba37e7da8d1ff31b70
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/utils.cpp
@@ -0,0 +1,64 @@
+#include "utils.h"
+
+#include <ros/ros.h>
+#include <eigen3/Eigen/Geometry>
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_eigen/tf2_eigen.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+using namespace Eigen;
+using namespace geometry_msgs;
+
+
+Affine3d matrix4fToAffine3d(const Matrix4f& in) {
+ // Convert a Eigen Matrix4F to an Affine3d by first casting
+ // float to double (to a Matrix4d), then calling the constructor for Affine3d
+ // with the Matrix4d
+ return Affine3d(in.cast<double>());
+}
+
+
+// Create a ROS tf2 TransformStamped from a Eigen Affine3d,
+// parent frame id and child frame id. Stamped with current time,
+// so should be broadcast ASAP
+TransformStamped eigAndFramesToTrans(
+ const Affine3d& transform,
+ std::string parent_frame_id, std::string child_frame_id) {
+
+ TransformStamped transform_stamped = tf2::eigenToTransform(transform);
+
+ transform_stamped.header.stamp = ros::Time::now();
+ transform_stamped.header.frame_id = parent_frame_id;
+ transform_stamped.child_frame_id = child_frame_id;
+
+ return transform_stamped;
+}
+
+
+// Create a transform from a translation, rotation, and parent and
+// child frame IDs. Will stamp the transform with ros::Time::now(),
+// so the returned transform should be broadcast asap
+ TransformStamped createTransform(
+ std::string parent_frame_id, std::string child_frame_id,
+ double x, double y, double z,
+ double roll, double pitch, double yaw) {
+
+ TransformStamped tr_stamped;
+
+ tr_stamped.header.stamp = ros::Time::now();
+ tr_stamped.header.frame_id = parent_frame_id;
+ tr_stamped.child_frame_id = child_frame_id;
+
+ tr_stamped.transform.translation.x = x;
+ tr_stamped.transform.translation.y = y;
+ tr_stamped.transform.translation.z = z;
+ tf2::Quaternion q;
+ q.setRPY(roll, pitch, yaw);
+ tr_stamped.transform.rotation.x = q.x();
+ tr_stamped.transform.rotation.y = q.y();
+ tr_stamped.transform.rotation.z = q.z();
+ tr_stamped.transform.rotation.w = q.w();
+
+ return tr_stamped;
+}
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..984656fe41ab6430fb5543353c1010ed2b28a06c
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/CMakeLists.txt
@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spot_micro_plot)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES spot_micro_keyboard_command
+# CATKIN_DEPENDS rospy std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/spot_micro_keyboard_command.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/spot_micro_keyboard_command_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_keyboard_command.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/launch/start_plotting.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/launch/start_plotting.launch
new file mode 100644
index 0000000000000000000000000000000000000000..b3085aba69e93fb28fa0280391de63368e18d140
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/launch/start_plotting.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Launch plotting node standalone -->
+
+<launch>
+ <!-- Defining the node and executable and publishing the output on terminal-->
+ <node name="spot_micro_plot_node" pkg="spot_micro_plot" type="spotMicroPlot.py" output="screen">
+ </node>
+</launch>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..6670863146b7f940f280c9ee76c8e943d451e57f
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/package.xml
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_plot</name>
+ <version>0.0.0</version>
+ <description>The spot_micro_plot package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="mike@todo.todo">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/spot_micro_keyboard_command</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>rospy</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <build_export_depend>std_msgs</build_export_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spotMicroPlot.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spotMicroPlot.py
new file mode 100755
index 0000000000000000000000000000000000000000..c438cc00df49f444aac825c72b859b9e47a72c09
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spotMicroPlot.py
@@ -0,0 +1,175 @@
+#!/usr/bin/python
+
+import numpy as np
+import time
+
+import rospy
+from spot_micro_kinematics_python.spot_micro_stick_figure import SpotMicroStickFigure
+from spot_micro_kinematics_python.utilities import transformations
+from math import pi
+from std_msgs.msg import Float32, Bool
+from std_msgs.msg import Float32MultiArray
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d.axes3d as p3
+import matplotlib.animation as animation
+
+
+#########################################################################
+########################## Global Variables #############################
+#########################################################################
+num_servos = 12
+
+r2d = 180/pi
+d2r = pi/180
+
+
+# Attaching 3D axis to the figure
+fig = plt.figure()
+ax = p3.Axes3D(fig)
+ax.set_proj_type('ortho')
+ax.set_facecolor('black')
+
+ax.set_xlabel('X')
+ax.set_ylabel('Z')
+ax.set_zlabel('Y')
+
+ax.set_xlim3d([-0.2, 0.2])
+ax.set_zlim3d([0, 0.3])
+ax.set_ylim3d([0.2,-0.2])
+
+x = 0
+
+# def update_x_speed_cmd(msg):
+# '''Updates x speed command from received message'''
+# x = msg.data
+# print('here')
+
+def update_lines(num, x, lines):
+ msg = rospy.wait_for_message("/body_state", Float32MultiArray, timeout=None)
+
+ foot_data = np.array([ [msg.data[0], msg.data[1], msg.data[2]],
+ [msg.data[3], msg.data[4], msg.data[5]],
+ [msg.data[6], msg.data[7], msg.data[8]],
+ [msg.data[9], msg.data[10], msg.data[11]] ])
+
+ xpos = msg.data[12]
+ ypos = msg.data[13]
+ zpos = msg.data[14]
+
+ phi = msg.data[15]
+ theta = msg.data[16]
+ psi = msg.data[17]
+
+ # print(foot_data)
+ sm.set_absolute_foot_coordinates(foot_data)
+ temp_rot = transformations.rotxyz(phi, psi, theta)
+ temp_pose = np.identity(4)
+ temp_pose[0:3, 0:3] = temp_rot
+ temp_pose[0,3] = xpos
+ temp_pose[1,3] = ypos
+ temp_pose[2,3] = zpos
+
+ sm.set_absolute_body_pose(temp_pose)
+
+ # print(len(msg.data))
+ # print(msg.data[0])
+ # l.set_data([0, num/100.0], [0, num/100.0])
+ # print(num/100.0)
+ # l.set_3d_properties([0, num/100.0])
+
+ # Get leg coordinates and append to data list
+ coord_data = sm.get_leg_coordinates()
+
+ line_to_leg__and_link_dict = {4:(0,0),
+ 5:(0,1),
+ 6:(0,2),
+ 7:(1,0),
+ 8:(1,1),
+ 9:(1,2),
+ 10:(2,0),
+ 11:(2,1),
+ 12:(2,2),
+ 13:(3,0),
+ 14:(3,1),
+ 15:(3,2)}
+
+ for line, i in zip(lines, range(len(lines))):
+
+ line.set_linewidth(4)
+ if i < 4:
+ # First four lines are the square body
+ if i == 3:
+ ind = -1
+ else:
+ ind = i
+ x_vals = [coord_data[ind][0][0], coord_data[ind+1][0][0]]
+ y_vals = [coord_data[ind][0][1], coord_data[ind+1][0][1]]
+ z_vals = [coord_data[ind][0][2], coord_data[ind+1][0][2]]
+ # NOTE: there is no .set_data() for 3 dim data...
+ line.set_data(x_vals,z_vals)
+ line.set_3d_properties(y_vals)
+
+ # Next 12 lines are legs
+ # Leg 1, link 1, link 2, link 3
+ # Leg 2, link 1, link 2, link 3...
+ else:
+ leg_num = line_to_leg__and_link_dict[i][0]
+ link_num = line_to_leg__and_link_dict[i][1]
+ x_vals = [coord_data[leg_num][link_num][0], coord_data[leg_num][link_num+1][0]]
+ y_vals = [coord_data[leg_num][link_num][1], coord_data[leg_num][link_num+1][1]]
+ z_vals = [coord_data[leg_num][link_num][2], coord_data[leg_num][link_num+1][2]]
+
+ line.set_data(x_vals,z_vals)
+ line.set_3d_properties(y_vals)
+ return lines
+
+# Set up and title the ros node for this code
+rospy.init_node('spot_micro_plot')
+
+# Instantiate spot micro stick figure obeject
+sm = SpotMicroStickFigure(x=0,y=0.093,z=0)
+
+coords = sm.get_leg_coordinates()
+
+# Initialize empty list top hold line objects
+lines = []
+
+# Construct the body of 4 lines from the first point of each leg (the four corners of the body)
+for i in range(4):
+ # For last leg, connect back to first leg point
+ if i == 3:
+ ind = -1
+ else:
+ ind = i
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [coords[ind][0][0], coords[ind+1][0][0]]
+ y_vals = [coords[ind][0][1], coords[ind+1][0][1]]
+ z_vals = [coords[ind][0][2], coords[ind+1][0][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color='k')[0])
+
+
+# Plot color order for leg links: (hip, upper leg, lower leg)
+plt_colors = ['r','c','b']
+for leg in coords:
+ for i in range(3):
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [leg[i][0], leg[i+1][0]]
+ y_vals = [leg[i][1], leg[i+1][1]]
+ z_vals = [leg[i][2], leg[i+1][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0])
+
+
+
+
+lines_ani = animation.FuncAnimation(fig, update_lines, frames=1000, fargs=(x,lines), interval=100)
+
+plt.show()
+
+
+
+
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..0d20b6487c61e7d1bde93acf4a14b7a89083a16d
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore
@@ -0,0 +1 @@
+*.pyc
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..8ef4fa971109be35b2f8ab2c9683fb20370b3a96
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2020 mike4192
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..5a0dba04d5b08f96280c8c45480c6c50bb9b27da
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md
@@ -0,0 +1,109 @@
+# Readme
+
+A python library for forward and inverse kinematic operations on a spot micro quadruped.
+
+
+
+This library is designed around the spot micro 3d printable model developed by KDY0523, which can be found on thingverse (https://www.thingiverse.com/thing:3445283). The only requirement for this library is numpy for matrix operations.
+
+The forward and inverse kinematics of this library are based on the following paper:
+
+ Sen, Muhammed Arif & Bakircioglu, Veli & Kalyoncu, Mete. (2017).
+ Inverse Kinematic Analysis Of A Quadruped Robot.
+ International Journal of Scientific & Technology Research. 6.
+
+The figures below, taken from the paper above, show the general leg and robot geometry implemented in this library. Note, the robot geometry implemented in this library is different than that of the paper. Legs **1** and **3** are rotated 180 degrees, as that's the way they are oriented on the spot micro frame.
+
+
+
+
+## Installation
+This package can be installed via pip install. Reccomend installing into a virtual environment instead of to your system!
+For example, if you have a python virtual environment somwhere, you could call pip install to the path of this package:
+```pip install /your/path/to/spot_micro_kinematics/```
+
+Alternatively, for a quick and dirty installation, copy the this package next to a python script that wants to use it.
+
+```
+├── your_script.py
+└── spot_micro_kinematics
+ ├── __init__.py
+ ├── spot_micro_stick_figure.py
+ ├── ...
+```
+
+And in your_script.py, just import:
+```python
+from spot_micro_kinematics.spot_micro_stick_figure import SpotMicroStickFigure
+```
+
+
+## Assumptions
+This library contains various hard coded lengths of the spot micro frame as measured by me with a set of digital calipers. All dimensions are approximate at best. These lengths can be modified, they are the following properties of the SpotMicroStickFigure class:
+```
+hip_length
+upper_leg_length
+lower_leg_length
+body_width
+body_length
+```
+
+## Use
+
+The library can be used by instantiating a SpotMicroStickFigure object at a height of 0.14 meters as follows. The constructor has optional arguments to set initial body x, y, z and phi, theta, psi position and orientation values. All distances are assumed to be meters, and all angles assumed to be radians.
+
+```python
+from spot_micro_kinematics.spot_micro_stick_figure import SpotMicroStickFigure
+
+sm = SpotMicroStickFigure(x=0,y=0.14,z=0)
+```
+
+One reccomended use case is to set the absolute coordinates of all four feet at a height of 0, and at a neutral stance (where the feet are directly below the hip and shoulder joints). This can be done as follows:
+
+```python
+# Define absolute position for the legs
+l = sm.body_length
+w = sm.body_width
+l1 = sm.hip_length
+l2 = sm.upper_leg_length
+l3 = sm.lower_leg_length
+desired_p4_points = np.array([ [-l/2, 0, w/2 + l1],
+ [ l/2 , 0, w/2 + l1],
+ [ l/2 , 0, -w/2 - l1],
+ [-l/2 , 0, -w/2 - l1] ])
+
+sm.set_absolute_foot_coordinates(desired_p4_points)
+```
+
+Setting a body orientatation (for example a pitch angle of 10 degrees) can be set as follows:
+```python
+from math import pi
+sm.set_body_angles(theta=10*pi/180.0)
+```
+
+Leg coordinates and leg joint angles can be retreived as follows:
+
+```python
+# Get leg coordinates
+coords = sm.get_leg_coordinates()
+
+# Get leg angles
+leg_angs = sm.get_leg_angles()
+```
+
+
+## Limitations
+This library contains no exception handling for attempting impossible poses. For example, if a foot position is set that is impossible to achieve, a domain error will result on an underlying trigonmetric function.
+
+Additionally, this library assumes an idealized leg geometry compared to the actual spot micro 3d print model. In the real 3d model, the the first and second joints (the hip and shoulder joints) are offset height wise by approximately 1 cm. This library neglects this offset and computes about a leg as presented in the paper referenced above.
+
+## Unit tests
+This library contains some unit tests. They can be run by cloning the library, cd'ing to the root directory of the project, and then running the following command:
+```python -m unittest discover -t ..```
+
+## Plotting
+Some basic plotting scripts are also provided in the tests folder. These require matplotlib. They can be executed by cd'ing to a directory one level above where this library is checked out, and running a command as folloiws if this library were checked out to a directory called smkpython:
+`python -m smkpython.tests.plotting.plot_animation.py`
+
+A sample of the animation is shown via the gif at the top of this readme.
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif
new file mode 100644
index 0000000000000000000000000000000000000000..6cd39447efc4a7a1794b081bcd0f5b0dd32558d4
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png
new file mode 100644
index 0000000000000000000000000000000000000000..5374325a0f0fe7853325dd18eb5af36fd6d86e47
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png
new file mode 100644
index 0000000000000000000000000000000000000000..3b41249688cf7a80b79fa17a65032d77385582d9
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt
new file mode 100644
index 0000000000000000000000000000000000000000..58ed21d45633ae86bc448abaa97867afefb74f7b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt
@@ -0,0 +1 @@
+numpy==1.16.6
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..fb661037f3934a1b3f5c5b8263771f74fe8629de
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py
@@ -0,0 +1,11 @@
+from setuptools import setup
+
+setup(
+ name='spot_micro_kinematics',
+ version='1.0',
+ description='A useful module',
+ author='Mike Romanko',
+ author_email='foomail@foo.com',
+ packages=['spot_micro_kinematics','spot_micro_kinematics.utilities'], #same as name
+ install_requires=['numpy'], #external packages as dependencies
+)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py
new file mode 100644
index 0000000000000000000000000000000000000000..9d13354bfc63f4c467ebcd2d26a22b2c87cc05e5
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py
@@ -0,0 +1,360 @@
+import math
+from math import pi, sin, cos
+import matplotlib.pyplot as plt
+import numpy as np
+from .utilities import spot_micro_kinematics as smk
+from .utilities import transformations
+
+d2r = pi/180
+r2d = 180/pi
+
+class SpotMicroLeg(object):
+ '''Encapsulates a spot micro leg that consists of 3 links and 3 joint angles
+
+ Attributes:
+ _q1: Rotation angle in radians of hip joint
+ _q2: Rotation angle in radians of upper leg joint
+ _q3: Rotation angle in radians of lower leg joint
+ _l1: Length of leg link 1 (i.e.: hip joint)
+ _l2: Length of leg link 2 (i.e.: upper leg)
+ _l3: Length of leg link 3 (i.e.: lower leg)
+ _ht_leg: Homogeneous transformation matrix of leg starting
+ position and coordinate system relative to robot body.
+ 4x4 np matrix
+ _leg12: Boolean specifying whether leg is 1 or 2 (rightback or rightfront)
+ or 3 or 4 (leftfront or leftback)
+ '''
+
+ def __init__(self,q1,q2,q3,l1,l2,l3,ht_leg_start,leg12):
+ '''Constructor'''
+ self._q1 = q1
+ self._q2 = q2
+ self._q3 = q3
+ self._l1 = l1
+ self._l2 = l2
+ self._l3 = l3
+ self._ht_leg_start = ht_leg_start
+ self._leg12 = leg12
+
+ # Create homogeneous transformation matrices for each joint
+ self._t01 = smk.t_0_to_1(self._q1,self._l1)
+ self._t12 = smk.t_1_to_2()
+ self._t23 = smk.t_2_to_3(self._q2,self._l2)
+ self._t34 = smk.t_3_to_4(self._q3,self._l3)
+
+
+ def set_angles(self,q1,q2,q3):
+ '''Set the three leg angles and update transformation matrices as needed'''
+ self._q1 = q1
+ self._q2 = q2
+ self._q3 = q3
+ self._t01 = smk.t_0_to_1(self._q1,self._l1)
+ self._t23 = smk.t_2_to_3(self._q2,self._l2)
+ self._t34 = smk.t_3_to_4(self._q3,self._l3)
+
+ def set_homog_transf(self,ht_leg_start):
+ '''Set the homogeneous transformation of the leg start position'''
+ self._ht_leg_start = ht_leg_start
+
+ def get_homog_transf(self):
+ '''Return this leg's homogeneous transformation of the leg start position'''
+ return self._ht_leg_start
+
+ def set_foot_position_in_local_coords(self,x4,y4,z4):
+ '''Set the position of the foot by computing joint angles via inverse kinematics from inputted coordinates.
+ Leg's coordinate frame is the frame defined by self._ht_leg_start
+
+ Args:
+ x4: Desired foot x position in leg's coordinate frame
+ y4: Desired foot y position in leg's coordinate frame
+ z4: Desired foot z position in leg's coordinate frame
+ Returns:
+ Nothing
+ '''
+ # Run inverse kinematics and get joint angles
+ leg_angs = smk.ikine(x4,y4,z4,self._l1,self._l2,self._l3,self._leg12)
+
+ # Call method to set joint angles for leg
+ self.set_angles(leg_angs[0],leg_angs[1],leg_angs[2])
+
+ def set_foot_position_in_global_coords(self,x4,y4,z4):
+ ''' Set the position of the foot by computing joint angles via inverse kinematics from inputted coordinates.
+ Inputted coordinates in the global coordinate frame
+
+ Args:
+ x4: Desired foot x position in global coordinate frame
+ y4: Desired foot y position in global coordinate frame
+ z4: Desired foot z position in global coordinate frame
+ Returns:
+ Nothing
+ '''
+ # Get inverse of leg's homogeneous transform
+ ht_leg_inv = transformations.ht_inverse(self._ht_leg_start)
+
+ # Convert the foot coordinates for use with homogeneous transforms, e.g.:
+ # p4 = [x4, y4, z4, 1]
+ p4_global_coord = np.block( [np.array([x4, y4, z4]), np.array([1])])
+
+ # Calculate foot coordinates in each leg's coordinate system
+ p4_in_leg_coords = ht_leg_inv.dot(p4_global_coord)
+
+ # Call this leg's position set function for coordinates in local frame
+ self.set_foot_position_in_local_coords(p4_in_leg_coords[0],p4_in_leg_coords[1],p4_in_leg_coords[2])
+
+
+ def get_leg_points(self):
+ '''Get coordinates of 4 points that define a wireframe of the leg:
+ Point 1: hip/body point
+ Point 2: upper leg/hip joint
+ Point 3: Knee, (upper/lower leg joint)
+ Point 4: Foot, leg end
+
+ Returns:
+ A length 4 tuple consisting of 4 length 3 numpy arrays representing the
+ x,y,z coordinates in the global frame of the 4 leg points
+ '''
+ # Build up the total homogeneous transformation incrementally, saving each leg
+ # point along the way
+ # The total homogeneous transformation builup is:
+ # ht = ht_leg_start @ t01 @ t12 @ t23 @ t34
+ p1 = self._ht_leg_start[0:3,3]
+
+ # ht_buildup = self._ht_leg_start @ self._t01 @ self._t12
+ ht_buildup = np.matmul(np.matmul(self._ht_leg_start, self._t01), self._t12)
+
+ p2 = ht_buildup[0:3,3]
+
+ # ht_buildup = ht_buildup @ self._t23
+ ht_buildup = np.matmul(ht_buildup, self._t23)
+
+ p3 = ht_buildup[0:3,3]
+
+ # ht_buildup = ht_buildup @ self._t34
+ ht_buildup = np.matmul(ht_buildup, self._t34)
+
+ p4 = ht_buildup[0:3,3]
+
+ return (p1,p2,p3,p4)
+
+ def get_foot_position_in_global_coords(self):
+ ''' Return coordinates of the foot in the leg's local coordinate frame'''
+ # ht_foot = self._ht_leg_start @ self._t01 @ self._t12 @ self._t23 @ self._t34
+ ht_foot = np.matmul(np.matmul(np.matmul(np.matmul(self._ht_leg_start, self._t01), self._t12), self._t23), self._t34)
+ return ht_foot[0:3,3]
+
+ def get_leg_angles(self):
+ '''Return leg angles as a tuple of 3 angles, (q1, q2, q3)'''
+
+ return (self._q1,self._q2,self._q3)
+
+
+class SpotMicroStickFigure(object):
+ """Encapsulates an 12 DOF spot micro stick figure
+
+ Encapuslates a 12 DOF spot micro stick figure. The 12 degrees of freedom represent the
+ twelve joint angles. Contains inverse kinematic capabilities
+
+ Attributes:
+ hip_length: Length of the hip joint
+ upper_leg_length: length of the upper leg link
+ lower_leg_length: length of the lower leg length
+ body_width: width of the robot body
+ body_height: length of the robot body
+
+ x: x position of body center
+ y: y position of body center
+ z: z position of body center
+
+ phi: roll angle in radians of body
+ theta: pitch angle in radians of body
+ psi: yaw angle in radians of body
+
+ ht_body: homogeneous transformation matrix of the body
+
+ rightback_leg_angles: length 3 list of joint angles. Order: hip, leg, knee
+ rightfront_leg_angles: length 3 list of joint angles. Order: hip, leg, knee
+ leftfront_leg_angles: length 3 list of joint angles. Order: hip, leg, knee
+ leftback_leg_angles: length 3 list of joint angles. Order: hip, leg, knee
+
+ leg_rightback
+ leg_rightfront
+ leg_leftfront
+ leg_leftback
+
+ """
+ def __init__(self,x=0,y=.18,z=0,phi=0,theta=0,psi=0):
+ '''constructor'''
+ self.hip_length = 0.055
+ self.upper_leg_length = 0.1075
+ self.lower_leg_length = 0.130
+ self.body_width = 0.078
+ self.body_length = 0.186
+
+ self.x = x
+ self.y = y
+ self.z = z
+
+ self.phi = phi
+ self.theta = theta
+ self.psi = psi
+
+ # Initialize Body Pose
+ # Convention for this class is to initialize the body pose at a x,y,z position, with a phi,theta,psi orientation
+ # To achieve this pose, need to apply a homogeneous translation first, then a homgeneous rotation
+ # If done the other way around, a coordinate system will be rotate first, then translated along the rotated coordinate system
+ # self.ht_body = transformations.homog_transxyz(self.x,self.y,self.z) @ transformations.homog_rotxyz(self.phi,self.psi,self.theta)
+ self.ht_body = np.matmul(transformations.homog_transxyz(self.x,self.y,self.z), transformations.homog_rotxyz(self.phi,self.psi,self.theta))
+
+ # Intialize all leg angles to 0, 30, 30 degrees
+ self.rb_leg_angles = [0,-30*d2r,60*d2r]
+ self.rf_leg_angles = [0,-30*d2r,60*d2r]
+ self.lf_leg_angles = [0,30*d2r,-60*d2r]
+ self.lb_leg_angles = [0,30*d2r,-60*d2r]
+
+ # Create a dictionary to hold the legs of this spot micro object.
+ # First initialize to empty dict
+ self.legs = {}
+
+ self.legs['leg_rightback'] = SpotMicroLeg(self.rb_leg_angles[0],self.rb_leg_angles[1],self.rb_leg_angles[2],
+ self.hip_length,self.upper_leg_length,self.lower_leg_length,
+ smk.t_rightback(self.ht_body,self.body_length,self.body_width),leg12=True)
+
+ self.legs['leg_rightfront'] = SpotMicroLeg(self.rf_leg_angles[0],self.rf_leg_angles[1],self.rf_leg_angles[2],
+ self.hip_length,self.upper_leg_length,self.lower_leg_length,
+ smk.t_rightfront(self.ht_body,self.body_length,self.body_width),leg12=True)
+
+ self.legs['leg_leftfront'] = SpotMicroLeg(self.lf_leg_angles[0],self.lf_leg_angles[1],self.lf_leg_angles[2],
+ self.hip_length,self.upper_leg_length,self.lower_leg_length,
+ smk.t_leftfront(self.ht_body,self.body_length,self.body_width),leg12=False)
+
+ self.legs['leg_leftback'] = SpotMicroLeg(self.lb_leg_angles[0],self.lb_leg_angles[1],self.lb_leg_angles[2],
+ self.hip_length,self.upper_leg_length,self.lower_leg_length,
+ smk.t_leftback(self.ht_body,self.body_length,self.body_width),leg12=False)
+
+ def get_leg_coordinates(self):
+ '''Return coordinates of each leg as a tuple of 4 sets of 4 leg points'''
+
+ return (self.legs['leg_rightback'].get_leg_points(),
+ self.legs['leg_rightfront'].get_leg_points(),
+ self.legs['leg_leftfront'].get_leg_points(),
+ self.legs['leg_leftback'].get_leg_points())
+
+ def set_leg_angles(self,leg_angs):
+ ''' Set the leg angles for all four legs
+
+ Args:
+ leg_angs: Tuple of 4 lists of leg angles. Legs in the order rightback
+ rightfront, leftfront, leftback. ANgles in the order q1,q2,q3.
+ An example input:
+ ((rb_q1,rb_q2,rb_q3),
+ (rf_q1,rf_q2,rf_q3),
+ (lf_q1,lf_q2,lf_q3),
+ (lb_q1,lb_q2,lb_q3))
+
+ Returns:
+ Nothing
+ '''
+ self.legs['leg_rightback'].set_angles(leg_angs[0][0],leg_angs[0][1],leg_angs[0][2])
+ self.legs['leg_rightfront'].set_angles(leg_angs[1][0],leg_angs[1][1],leg_angs[1][2])
+ self.legs['leg_leftfront'].set_angles(leg_angs[2][0],leg_angs[2][1],leg_angs[2][2])
+ self.legs['leg_leftback'].set_angles(leg_angs[3][0],leg_angs[3][1],leg_angs[3][2])
+
+
+ def set_absolute_foot_coordinates(self,foot_coords):
+ '''Set foot coordinates to a set inputted in the global coordinate frame and compute
+ and set the joint angles to achieve them using inverse kinematics
+
+ Args:
+ foot_coords: A 4x3 numpy matrix of desired (x4,y4,z4) positions for the end point (point 4) of each of
+ the four legs. I.e., the foot.
+ Leg order: rigthback, rightfront, leftfront, leftback. Example input:
+ np.array( [ [x4_rb,y4_rb,z4_rb],
+ [x4_rf,y4_rf,z4_rf],
+ [x4_lf,y4_lf,z4_lf],
+ [x4_lb,y4_lb,z4_lb] ])
+ Returns:
+ Nothing
+ '''
+
+ # For each leg, call its method to set foot position in global coordinate frame
+
+ foot_coords_dict = {'leg_rightback':foot_coords[0],
+ 'leg_rightfront':foot_coords[1],
+ 'leg_leftfront':foot_coords[2],
+ 'leg_leftback':foot_coords[3]}
+
+ for leg_name in self.legs:
+ x4 = foot_coords_dict[leg_name][0]
+ y4 = foot_coords_dict[leg_name][1]
+ z4 = foot_coords_dict[leg_name][2]
+ self.legs[leg_name].set_foot_position_in_global_coords(x4,y4,z4)
+
+ def set_absolute_body_pose(self, ht_body):
+ '''Set absolute pose of body, while holding foot positions in place'''
+
+ # Get current foot position of each leg in global coordinate system
+ # These are 1x3 numpy arrays
+ foot_coords = {}
+ for leg_name in self.legs:
+ foot_coords[leg_name] = self.legs[leg_name].get_foot_position_in_global_coords()
+
+ # Set body pose
+ self.ht_body = ht_body
+
+ # Update each leg's homogeneous transformation
+ self.legs['leg_rightback'].set_homog_transf(smk.t_rightback(self.ht_body,self.body_length,self.body_width))
+ self.legs['leg_rightfront'].set_homog_transf(smk.t_rightfront(self.ht_body,self.body_length,self.body_width))
+ self.legs['leg_leftfront'].set_homog_transf(smk.t_leftfront(self.ht_body,self.body_length,self.body_width))
+ self.legs['leg_leftback'].set_homog_transf(smk.t_leftback(self.ht_body,self.body_length,self.body_width))
+
+ # Prep foot coordinates to call method to set absolute foot coordinates
+ foot_coords_matrix = np.block([ [foot_coords['leg_rightback']],
+ [foot_coords['leg_rightfront']],
+ [foot_coords['leg_leftfront']],
+ [foot_coords['leg_leftback']] ])
+
+ self.set_absolute_foot_coordinates(foot_coords_matrix)
+
+ def set_body_angles(self,phi=0,theta=0,psi=0):
+ '''Set a body angles without translation of the body
+
+ Args:
+ phi: roll angle in radians
+ theta: pitch angle in radians
+ psi: yaw angle in radians
+ Returns:
+ Nothing
+ '''
+ # Create a xyz rotation matrix
+ r_xyz = transformations.rotxyz(phi,psi,theta)
+
+ # Get current body pose, and replace rotation part with r_xyz
+ ht_body = self.ht_body
+
+ ht_body[0:3,0:3] = r_xyz
+
+ # Call method to set absolute body pose
+ self.set_absolute_body_pose(ht_body)
+
+ def get_leg_angles(self):
+ ''' Get the leg angles for all four legs
+ Args:
+ None
+ Returns:
+ leg_angs: Tuple of 4 of the leg angles. Legs in the order rightback
+ rightfront, leftfront, leftback. Angles in the order q1,q2,q3.
+ An example output:
+ ((rb_q1,rb_q2,rb_q3),
+ (rf_q1,rf_q2,rf_q3),
+ (lf_q1,lf_q2,lf_q3),
+ (lb_q1,lb_q2,lb_q3))
+ '''
+ return ( self.legs['leg_rightback'].get_leg_angles(),
+ self.legs['leg_rightfront'].get_leg_angles(),
+ self.legs['leg_leftfront'].get_leg_angles(),
+ self.legs['leg_leftback'].get_leg_angles() )
+
+
+ def print_leg_angles(self):
+ ''' Print the joint angles for alll four legs'''
+ return None
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py
new file mode 100755
index 0000000000000000000000000000000000000000..37e0af69d398eebc35c668fbfa029fc3e9c10445
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py
@@ -0,0 +1,147 @@
+#!/usr/bin/env python
+
+import numpy as np
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d.axes3d as p3
+import matplotlib.animation as animation
+from math import pi
+from ...spot_micro_stick_figure import SpotMicroStickFigure
+from ...utilities import spot_micro_kinematics as smk
+
+d2r = pi/180
+r2d = 180/pi
+
+
+def update_lines(num, coord_data, lines):
+
+ line_to_leg__and_link_dict = {4:(0,0),
+ 5:(0,1),
+ 6:(0,2),
+ 7:(1,0),
+ 8:(1,1),
+ 9:(1,2),
+ 10:(2,0),
+ 11:(2,1),
+ 12:(2,2),
+ 13:(3,0),
+ 14:(3,1),
+ 15:(3,2)}
+
+ for line, i in zip(lines, range(len(lines))):
+
+ if i < 4:
+ # First four lines are the square body
+ if i == 3:
+ ind = -1
+ else:
+ ind = i
+ x_vals = [coord_data[num][ind][0][0], coord_data[num][ind+1][0][0]]
+ y_vals = [coord_data[num][ind][0][1], coord_data[num][ind+1][0][1]]
+ z_vals = [coord_data[num][ind][0][2], coord_data[num][ind+1][0][2]]
+ # NOTE: there is no .set_data() for 3 dim data...
+ line.set_data(x_vals,z_vals)
+ line.set_3d_properties(y_vals)
+
+ # Next 12 lines are legs
+ # Leg 1, link 1, link 2, link 3
+ # Leg 2, link 1, link 2, link 3...
+ else:
+ leg_num = line_to_leg__and_link_dict[i][0]
+ link_num = line_to_leg__and_link_dict[i][1]
+ x_vals = [coord_data[num][leg_num][link_num][0], coord_data[num][leg_num][link_num+1][0]]
+ y_vals = [coord_data[num][leg_num][link_num][1], coord_data[num][leg_num][link_num+1][1]]
+ z_vals = [coord_data[num][leg_num][link_num][2], coord_data[num][leg_num][link_num+1][2]]
+
+ line.set_data(x_vals,z_vals)
+ line.set_3d_properties(y_vals)
+ return lines
+
+
+# Attaching 3D axis to the figure
+fig = plt.figure()
+ax = p3.Axes3D(fig)
+
+ax.set_xlabel('X')
+ax.set_ylabel('Z')
+ax.set_zlabel('Y')
+
+ax.set_xlim3d([-0.2, 0.2])
+ax.set_zlim3d([0, 0.4])
+ax.set_ylim3d([-0.2,0.2])
+
+# Set azimtuth and elevation of plot
+# ax.view_init(elev=135,azim=0)
+
+# Instantiate spot micro stick figure obeject
+sm = SpotMicroStickFigure(x=0,y=0.14,z=0, theta=00*d2r)
+
+# Define absolute position for the legs
+l = sm.body_length
+w = sm.body_width
+l1 = sm.hip_length
+l2 = sm.upper_leg_length
+l3 = sm.lower_leg_length
+desired_p4_points = np.array([ [-l/2, 0, w/2 + l1],
+ [ l/2 , 0, w/2 + l1],
+ [ l/2 , 0, -w/2 - l1],
+ [-l/2 , 0, -w/2 - l1] ])
+
+sm.set_absolute_foot_coordinates(desired_p4_points)
+
+# Set a pitch angle
+sm.set_body_angles(theta=00*d2r)
+
+# Get leg coordinates
+coords = sm.get_leg_coordinates()
+
+# Initialize empty list top hold line objects
+lines = []
+
+# Construct the body of 4 lines from the first point of each leg (the four corners of the body)
+for i in range(4):
+ # For last leg, connect back to first leg point
+ if i == 3:
+ ind = -1
+ else:
+ ind = i
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [coords[ind][0][0], coords[ind+1][0][0]]
+ y_vals = [coords[ind][0][1], coords[ind+1][0][1]]
+ z_vals = [coords[ind][0][2], coords[ind+1][0][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color='k')[0])
+
+
+# Plot color order for leg links: (hip, upper leg, lower leg)
+plt_colors = ['r','c','b']
+for leg in coords:
+ for i in range(3):
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [leg[i][0], leg[i+1][0]]
+ y_vals = [leg[i][1], leg[i+1][1]]
+ z_vals = [leg[i][2], leg[i+1][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0])
+
+# Create data of robot pitching up and down
+num_angles = 25
+pitch_angles = np.linspace(-30*d2r,30*d2r,num_angles)
+coord_data = []
+for theta in pitch_angles:
+ # Set a pitch angle
+ sm.set_body_angles(theta=theta)
+
+ x = sm.get_leg_angles()
+
+ # Get leg coordinates and append to data list
+ coord_data.append(sm.get_leg_coordinates())
+
+coord_data = coord_data + coord_data[::-1]
+
+# Creating the Animation object
+line_ani = animation.FuncAnimation(fig, update_lines, num_angles*2, fargs=(coord_data, lines),
+ interval=75, blit=False)
+
+plt.show()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py
new file mode 100755
index 0000000000000000000000000000000000000000..e4fcad67c29f9d1c785a1c92de4ff3020ddc088e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py
@@ -0,0 +1,88 @@
+#!/usr/bin/env python
+
+import numpy as np
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d.axes3d as p3
+import matplotlib.animation as animation
+from math import pi
+from ...spot_micro_stick_figure import SpotMicroStickFigure
+from ...utilities import spot_micro_kinematics as smk
+
+d2r = pi/180
+r2d = 180/pi
+
+# Attaching 3D axis to the figure
+fig = plt.figure()
+ax = p3.Axes3D(fig)
+
+ax.set_xlabel('X')
+ax.set_ylabel('Z')
+ax.set_zlabel('Y')
+
+ax.set_xlim3d([-0.2, 0.2])
+ax.set_zlim3d([0, 0.4])
+ax.set_ylim3d([-0.2,0.2])
+
+# Set azimtuth and elevation of plot
+# ax.view_init(elev=135,azim=0)
+
+# Instantiate spot micro stick figure obeject
+sm = SpotMicroStickFigure(x=0,y=0.18,z=0, theta=00*d2r)
+
+# # Try setting each leg to a desired y position
+# x4 = -.055
+# y4 = -.18
+# z4 = 0
+
+# Define absolute position for the legs
+l = sm.body_length
+w = sm.body_width
+l1 = sm.hip_length
+l2 = sm.upper_leg_length
+l3 = sm.lower_leg_length
+desired_p4_points = np.array([ [-l/2, 0, w/2 + l1],
+ [ l/2 , 0, w/2 + l1],
+ [ l/2 , 0, -w/2 - l1],
+ [-l/2 , 0, -w/2 - l1] ])
+
+sm.set_absolute_foot_coordinates(desired_p4_points)
+
+# Set a pitch angle
+sm.set_body_angles(theta=10*d2r)
+
+# Get leg coordinates
+coords = sm.get_leg_coordinates()
+
+# Initialize empty list top hold line objects
+lines = []
+
+# Construct the body of 4 lines from the first point of each leg (the four corners of the body)
+for i in range(4):
+ # For last leg, connect back to first leg point
+ if i == 3:
+ ind = -1
+ else:
+ ind = i
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [coords[ind][0][0], coords[ind+1][0][0]]
+ y_vals = [coords[ind][0][1], coords[ind+1][0][1]]
+ z_vals = [coords[ind][0][2], coords[ind+1][0][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color='k')[0])
+
+
+# Plot color order for leg links: (hip, upper leg, lower leg)
+plt_colors = ['r','c','b']
+for leg in coords:
+ for i in range(3):
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [leg[i][0], leg[i+1][0]]
+ y_vals = [leg[i][1], leg[i+1][1]]
+ z_vals = [leg[i][2], leg[i+1][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0])
+
+plt.show()
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py
new file mode 100755
index 0000000000000000000000000000000000000000..61c804feae1ddd50cd9db2bcf7202f65bdc0ff1b
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+import numpy as np
+import matplotlib.pyplot as plt
+import mpl_toolkits.mplot3d.axes3d as p3
+import matplotlib.animation as animation
+from math import pi
+from ...spot_micro_stick_figure import SpotMicroLeg
+from ...utilities import transformations
+from ...utilities import spot_micro_kinematics as smk
+
+d2r = pi/180
+r2d = 180/pi
+
+# Attaching 3D axis to the figure
+fig = plt.figure()
+ax = p3.Axes3D(fig)
+
+ax.set_xlabel('X')
+ax.set_zlabel('Y')
+ax.set_ylabel('Z')
+
+ax.set_xlim3d([-0.15, 0.15])
+ax.set_zlim3d([-0.25, 0.05])
+ax.set_ylim3d([-0.15,0.15])
+
+# ax.invert_yaxis()
+
+# Set azimtuth and elevation of plot
+# ax.view_init(elev=135,azim=0)
+
+# Instantiate spot micro stick figure obeject
+ht_start = transformations.homog_transform(0,0,0,0,0,0)
+l1 = 0.055
+l2 = 0.1075
+l3 = 0.13
+sml = SpotMicroLeg(0,0,0,l1,l2,l3,ht_start,leg12=True)
+
+# Try leg to a desired position
+x4 = 0
+y4 = -0.18
+z4 = 00.05
+
+(q1,q2,q3) = smk.ikine(x4,y4,z4,l1,l2,l3,legs12 = True)
+
+print('Leg angles')
+print('q1: %2.1f deg, q2: %2.1f deg, q3: %2.1f deg'%(q1*r2d,q2*r2d,q3*r2d))
+
+sml.set_angles(q1,q2,q3)
+# sml.set_angles(10*d2r,0*d2r,00*d2r)
+
+
+# Get leg coordinates
+coords = sml.get_leg_points()
+
+print('Right Back p4 coordinates:')
+print('x4: %1.3f, y4: %1.3f, z4: %1.3f'%(coords[3][0],coords[3][1],coords[3][2]))
+
+
+# Initialize empty list top hold line objects
+lines = []
+
+# Plot color order for leg links: (hip, upper leg, lower leg)
+plt_colors = ['r','c','b']
+for i in range(3):
+
+ # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure
+ # appear oriented better
+ x_vals = [coords[i][0], coords[i+1][0]]
+ y_vals = [coords[i][1], coords[i+1][1]]
+ z_vals = [coords[i][2], coords[i+1][2]]
+ lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0])
+
+
+plt.show()
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py
new file mode 100644
index 0000000000000000000000000000000000000000..895d5714e826610ffd6f20f1ae24733e63581a9a
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py
@@ -0,0 +1,52 @@
+'''Tests the spot micro stick figure and spot micro leg classes'''
+
+import unittest
+from ..spot_micro_stick_figure import SpotMicroLeg
+from ..utilities import spot_micro_kinematics as smk
+from ..utilities import transformations
+import numpy as np
+from math import cos, sin, pi
+
+d2r = pi/180
+r2d = 180/pi
+
+class TestSpotMicroLeg(unittest.TestCase):
+ '''Tests spot micro leg class'''
+
+ def test_leg(self):
+ '''Test a spot micro leg with contrived angles and leg link lengths'''
+
+ q1 = 0*d2r
+ q2 = 0*d2r
+ q3 = 30*d2r
+ l1 = 1
+ l2 = 2
+ l3 = 3
+
+ # Body coordinate system homogeneous transformation. Center at 0, no rotations
+ body_ht = transformations.homog_transform(0,0,0,0,0,0)
+
+ # Right front leg homogeneous transform. For test, create with 0 width and length, so
+ # leg start position should be at origin aligned with global coordinate frame
+ ht_rf = smk.t_rightfront(body_ht,0,0)
+
+ leg = SpotMicroLeg(q1,q2,q3,l1,l2,l3,ht_rf,leg12=True)
+
+ (p1,p2,p3,p4) = leg.get_leg_points()
+
+ known_true_p1 = np.array([0,0,0])
+ known_true_p2 = np.array([0,0,1])
+ known_true_p3 = np.array([0,-2,1])
+ known_true_p4 = np.array([l3*sin(q3),-l3*cos(q3)+-2,1])
+
+ try:
+ np.testing.assert_array_almost_equal(p1, known_true_p1)
+ np.testing.assert_array_almost_equal(p2, known_true_p2)
+ np.testing.assert_array_almost_equal(p3, known_true_p3)
+ np.testing.assert_array_almost_equal(p4, known_true_p4)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py
new file mode 100644
index 0000000000000000000000000000000000000000..be9a3673dbd16306c350c556b3cb57af289e183e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py
@@ -0,0 +1,233 @@
+"""Forward and inverse kinematic transformations for a quadriped robot.
+
+Equations from:
+Sen, Muhammed Arif & Bakircioglu, Veli & Kalyoncu, Mete. (2017).
+Inverse Kinematic Analysis Of A Quadruped Robot.
+International Journal of Scientific & Technology Research. 6.
+"""
+
+from . import transformations
+from math import pi, cos, sin, atan2, sqrt
+import numpy as np
+
+def t_rightback(t_m,l,w):
+ '''Creates a 4x4 numpy homogeneous transformation matrix representing coordinate system and
+ position of the rightback leg of a quadriped. Assumes legs postioned in corners of a rectangular
+ plane defined by a width and length
+
+ Args:
+ t_m: 4x4 numpy matrix. Homogeneous transform representing the coordinate system of the center
+ of the robot body
+ l: length of the robot body
+ w: width of the robot body
+
+ Returns:
+ 4x4 numpy matrix. A homogeneous transformation representing the position of the right back leg
+ '''
+ temp_homog_transf = np.block( [ [ transformations.roty(pi/2), np.array([[-l/2],[0],[w/2]]) ],
+ [np.array([0,0,0,1])] ] )
+ # return t_m @ temp_homog_transf
+ return np.matmul(t_m,temp_homog_transf)
+
+def t_rightfront(t_m,l,w):
+ '''Creates a 4x4 numpy homogeneous transformation matrix representing coordinate system and
+ position of the rightfront leg of a quadriped. Assumes legs postioned in corners of a rectangular
+ plane defined by a width and length
+
+ Args:
+ t_m: 4x4 numpy matrix. Homogeneous transform representing the coordinate system of the center
+ of the robot body
+ l: length of the robot body
+ w: width of the robot body
+
+ Returns:
+ 4x4 numpy matrix. A homogeneous transformation representing the position of the right front leg
+ '''
+ temp_homog_transf = np.block( [ [ transformations.roty(pi/2), np.array([[l/2],[0],[w/2]]) ],
+ [np.array([0,0,0,1])] ] )
+ # return t_m @ temp_homog_transf
+ return np.matmul(t_m,temp_homog_transf)
+
+def t_leftfront(t_m,l,w):
+ '''Creates a 4x4 numpy homogeneous transformation matrix representing coordinate system and
+ position of the left front leg of a quadriped. Assumes legs postioned in corners of a rectangular
+ plane defined by a width and length
+
+ Args:
+ t_m: 4x4 numpy matrix. Homogeneous transform representing the coordinate system of the center
+ of the robot body
+ l: length of the robot body
+ w: width of the robot body
+
+ Returns:
+ 4x4 numpy matrix. A homogeneous transformation representing the position of the left front leg
+ '''
+ temp_homog_transf = np.block( [ [ transformations.roty(-pi/2), np.array([[l/2],[0],[-w/2]]) ],
+ [np.array([0,0,0,1])] ] )
+ # return t_m @ temp_homog_transf
+ return np.matmul(t_m,temp_homog_transf)
+
+def t_leftback(t_m,l,w):
+ '''Creates a 4x4 numpy homogeneous transformation matrix representing coordinate system and
+ position of the left back leg of a quadriped. Assumes legs postioned in corners of a rectangular
+ plane defined by a width and length
+
+ Args:
+ t_m: 4x4 numpy matrix. Homogeneous transform representing the coordinate system of the center
+ of the robot body
+ l: length of the robot body
+ w: width of the robot body
+
+ Returns:
+ 4x4 numpy matrix. A homogeneous transformation representing the position of the left back leg
+ '''
+ temp_homog_transf = np.block( [ [ transformations.roty(-pi/2), np.array([[-l/2],[0],[-w/2]]) ],
+ [np.array([0,0,0,1])] ] )
+ # return t_m @ temp_homog_transf
+ return np.matmul(t_m,temp_homog_transf)
+
+
+def t_0_to_1(theta1,l1):
+ '''Create the homogeneous transformation matrix for joint 0 to 1 for a quadriped leg.
+
+ Args:
+ theta1: Rotation angle in radians of the hip joint
+ l1: Length of the hip joint link
+
+ Returns:
+ A 4x4 numpy matrix. Homogeneous transform from joint 0 to 1
+ '''
+ # I believe there is a typo in the paper. The paper lists this matrix as:
+ #
+ # t = [ cos(theta1) -sin(theta1) 0 -l1*cos(theta1);
+ # sin(theta1) -cos(theta1) 0 -l1*sin(theta1);
+ # 1 0 0 0;
+ # 0 0 0 1;]
+ #
+ # However I believe index [2],[0] should be zero, and index [2],[2] should be 1 instead.
+ # If not, then the rotated z axis disapears?? And from the diagram, it appears the transformed
+ # axis's z axis is the same as the original. So I think the matrix should be:
+ #
+ # t = [ cos(theta1) -sin(theta1) 0 -l1*cos(theta1);
+ # sin(theta1) -cos(theta1) 0 -l1*sin(theta1);
+ # 0 0 1 0;d
+ # 0 0 0 1;]
+
+ t_01 = np.block( [ [ transformations.rotz(theta1), np.array([[-l1*cos(theta1)],[-l1*sin(theta1)],[0]]) ],
+ [np.array([0,0,0,1])] ] )
+ return t_01
+
+
+def t_1_to_2():
+ '''Create the homogeneous transformation matrix for joint 1 to 2 for a quadriped leg.
+
+ Args:
+ None
+
+ Returns:
+ A 4x4 numpy matrix. Homogeneous transform from joint 1 to 2
+ '''
+ # I believe there is a typo in the paper. The paper lists this matrix as:
+ #
+ # t = [ 0 0 -1 0;
+ # -1 0 0 0;
+ # 0 0 1 0;
+ # 0 0 0 1;]
+ #
+ # However I believe index [1],[2] should be 1, and index [2],[2] should be 0.
+ # If not, then the rotated y axis disapears?? So I think the matrix should be:
+ #
+ # t = [ 0 0 -1 0;
+ # -1 0 0 0;
+ # 0 1 0 0;
+ # 0 0 0 1;]
+ #
+ t_12 = np.array([[ 0, 0, -1, 0],
+ [-1, 0, 0, 0],
+ [ 0, 1, 0, 0],
+ [ 0, 0, 0, 1]])
+ return t_12
+
+def t_2_to_3(theta2,l2):
+ '''Create the homogeneous transformation matrix for joint 1 to 2 for a quadriped leg.
+
+ Args:
+ theta2: Rotation angle in radians of the leg joint
+ l2: Length of the upper leg link
+
+ Returns:
+ A 4x4 numpy matrix. Homogeneous transform from joint 2 to 3
+ '''
+
+ t_23 = np.block( [ [ transformations.rotz(theta2), np.array([[l2*cos(theta2)],[l2*sin(theta2)],[0]]) ],
+ [np.array([0,0,0,1])] ] )
+ return t_23
+
+def t_3_to_4(theta3,l3):
+ '''Create the homogeneous transformation matrix for joint 3 to 4 for a quadriped leg.
+
+ Args:
+ theta3: Rotation angle in radians of the knee joint
+ l3: Length of the lower leg link
+
+ Returns:
+ A 4x4 numpy matrix. Homogeneous transform from joint 3 to 4
+ '''
+
+ t_34 = np.block( [ [ transformations.rotz(theta3), np.array([[l3*cos(theta3)],[l3*sin(theta3)],[0]]) ],
+ [np.array([0,0,0,1])] ] )
+ return t_34
+
+def t_0_to_4(theta1, theta2, theta3, l1, l2, l3):
+ '''Create the homogeneous transformation matrix from joint 0 to 4 of a quadriped leg
+
+ Args:
+ theta1: Rotation angle in radians of joint 1
+ theta2: Rotation angle in radians of joint 2
+ theta3: Rotation angle in radians of joint 3
+ l1: Length of leg link 1, the hip length
+ l2: Length of leg link 2, the uppper leg length
+ l3: Length of leg link 3, the lower leg
+
+ Returns:
+ A 4x4 numpy matrix. Homogeneous transform from joint 0 to 4
+ '''
+ # return t_0_to_1(theta1,l1) @ t_1_to_2() @ t_2_to_3(theta2,l2) @ t_3_to_4(theta3,l3)
+ return np.matmul(np.matmul(np.matmul(t_0_to_1(theta1,l1), t_1_to_2()), t_2_to_3(theta2,l2)), t_3_to_4(theta3,l3))
+
+def ikine(x4,y4,z4,l1,l2,l3,legs12=True):
+ '''Use inverse kinematics fo calculate the leg angles for a leg to achieve a desired
+ leg end point position (x4,y4,z4)
+
+ Args:
+ x4: x position of leg end point relative to leg start point coordinate system.
+ y4: y position of leg end point relative to leg start point coordinate system.
+ z4: z position of leg end point relative to leg start point coordinate system.
+ l1: leg link 1 length
+ l2: leg link 2 length
+ l3: leg link 3 length
+ legs12: Optional input, boolean indicating whether equations are for legs 1 or 2.
+ If false, then equation for legs 3 and 4 is used
+
+ Returns:
+ A length 3 tuple of leg angles in the order (q1,q2,q3)
+ '''
+
+ # Supporting variable D
+ D = (x4**2 + y4**2 + z4**2 - l1**2 - l2**2 - l3**2)/(2*l2*l3)
+
+ if legs12 == True:
+ q3 = atan2(sqrt(1-D**2),D)
+ else:
+ q3 = atan2(-sqrt(1-D**2),D)
+
+ q2 = atan2(z4, sqrt(x4**2 + y4**2 - l1**2)) - atan2(l3*sin(q3), l2 + l3*cos(q3) )
+
+ # After using the equations, there seem to be two errors:
+ # 1. The first y4 should not have a negative sign
+ # 2. The entire equation should be multiplied by -1
+ # The equation for q1 below reflects these changes
+ q1 = atan2(y4, x4) + atan2(sqrt(x4**2 + y4**2 - l1**2), -l1)
+
+ return (q1,q2,q3)
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py
new file mode 100644
index 0000000000000000000000000000000000000000..2a511a787027f6b9183cb756136f50f805637e5c
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py
@@ -0,0 +1,221 @@
+"""Tests for the spot micro kinematics module"""
+
+import unittest
+from .. import spot_micro_kinematics
+from .. import transformations
+import numpy as np
+from math import cos, sin, pi
+
+class TestSpotMicroKinematics(unittest.TestCase):
+ '''Tests rotation matrices'''
+
+ def test_t_rightback(self):
+ '''Test the homogeneous transformation to the right back leg via a contrived example'''
+
+ # First create a homogenous transformation representing the robot center body.
+ # No rotations, no translations
+
+ t_m = transformations.homog_transform(0,0,0,0,0,0)
+ l = 2
+ w = 3
+ t_rightback = spot_micro_kinematics.t_rightback(t_m,l,w)
+
+ known_true_rot = np.array(
+ [ [cos(pi/2), 0, sin(pi/2), -l/2],
+ [0, 1, 0, 0],
+ [-sin(pi/2), 0, cos(pi/2), w/2],
+ [0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t_rightback, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+
+ def test_t_rightfront(self):
+ '''Test the homogeneous transformation to the right front leg via a contrived example'''
+
+ # First create a homogenous transformation representing the robot center body.
+ # No rotations, no translations
+
+ t_m = transformations.homog_transform(0,0,0,0,0,0)
+ l = 2
+ w = 3
+ t_rightfront = spot_micro_kinematics.t_rightfront(t_m,l,w)
+
+ known_true_rot = np.array(
+ [ [cos(pi/2), 0, sin(pi/2), l/2],
+ [0, 1, 0, 0],
+ [-sin(pi/2), 0, cos(pi/2), w/2],
+ [0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t_rightfront, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_leftfront(self):
+ '''Test the homogeneous transformation to the left front leg via a contrived example'''
+
+ # First create a homogenous transformation representing the robot center body.
+ # No rotations, no translations
+
+ t_m = transformations.homog_transform(0,0,0,0,0,0)
+ l = 2
+ w = 3
+ t_leftfront = spot_micro_kinematics.t_leftfront(t_m,l,w)
+
+ known_true_rot = np.array(
+ [ [cos(-pi/2), 0, sin(-pi/2), l/2],
+ [0, 1, 0, 0],
+ [-sin(-pi/2), 0, cos(-pi/2), -w/2],
+ [0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t_leftfront, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_leftback(self):
+ '''Test the homogeneous transformation to the left back leg via a contrived example'''
+
+ # First create a homogenous transformation representing the robot center body.
+ # No rotations, no translations
+
+ t_m = transformations.homog_transform(0,0,0,0,0,0)
+ l = 2
+ w = 3
+ t_leftback = spot_micro_kinematics.t_leftback(t_m,l,w)
+
+ known_true_rot = np.array(
+ [ [cos(-pi/2), 0, sin(-pi/2), -l/2],
+ [0, 1, 0, 0],
+ [-sin(-pi/2), 0, cos(-pi/2), -w/2],
+ [0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t_leftback, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_0_to_1(self):
+ '''Test the homogeneous transformation of leg joint 0 to joint 1 via a contrived example'''
+
+ theta1 = 35*pi/180
+ l1 = 0.4
+ t = spot_micro_kinematics.t_0_to_1(theta1,l1)
+
+ known_true = np.array(
+ [ [cos(theta1), -sin(theta1), 0, -l1*cos(theta1)],
+ [sin(theta1), cos(theta1), 0, -l1*sin(theta1)],
+ [ 0, 0, 1, 0],
+ [ 0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t, known_true)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_1_to_2(self):
+ '''Test the homogeneous transformation of leg joint coord. system 1 to 2 via a contrived example'''
+
+ t = spot_micro_kinematics.t_1_to_2()
+
+ known_true = np.array(
+ [ [ 0, 0, -1, 0],
+ [-1, 0, 0, 0],
+ [ 0, 1, 0, 0],
+ [ 0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t, known_true)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_2_to_3(self):
+ '''Test the homogeneous transformation of leg joint 2 to 3 via a contrived example'''
+
+ theta2 = 35*pi/180
+ l2 = 0.4
+ t = spot_micro_kinematics.t_2_to_3(theta2,l2)
+
+ known_true = np.array(
+ [ [cos(theta2), -sin(theta2), 0, l2*cos(theta2)],
+ [sin(theta2), cos(theta2), 0, l2*sin(theta2)],
+ [ 0, 0, 1, 0],
+ [ 0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t, known_true)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_3_to_4(self):
+ '''Test the homogeneous transformation of leg joint 3 to 4 via a contrived example'''
+
+ theta3 = 35*pi/180
+ l3 = 0.4
+ t = spot_micro_kinematics.t_3_to_4(theta3,l3)
+
+ known_true = np.array(
+ [ [cos(theta3), -sin(theta3), 0, l3*cos(theta3)],
+ [sin(theta3), cos(theta3), 0, l3*sin(theta3)],
+ [ 0, 0, 1, 0],
+ [ 0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(t, known_true)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_t_0_to_4(self):
+ '''Test the homogeneous transformation of leg joint 0 to 4 via a contrived example'''
+
+ theta1 = 10*pi/180
+ theta2 = 15*pi/180
+ theta3 = 35*pi/180
+ l1 = 0.1
+ l2 = 0.2
+ l3 = 0.4
+
+ t01 = spot_micro_kinematics.t_0_to_1(theta1,l1)
+ t12 = spot_micro_kinematics.t_1_to_2()
+ t23 = spot_micro_kinematics.t_2_to_3(theta2,l2)
+ t34 = spot_micro_kinematics.t_3_to_4(theta3,l3)
+
+ # known_true = t01 @ t12 @ t23 @ t34
+ known_true = np.matmul(np.matmul(np.matmul(t01, t12), t23), t34)
+
+ t = spot_micro_kinematics.t_0_to_4(theta1,theta2,theta3,l1,l2,l3)
+
+ try:
+ np.testing.assert_array_almost_equal(t, known_true)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py
new file mode 100644
index 0000000000000000000000000000000000000000..e4173698531ff5aaf3f9b1d8dfcb70004dd692e3
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py
@@ -0,0 +1,177 @@
+"""Tests for the transformations module"""
+
+from .. import transformations
+import unittest
+import math
+import numpy as np
+
+d2r = math.pi/180 # deg to rad conversion
+r2d = 180/math.pi # rad to deg conversion
+
+class TestRotationMatrices(unittest.TestCase):
+ '''Tests rotation matrices'''
+
+ def test_rotx(self):
+ '''Test the x rotation matrix via a contrived example'''
+ rot = transformations.rotx(90*d2r)
+ known_true_rot = np.array(
+ [ [1, 0, 0],
+ [0, 0, -1],
+ [0, 1, 0] ])
+
+ try:
+ np.testing.assert_array_almost_equal(rot, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_roty(self):
+ '''Test the y rotation matrix via a contrived example'''
+ rot = transformations.roty(90*d2r)
+ known_true_rot = np.array(
+ [ [0, 0, 1],
+ [0, 1, 0],
+ [-1, 0, 0] ])
+
+ try:
+ np.testing.assert_array_almost_equal(rot, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_rotz(self):
+ '''Test the z rotation matrix via a contrived example'''
+ rot = transformations.rotz(90*d2r)
+ known_true_rot = np.array(
+ [ [0, -1, 0],
+ [1, 0, 0],
+ [0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(rot, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_rotxyz(self):
+ '''Test the x,y,z rotation matrix via a contrived example'''
+
+ rot = transformations.rotxyz(90*d2r,90*d2r,90*d2r)
+
+ known_true_rot = np.array(
+ [ [0, 0, 1],
+ [0, -1, 0],
+ [1, 0, 0] ])
+
+ try:
+ np.testing.assert_array_almost_equal(rot, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_translate(self):
+ '''Test the linear translation on a vector example'''
+
+ vec = np.array([1,2,3,1])
+
+ tranlate_matrix = transformations.homog_transxyz(1,2,3)
+
+ vec_translated = tranlate_matrix.dot(vec)
+
+ known_answer = np.array([2,4,6,1])
+
+ try:
+ np.testing.assert_array_almost_equal(vec_translated, known_answer)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_homog_rotxyz(self):
+ '''Test the homogeneous rotation matrix generation'''
+
+ hr = transformations.homog_rotxyz(90*d2r,90*d2r,90*d2r)
+
+ known_answer = np.array([[0,0,1,0],
+ [0,-1,0,0],
+ [1,0,0,0],
+ [0,0,0,1]])
+
+ try:
+ np.testing.assert_array_almost_equal(hr, known_answer)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_homog_transform(self):
+ '''Test the homogeneous transformation via a contrived example'''
+
+ ht = transformations.homog_transform(90*d2r,90*d2r,90*d2r,1,2,3)
+
+ known_true_rot = np.array(
+ [ [0, 0, 1, 3],
+ [0, -1, 0, -2],
+ [1, 0, 0, 1],
+ [0, 0, 0, 1] ])
+
+ try:
+ np.testing.assert_array_almost_equal(ht, known_true_rot)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+ def test_homog_transform_inverse(self):
+ '''Test a homogeneous transformation inverse'''
+
+ # Test by running a forward transformation on a set of coordinates, then reversing
+ # it via the inverse, and making sure the two coordinates match
+
+ phi = 10*d2r
+ theta = 20*d2r
+ psi = -30*d2r
+
+ x_t = -5
+ y_t = 10
+ z_t = 2.56
+
+ test_x = 0.23
+ test_y = 100
+ test_z = -62
+ orig_coords = np.array([test_x,test_y,test_z,1])
+
+ # Get homogeneous transformation
+
+ ht = transformations.homog_transform(phi,theta,psi,x_t,y_t,z_t)
+ # Forward transformation
+ transformed_coords = ht.dot(orig_coords)
+
+ # Inverse of homogeneous transform
+ inv_ht = transformations.ht_inverse(ht)
+
+ # Getting back original coordinates
+ test_coords = inv_ht.dot(transformed_coords)
+
+ try:
+ np.testing.assert_array_almost_equal(orig_coords, test_coords)
+ res = True
+ except AssertionError as err:
+ res = False
+ print (err)
+ self.assertTrue(res)
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py
new file mode 100644
index 0000000000000000000000000000000000000000..ff2092a4742495c653287b7c1ff5a7c686869e25
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py
@@ -0,0 +1,182 @@
+#!/usr/bin/env python
+
+""" Functions for computing 3D rotation, transformation, and homogenenous transformation matrices """
+
+import numpy as np
+from math import cos, sin
+
+
+def rotx(ang):
+ """Create a 3x3 numpy rotation matrix about the x axis
+
+ The three columns represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix.
+
+ Args:
+ ang: angle for rotation in radians
+
+ Returns:
+ The 3D rotation matrix about the x axis
+ """
+ rotxMatrix = np.array(
+ [ [1, 0, 0],
+ [0, cos(ang), -sin(ang)],
+ [0, sin(ang), cos(ang)] ])
+
+ return rotxMatrix
+
+
+def roty(ang):
+ """Create a 3x3 numpy rotation matrix about the y axis
+
+ The three columns represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix.
+
+ Args:
+ ang: angle for rotation in radians
+
+ Returns:
+ The 3D rotation matrix about the y axis
+ """
+ rotyMatrix = np.array(
+ [ [ cos(ang), 0, sin(ang)],
+ [ 0, 1, 0],
+ [-sin(ang), 0, cos(ang)] ])
+
+ return rotyMatrix
+
+
+def rotz(ang):
+ """Create a 3x3 numpy rotation matrix about the z axis
+
+ The three columns represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix.
+
+ Args:
+ ang: angle for rotation in radians
+
+ Returns:
+ The 3D rotation matrix about the z axis
+ """
+ rotzMatrix = np.array(
+ [ [cos(ang), -sin(ang), 0],
+ [sin(ang), cos(ang), 0],
+ [ 0, 0, 1] ])
+
+ return rotzMatrix
+
+def rotxyz(x_ang,y_ang,z_ang):
+ """Creates a 3x3 numpy rotation matrix from three rotations done in the order
+ of x, y, and z in the local coordinate frame as it rotates.
+
+ The three columns represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix.
+
+ Args:
+ x_ang: angle for rotation about the x axis in radians
+ y_ang: angle for rotation about the y axis in radians
+ z_ang: angle for rotation about the z axis in radians
+
+ Returns:
+ The 3D rotation matrix for a x, y, z rotation
+ """
+ # return rotx(x_ang) @ roty(y_ang) @ rotz(z_ang)
+ return np.matmul(np.matmul(rotx(x_ang), roty(y_ang)), rotz(z_ang))
+
+
+def homog_rotxyz(x_ang,y_ang,z_ang):
+ """Creates a 4x4 numpy homogeneous rotation matrix from three rotations
+ done in the order x, y, and z in the local coordinate frame as it rotates. This is
+ the same as the output of homog_trans except with no translation
+
+ The three columns and rows represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix.
+
+ Args:
+ x_ang: angle for rotation about the x axis in radians
+ y_ang: angle for rotation about the y axis in radians
+ z_ang: angle for rotation about the z axis in radians
+
+ Returns:
+ The homogenous transformation matrix for a x, y, z rotation and translation
+ """
+ return np.block( [ [rotxyz(x_ang,y_ang,z_ang), np.array([[0],[0],[0]])], [np.array([0,0,0,1])] ] )
+
+def homog_transxyz(x,y,z):
+ """Creates a 4x4 numpy linear transformation matrix
+
+ Args:
+ x: translation in x
+ y: translation in y
+ z: translation in z
+
+ Returns:
+ 4x4 numpy array for a linear translation on a 4x1 vector
+ """
+
+ return np.block([ [np.eye(3,3), np.array([[x],[y],[z]]) ], [np.array([0,0,0,1])] ] )
+
+def homog_transform(x_ang,y_ang,z_ang,x_t,y_t,z_t):
+ """Creates a 4x4 numpy rotation and transformation matrix from three rotations
+ done in the order x, y, and z in the local coordinate frame as it rotates, then
+ a transformation in x, y, and z in that rotate coordinate frame.
+
+ The three columns and rows represent the new basis vectors in the global coordinate
+ system of a coordinate system rotated by this matrix. The last column and three rows
+ represents the translation in the rotated coordinate frame
+
+ Args:
+ x_ang: angle for rotation about the x axis in radians
+ y_ang: angle for rotation about the y axis in radians
+ z_ang: angle for rotation about the z axis in radians
+ x_t: linear translation in x
+ y_t: linear translation in y
+ z_t: linear translation in z
+
+ Returns:
+ The homogenous transformation matrix for a x, y, z rotation and translation
+ """
+ # return homog_rotxyz(x_ang,y_ang,z_ang) @ homog_transxyz(x_t,y_t,z_t)
+ return np.matmul(homog_rotxyz(x_ang,y_ang,z_ang), homog_transxyz(x_t,y_t,z_t))
+
+def ht_inverse(ht):
+ '''Calculate the inverse of a homogeneous transformation matrix
+
+ The inverse of a homogeneous transformation matrix can be represented as a
+ a matrix product of the following:
+
+ ------------------- -------------------
+ | | 0 | | 1 0 0 -x_t |
+ ht_inv = | R^-1 | 0 | * | 0 1 0 -y_t |
+ |___________| 0 | | 0 0 1 -z_t |
+ | 0 0 0 | 1 | | 0 0 0 1 |
+ ------------------- -------------------
+
+ Where R^-1 is the ivnerse of the rotation matrix portion of the homogeneous
+ transform (the first three rows and columns). Note that the inverse
+ of a rotation matrix is equal to its transpose. And x_t, y_t, z_t are the
+ linear trasnformation portions of the original transform.
+
+ Args
+ ht: Input 4x4 nump matrix homogeneous transformation
+
+ Returns:
+ A 4x4 numpy matrix that is the inverse of the inputted transformation
+ '''
+ # Get the rotation matrix part of the homogeneous transform and take the transpose to get the inverse
+ temp_rot = ht[0:3,0:3].transpose()
+
+ # Get the linear transformation portion of the transform, and multiply elements by -1
+ temp_vec = -1*ht[0:3,3]
+
+ # Block the inverted rotation matrix back to a 4x4 homogeneous transform matrix
+ temp_rot_ht = np.block([ [temp_rot , np.zeros((3,1))],
+ [np.zeros((1,3)) , np.eye(1)] ])
+
+ # Create a linear translation homogeneous transformation matrix
+ temp_vec_ht = np.eye(4)
+ temp_vec_ht[0:3,3] = temp_vec
+
+ # Return the matrix product
+ # return temp_rot_ht @ temp_vec_ht
+ return np.matmul(temp_rot_ht, temp_vec_ht)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9f2b73ab80a57570b4330ad2b537897c3b2182a8
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/CMakeLists.txt
@@ -0,0 +1,198 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spot_micro_rviz)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES spotmicroai
+# CATKIN_DEPENDS roscpp rospy
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/spotmicroai.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/spotmicroai_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_spotmicroai.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_and_move_model_via_gui.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_and_move_model_via_gui.launch
new file mode 100644
index 0000000000000000000000000000000000000000..de36de8be6dc98828a70bac73a47760d314e8e69
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_and_move_model_via_gui.launch
@@ -0,0 +1,15 @@
+<launch>
+
+ <arg name="model" default="$(find spot_micro_rviz)/urdf/spot_micro.urdf.xacro"/>
+ <arg name="gui" default="true" />
+ <arg name="rvizconfig" default="$(find spot_micro_rviz)/rviz/spot_micro.rviz" />
+
+ <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+ <param name="use_gui" value="$(arg gui)"/>
+
+ <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+</launch>
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_model.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_model.launch
new file mode 100644
index 0000000000000000000000000000000000000000..3e9613bb3139bb68d0510668f6d213d837f8919e
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_model.launch
@@ -0,0 +1,11 @@
+<launch>
+
+ <arg name="model" default="$(find spot_micro_rviz)/urdf/spot_micro.urdf.xacro"/>
+ <arg name="rvizconfig" default="$(find spot_micro_rviz)/rviz/spot_micro.rviz" />
+
+ <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+
+</launch>
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/slam.launch b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/slam.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a6235162f18fda12f4fdefe7b4d4e2a79f14ac84
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/slam.launch
@@ -0,0 +1,13 @@
+<launch>
+
+ <arg name="rvizconfig" default="$(find spot_micro_rviz)/rviz/spot_micro_slam.rviz" />
+ <arg name="model" default="$(find spot_micro_rviz)/urdf/spot_micro.urdf.xacro"/>
+
+ <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
+
+
+ <node pkg="rviz" type="rviz" name="rviz"
+ args="-d $(arg rvizconfig)"/>
+
+</launch>
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/package.xml b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1708d24431b913153ace74b7e82f622d31f3dace
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/package.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>spot_micro_rviz</name>
+ <version>0.0.0</version>
+ <description>Spot Micro RVIZ Visualization Package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="michael.romanko@gmail.com">mike</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>rospy</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/mapping_demo.rviz b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/mapping_demo.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..603a32dbb7b264e657f8ed1061dd125bac677e17
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/mapping_demo.rviz
@@ -0,0 +1,137 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /Map1
+ - /Path1
+ - /Pose1
+ Splitter Ratio: 0.5
+ Tree Height: 540
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.03
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 0.7
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic: /map
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Name: Path
+ Topic: /trajectory
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.1
+ Class: rviz/Pose
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.3
+ Head Radius: 0.1
+ Name: Pose
+ Shaft Length: 1
+ Shaft Radius: 0.05
+ Shape: Axes
+ Topic: /slam_out_pose
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/XYOrbit
+ Distance: 44.388
+ Focal Point:
+ X: 9.50434
+ Y: -0.685607
+ Z: 0
+ Name: Current View
+ Near Clip Distance: 0.01
+ Pitch: 1.56976
+ Target Frame: <Fixed Frame>
+ Value: XYOrbit (rviz)
+ Yaw: -0.7846
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 846
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000013c000002abfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000002ab000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1200
+ X: 53
+ Y: 60
\ No newline at end of file
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro.rviz b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..b3258b3d40aaf7d4e6ff55d109ae5c1b383b3d21
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro.rviz
@@ -0,0 +1,355 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /TF1
+ Splitter Ratio: 0.5
+ Tree Height: 666
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lidar_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ front_left_foot_link:
+ Value: true
+ front_left_leg_link:
+ Value: true
+ front_left_leg_link_cover:
+ Value: true
+ front_left_shoulder_link:
+ Value: true
+ front_left_toe_link:
+ Value: true
+ front_link:
+ Value: true
+ front_right_foot_link:
+ Value: true
+ front_right_leg_link:
+ Value: true
+ front_right_leg_link_cover:
+ Value: true
+ front_right_shoulder_link:
+ Value: true
+ front_right_toe_link:
+ Value: true
+ lidar_link:
+ Value: true
+ odom:
+ Value: true
+ rear_left_foot_link:
+ Value: true
+ rear_left_leg_link:
+ Value: true
+ rear_left_leg_link_cover:
+ Value: true
+ rear_left_shoulder_link:
+ Value: true
+ rear_left_toe_link:
+ Value: true
+ rear_link:
+ Value: true
+ rear_right_foot_link:
+ Value: true
+ rear_right_leg_link:
+ Value: true
+ rear_right_leg_link_cover:
+ Value: true
+ rear_right_shoulder_link:
+ Value: true
+ rear_right_toe_link:
+ Value: true
+ Marker Scale: 0.5
+ Name: TF
+ Show Arrows: false
+ Show Axes: false
+ Show Names: false
+ Tree:
+ odom:
+ base_footprint:
+ base_link:
+ front_left_shoulder_link:
+ front_left_leg_link:
+ front_left_foot_link:
+ front_left_toe_link:
+ {}
+ front_left_leg_link_cover:
+ {}
+ front_link:
+ {}
+ front_right_shoulder_link:
+ front_right_leg_link:
+ front_right_foot_link:
+ front_right_toe_link:
+ {}
+ front_right_leg_link_cover:
+ {}
+ lidar_link:
+ {}
+ rear_left_shoulder_link:
+ rear_left_leg_link:
+ rear_left_foot_link:
+ rear_left_toe_link:
+ {}
+ rear_left_leg_link_cover:
+ {}
+ rear_link:
+ {}
+ rear_right_shoulder_link:
+ rear_right_leg_link:
+ rear_right_foot_link:
+ rear_right_toe_link:
+ {}
+ rear_right_leg_link_cover:
+ {}
+ Update Interval: 0
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base_footprint
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 0.993481159
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.619797409
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 3.8817699
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1000
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001ad0000033bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000340000033b000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001320000033bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000340000033b000000ca00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004afc0100000002fb0000000800540069006d00650100000000000007800000043900fffffffb0000000800540069006d00650100000000000004500000000000000000000005ca0000033b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 1736
+ Y: 317
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro_slam.rviz b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro_slam.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..f22f6f626c867469317c4227dc0f7c02e7b32570
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro_slam.rviz
@@ -0,0 +1,413 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /TF1
+ - /Map1
+ - /Path1
+ - /Pose1
+ Splitter Ratio: 0.5
+ Tree Height: 1283
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ lidar_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_left_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_foot_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_leg_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_leg_link_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ rear_right_toe_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base_footprint:
+ Value: true
+ base_link:
+ Value: true
+ front_left_foot_link:
+ Value: true
+ front_left_leg_link:
+ Value: true
+ front_left_leg_link_cover:
+ Value: true
+ front_left_shoulder_link:
+ Value: true
+ front_left_toe_link:
+ Value: true
+ front_link:
+ Value: true
+ front_right_foot_link:
+ Value: true
+ front_right_leg_link:
+ Value: true
+ front_right_leg_link_cover:
+ Value: true
+ front_right_shoulder_link:
+ Value: true
+ front_right_toe_link:
+ Value: true
+ laser:
+ Value: true
+ map:
+ Value: true
+ odom:
+ Value: true
+ rear_left_foot_link:
+ Value: true
+ rear_left_leg_link:
+ Value: true
+ rear_left_leg_link_cover:
+ Value: true
+ rear_left_shoulder_link:
+ Value: true
+ rear_left_toe_link:
+ Value: true
+ rear_link:
+ Value: true
+ rear_right_foot_link:
+ Value: true
+ rear_right_leg_link:
+ Value: true
+ rear_right_leg_link_cover:
+ Value: true
+ rear_right_shoulder_link:
+ Value: true
+ rear_right_toe_link:
+ Value: true
+ scanmatcher_frame:
+ Value: true
+ Marker Scale: 0.5
+ Name: TF
+ Show Arrows: false
+ Show Axes: false
+ Show Names: false
+ Tree:
+ map:
+ odom:
+ base_footprint:
+ base_link:
+ front_left_shoulder_link:
+ front_left_leg_link:
+ front_left_foot_link:
+ front_left_toe_link:
+ {}
+ front_left_leg_link_cover:
+ {}
+ front_link:
+ {}
+ front_right_shoulder_link:
+ front_right_leg_link:
+ front_right_foot_link:
+ front_right_toe_link:
+ {}
+ front_right_leg_link_cover:
+ {}
+ laser:
+ {}
+ rear_left_shoulder_link:
+ rear_left_leg_link:
+ rear_left_foot_link:
+ rear_left_toe_link:
+ {}
+ rear_left_leg_link_cover:
+ {}
+ rear_link:
+ {}
+ rear_right_shoulder_link:
+ rear_right_leg_link:
+ rear_right_foot_link:
+ rear_right_toe_link:
+ {}
+ rear_right_leg_link_cover:
+ {}
+ scanmatcher_frame:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 0.699999988
+ Class: rviz/Map
+ Color Scheme: map
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic: /map
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.300000012
+ Head Length: 0.200000003
+ Length: 0.300000012
+ Line Style: Lines
+ Line Width: 0.0299999993
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: Axes
+ Radius: 0.0199999996
+ Shaft Diameter: 0.100000001
+ Shaft Length: 0.100000001
+ Topic: /trajectory
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Axes Length: 0.5
+ Axes Radius: 0.0199999996
+ Class: rviz/Pose
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.300000012
+ Head Radius: 0.100000001
+ Name: Pose
+ Shaft Length: 1
+ Shaft Radius: 0.0500000007
+ Shape: Axes
+ Topic: /slam_out_pose
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 4.33504772
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.0500000007
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.689797103
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 3.92677736
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1617
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001ad000005a4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000034000005a4000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001320000033bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000340000033b000000ca00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004afc0100000002fb0000000800540069006d00650100000000000007800000043900fffffffb0000000800540069006d00650100000000000004500000000000000000000005ca000005a400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1920
+ X: 392
+ Y: 205
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/gen_urdf.sh b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/gen_urdf.sh
new file mode 100755
index 0000000000000000000000000000000000000000..651e2b685768e99d403132b649e5ebcb9abc06cd
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/gen_urdf.sh
@@ -0,0 +1,2 @@
+export LC_NUMERIC="en_US.UTF-8"
+xacro spot_micro.urdf.xacro > spot_micro.urdf
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..0eb23cbb6d9b84d68c22e42637525def8b32ffed
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf
@@ -0,0 +1,611 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- | This document was autogenerated by xacro from spot_micro.urdf.xacro | -->
+<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
+<!-- =================================================================================== -->
+<robot name="spot_micro_rviz">
+ <material name="yellow">
+ <color rgba="0.92 0.83 0.0 1"/>
+ </material>
+ <material name="black">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ <material name="grey">
+ <color rgba="0.6 0.6 0.6 1"/>
+ </material>
+ <!-- Params -->
+ <!-- Macros -->
+ <!-- Robot Body -->
+ <link name="base_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/mainbody.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="black"/>
+ <origin rpy="0 0 0" xyz="-0.042 -0.055 -0.010"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.14 0.11 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="2.80"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <!-- Lidar Backpack -->
+ <link name="lidar_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rplidar_main.STL" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="black"/>
+ <origin rpy="1.57075 0 0" xyz="-0.03425 0.03585 0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.0985 0.0577 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.50"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <joint name="base_lidar" type="fixed">
+ <parent link="base_link"/>
+ <child link="lidar_link"/>
+ <origin xyz="0 0 0.035"/>
+ </joint>
+ <link name="rear_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/backpart.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010"/>
+ <material name="yellow"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.04 0.11 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0.135 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.20"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <joint name="base_rear" type="fixed">
+ <parent link="base_link"/>
+ <child link="rear_link"/>
+ </joint>
+ <link name="front_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/frontpart.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010"/>
+ <material name="yellow"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.058 0.11 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="-0.145 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.20"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <joint name="base_front" type="fixed">
+ <parent link="base_link"/>
+ <child link="front_link"/>
+ </joint>
+ <!-- create Legs -->
+ <link name="front_left_shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.044 0.038 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.10"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <link name="front_left_leg_link_cover">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ <material name="yellow"/>
+ </visual>
+ </link>
+ <link name="front_left_leg_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ <material name="black"/>
+ <!-- <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <material name="yellow"/>-->
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <geometry>
+ <box size="0.028 0.036 0.1075"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.15"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="front_left_foot_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.026 0.020 0.13"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="front_left_toe_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/foot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/>
+ <material name="grey"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="0.02"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <contact_coefficients mu="1.1"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <joint name="front_left_shoulder" type="revolute">
+ <parent link="base_link"/>
+ <child link="front_left_shoulder_link"/>
+ <axis xyz="1 0 0"/>
+ <origin rpy="0 0 0" xyz="0.093 0.039 0"/>
+ <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="front_left_leg" type="revolute">
+ <parent link="front_left_shoulder_link"/>
+ <child link="front_left_leg_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0.055 0"/>
+ <limit effort="1000.0" lower="-2.666" upper="1.548" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <joint name="front_left_leg_cover_joint" type="fixed">
+ <parent link="front_left_leg_link"/>
+ <child link="front_left_leg_link_cover"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+ <joint name="front_left_foot" type="revolute">
+ <parent link="front_left_leg_link"/>
+ <child link="front_left_foot_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.1075"/>
+ <limit effort="1000.0" lower="-2.6" upper="0.1" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="front_left_toe" type="fixed">
+ <parent link="front_left_foot_link"/>
+ <child link="front_left_toe_link"/>
+ <origin xyz="0 0 -0.13"/>
+ </joint>
+ <link name="front_right_shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.044 0.038 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.10"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <link name="front_right_leg_link_cover">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ <material name="yellow"/>
+ </visual>
+ </link>
+ <link name="front_right_leg_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ <material name="black"/>
+ <!-- <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <material name="yellow"/>-->
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <geometry>
+ <box size="0.028 0.036 0.1075"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.15"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="front_right_foot_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.026 0.020 0.13"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="front_right_toe_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/foot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/>
+ <material name="grey"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="0.02"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <contact_coefficients mu="1.1"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <joint name="front_right_shoulder" type="revolute">
+ <parent link="base_link"/>
+ <child link="front_right_shoulder_link"/>
+ <axis xyz="1 0 0"/>
+ <origin rpy="0 0 0" xyz="0.093 -0.039 0"/>
+ <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="front_right_leg" type="revolute">
+ <parent link="front_right_shoulder_link"/>
+ <child link="front_right_leg_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 -0.055 0"/>
+ <limit effort="1000.0" lower="-2.666" upper="1.548" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <joint name="front_right_leg_cover_joint" type="fixed">
+ <parent link="front_right_leg_link"/>
+ <child link="front_right_leg_link_cover"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+ <joint name="front_right_foot" type="revolute">
+ <parent link="front_right_leg_link"/>
+ <child link="front_right_foot_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.1075"/>
+ <limit effort="1000.0" lower="-2.6" upper="0.1" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="front_right_toe" type="fixed">
+ <parent link="front_right_foot_link"/>
+ <child link="front_right_toe_link"/>
+ <origin xyz="0 0 -0.13"/>
+ </joint>
+ <link name="rear_left_shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.044 0.038 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.10"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <link name="rear_left_leg_link_cover">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ <material name="yellow"/>
+ </visual>
+ </link>
+ <link name="rear_left_leg_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ <material name="black"/>
+ <!-- <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <material name="yellow"/>-->
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <geometry>
+ <box size="0.028 0.036 0.1075"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.15"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="rear_left_foot_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.026 0.020 0.13"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="rear_left_toe_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/foot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/>
+ <material name="grey"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="0.02"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <contact_coefficients mu="1.1"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <joint name="rear_left_shoulder" type="revolute">
+ <parent link="base_link"/>
+ <child link="rear_left_shoulder_link"/>
+ <axis xyz="1 0 0"/>
+ <origin rpy="0 0 0" xyz="-0.093 0.039 0"/>
+ <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="rear_left_leg" type="revolute">
+ <parent link="rear_left_shoulder_link"/>
+ <child link="rear_left_leg_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0.055 0"/>
+ <limit effort="1000.0" lower="-2.666" upper="1.548" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <joint name="rear_left_leg_cover_joint" type="fixed">
+ <parent link="rear_left_leg_link"/>
+ <child link="rear_left_leg_link_cover"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+ <joint name="rear_left_foot" type="revolute">
+ <parent link="rear_left_leg_link"/>
+ <child link="rear_left_foot_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.1075"/>
+ <limit effort="1000.0" lower="-2.6" upper="0.1" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="rear_left_toe" type="fixed">
+ <parent link="rear_left_foot_link"/>
+ <child link="rear_left_toe_link"/>
+ <origin xyz="0 0 -0.13"/>
+ </joint>
+ <link name="rear_right_shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.044 0.038 0.07"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.10"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100"/>
+ </inertial>
+ </link>
+ <link name="rear_right_leg_link_cover">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ <material name="yellow"/>
+ </visual>
+ </link>
+ <link name="rear_right_leg_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ <material name="black"/>
+ <!-- <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <material name="yellow"/>-->
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <geometry>
+ <box size="0.028 0.036 0.1075"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.15"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="rear_right_foot_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1"/>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.026 0.020 0.13"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <link name="rear_right_toe_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/foot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/>
+ <material name="grey"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="0.02"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0.02"/>
+ <contact_coefficients mu="1.1"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000"/>
+ </inertial>
+ </link>
+ <joint name="rear_right_shoulder" type="revolute">
+ <parent link="base_link"/>
+ <child link="rear_right_shoulder_link"/>
+ <axis xyz="1 0 0"/>
+ <origin rpy="0 0 0" xyz="-0.093 -0.039 0"/>
+ <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="rear_right_leg" type="revolute">
+ <parent link="rear_right_shoulder_link"/>
+ <child link="rear_right_leg_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 -0.055 0"/>
+ <limit effort="1000.0" lower="-2.666" upper="1.548" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+ <joint name="rear_right_leg_cover_joint" type="fixed">
+ <parent link="rear_right_leg_link"/>
+ <child link="rear_right_leg_link_cover"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+ <joint name="rear_right_foot" type="revolute">
+ <parent link="rear_right_leg_link"/>
+ <child link="rear_right_foot_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0 -0.1075"/>
+ <limit effort="1000.0" lower="-2.6" upper="0.1" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ <joint name="rear_right_toe" type="fixed">
+ <parent link="rear_right_foot_link"/>
+ <child link="rear_right_toe_link"/>
+ <origin xyz="0 0 -0.13"/>
+ </joint>
+</robot>
+
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf.xacro b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..622b6f72f15043e6760b36c119706fbbd01a178c
--- /dev/null
+++ b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf.xacro
@@ -0,0 +1,341 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="spot_micro_rviz">
+
+ <material name="yellow">
+ <color rgba="0.92 0.83 0.0 1"/>
+ </material>
+ <material name="black">
+ <color rgba="0.1 0.1 0.1 1"/>
+ </material>
+ <material name="grey">
+ <color rgba="0.6 0.6 0.6 1"/>
+ </material>
+
+ <!-- Params -->
+
+ <xacro:property name="body_length" value="0.140" />
+ <xacro:property name="body_width" value="0.110" />
+ <xacro:property name="body_height" value="0.070" />
+
+ <xacro:property name="lidar_length" value="0.0985" />
+ <xacro:property name="lidar_width" value="0.0577" />
+ <xacro:property name="lidar_height" value="0.070" />
+
+
+ <xacro:property name="front_length" value="0.058" />
+ <xacro:property name="rear_length" value="0.040" />
+
+ <xacro:property name="shoulder_length" value="0.044" />
+ <xacro:property name="shoulder_width" value="0.038" />
+
+ <xacro:property name="leg_length" value="0.1075" />
+ <xacro:property name="foot_length" value="0.130" />
+
+ <xacro:property name="toe_radius" value="0.020" />
+ <!-- <xacro:property name="toe_radius" value="0.014" /> -->
+ <xacro:property name="toe_width" value="0.020" />
+ <xacro:property name="shift" value="0.055" />
+ <xacro:property name="shiftx" value="0.093" />
+ <xacro:property name="shifty" value="0.039" />
+
+ <!-- Macros -->
+
+ <xacro:macro name="gen_shoulder" params="name left">
+ <link name="${name}">
+ <visual>
+ <xacro:if value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.015 -0.01"/>
+ </xacro:if>
+ <xacro:unless value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rshoulder.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.135 0.095 -0.01"/>
+ </xacro:unless>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="${shoulder_length} ${shoulder_width} ${body_height}"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.10"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ </link>
+ </xacro:macro>
+
+ <xacro:macro name="gen_shoulder_joint" params="pos shiftx shifty">
+ <joint name="${pos}_shoulder" type="revolute">
+ <parent link="base_link"/>
+ <child link="${pos}_shoulder_link"/>
+ <axis xyz="1 0 0"/>
+ <origin rpy="0 0 0" xyz="${shiftx} ${shifty} 0"/>
+ <limit effort="1000.0" lower="-0.548" upper="0.548" velocity="0.7"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ </xacro:macro>
+
+ <xacro:macro name="gen_leg" params="name left">
+ <link name="${name}_cover">
+ <visual>
+ <xacro:if value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ </xacro:if>
+ <xacro:unless value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm_cover.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ </xacro:unless>
+ <material name="yellow"/>
+ </visual>
+ </link>
+ <link name="${name}">
+ <visual>
+ <xacro:if value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/larm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 -0.040 -0.025"/>
+ </xacro:if>
+ <xacro:unless value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rarm.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.139 3.14159" xyz="0.130 0.15 -0.025"/>
+ </xacro:unless>
+ <material name="black"/>
+<!-- <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <material name="yellow"/>-->
+ </visual>
+ <collision>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ <geometry>
+ <box size="0.028 0.036 ${leg_length}"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="0.15"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ </link>
+ </xacro:macro>
+
+ <xacro:macro name="gen_leg_joint" params="pos shift">
+ <joint name="${pos}_leg" type="revolute">
+ <parent link="${pos}_shoulder_link"/>
+ <child link="${pos}_leg_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 ${shift} 0"/>
+ <limit effort="1000.0" lower="-2.666" upper="1.548" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.0"/>
+ </joint>
+
+ <joint name="${pos}_leg_cover_joint" type="fixed">
+ <parent link="${pos}_leg_link"/>
+ <child link="${pos}_leg_link_cover"/>
+ <origin xyz="0 0 0"/>
+ </joint>
+ </xacro:macro>
+
+ <xacro:macro name="gen_foot" params="name left">
+ <link name="${name}">
+ <visual>
+ <xacro:if value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/lfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 -0.04 0.1"/>
+ </xacro:if>
+ <xacro:unless value="${left}">
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rfoot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.120 0.15 0.1"/>
+ </xacro:unless>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.026 0.020 ${foot_length}"/>
+ </geometry>
+ <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.050"/>
+ </collision>
+ <inertial>
+ <mass value="0.1"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ </link>
+ </xacro:macro>
+
+ <xacro:macro name="gen_foot_joint" params="pos">
+ <joint name="${pos}_foot" type="revolute">
+ <parent link="${pos}_leg_link"/>
+ <child link="${pos}_foot_link"/>
+ <axis xyz="0 1 0"/>
+ <origin rpy="0 0 0" xyz="0 0 -${leg_length}"/>
+ <limit effort="1000.0" lower="-2.6" upper="0.1" velocity="0.5"/>
+ <dynamics damping="0.0" friction="0.5"/>
+ </joint>
+ </xacro:macro>
+
+ <xacro:macro name="gen_toe" params="name">
+ <link name="${name}">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/foot.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 -0.40010 3.14159" xyz="0.00 0.01 0.015"/>
+ <material name="grey"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="${toe_radius}" />
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 ${toe_radius}"/>
+ <contact_coefficients mu="1.1" />
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ </link>
+ </xacro:macro>
+
+ <xacro:macro name="gen_toe_joint" params="pos">
+ <joint name="${pos}_toe" type="fixed">
+ <parent link="${pos}_foot_link"/>
+ <child link="${pos}_toe_link"/>
+ <origin xyz="0 0 -${foot_length}"/>
+ </joint>
+ </xacro:macro>
+
+ <xacro:macro name="gen_full_leg_joint" params="pos shiftx shifty shift left">
+ <xacro:gen_shoulder name="${pos}_shoulder_link" left="${left}"/>
+ <xacro:gen_leg name="${pos}_leg_link" left="${left}"/>
+ <xacro:gen_foot name="${pos}_foot_link" left="${left}"/>
+ <xacro:gen_toe name="${pos}_toe_link"/>
+
+ <xacro:gen_shoulder_joint pos="${pos}" shiftx="${shiftx}" shifty="${shifty}"/>
+ <xacro:gen_leg_joint pos="${pos}" shift="${shift}"/>
+ <xacro:gen_foot_joint pos="${pos}"/>
+ <xacro:gen_toe_joint pos="${pos}"/>
+ </xacro:macro>
+
+ <!-- Robot Body -->
+
+ <link name="base_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/mainbody.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="black"/>
+ <origin rpy="0 0 0" xyz="-0.042 -0.055 -0.010"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="${body_length} ${body_width} ${body_height}"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="2.80"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ </link>
+
+ <!-- Lidar Backpack -->
+
+ <link name="lidar_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/rplidar_main.STL" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <material name="black"/>
+ <origin rpy="1.57075 0 0" xyz="${-lidar_length/2+0.015} ${lidar_width/2+0.007} 0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="${lidar_length} ${lidar_width} ${lidar_height}"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.50"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ </link>
+ <joint name="base_lidar" type="fixed">
+ <parent link="base_link"/>
+ <child link="lidar_link"/>
+ <origin xyz="0 0 ${body_height/2}"/>
+ </joint>
+
+ <link name="rear_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/backpart.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.04 0.055 -0.010"/>
+ <material name="yellow"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="${rear_length} ${body_width} ${body_height}"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="0.135 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.20"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ </link>
+ <joint name="base_rear" type="fixed">
+ <parent link="base_link"/>
+ <child link="rear_link"/>
+ </joint>
+
+ <link name="front_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://spot_micro_rviz/urdf/stl/frontpart.stl" scale="0.001 0.001 0.001"/>
+ </geometry>
+ <origin rpy="0 0 3.14159" xyz="0.040 0.055 -0.010"/>
+ <material name="yellow"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="${front_length} ${body_width} ${body_height}"/>
+ </geometry>
+ <origin rpy="0 0 0" xyz="-0.145 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.20"/>
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ </link>
+ <joint name="base_front" type="fixed">
+ <parent link="base_link"/>
+ <child link="front_link"/>
+ </joint>
+
+ <!-- create Legs -->
+
+ <xacro:gen_full_leg_joint pos="front_left" shiftx="${shiftx}" shifty="${shifty}" shift="${shift}" left="true"/>
+ <xacro:gen_full_leg_joint pos="front_right" shiftx="${shiftx}" shifty="-${shifty}" shift="-${shift}" left="false"/>
+ <xacro:gen_full_leg_joint pos="rear_left" shiftx="-${shiftx}" shifty="${shifty}" shift="${shift}" left="true"/>
+ <xacro:gen_full_leg_joint pos="rear_right" shiftx="-${shiftx}" shifty="-${shifty}" shift="-${shift}" left="false"/>
+
+</robot>
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/backpart.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/backpart.stl
new file mode 100644
index 0000000000000000000000000000000000000000..375dad5a67bfcf951e9d7721b6476777efc6aef2
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/backpart.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/foot.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/foot.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ff9476710e325f69548fb01f5e915671ba82dee1
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/foot.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/frontpart.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/frontpart.stl
new file mode 100644
index 0000000000000000000000000000000000000000..536217feea4e42fe854766beba5c0c53f29091b3
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/frontpart.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..33e1ced4a0758369fd7d0e8836360ce43fdcaebb
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm_cover.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm_cover.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e9e79c0c784a85c7bd35dfc9684f85b6addb2f42
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm_cover.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lfoot.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lfoot.stl
new file mode 100644
index 0000000000000000000000000000000000000000..754105aeedd4f0b7086d91e47e4e4b142853ece1
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lfoot.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lshoulder.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lshoulder.stl
new file mode 100644
index 0000000000000000000000000000000000000000..36e7893121d4a516ca0efa3ff8261db1906fd5ac
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lshoulder.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/mainbody.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/mainbody.stl
new file mode 100644
index 0000000000000000000000000000000000000000..448fe6ee5c89a29b2fcf21ccc1281b736b0c1c7c
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/mainbody.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm.stl
new file mode 100644
index 0000000000000000000000000000000000000000..4fd1eb6bd801cb667eab66bb99e6d4a33dbac5f1
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm_cover.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm_cover.stl
new file mode 100644
index 0000000000000000000000000000000000000000..9b5a6e47d88e64f14dddfb13a0cec62c3d01be3f
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm_cover.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rfoot.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rfoot.stl
new file mode 100644
index 0000000000000000000000000000000000000000..8c4d3c5b903bd6c2480bc08e906705e29bb7a0c6
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rfoot.stl differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rplidar_main.STL b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rplidar_main.STL
new file mode 100644
index 0000000000000000000000000000000000000000..beced163aeefb2220429e655a18a78973e9f9db5
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rplidar_main.STL differ
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rshoulder.stl b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rshoulder.stl
new file mode 100644
index 0000000000000000000000000000000000000000..20d7f6679fbe53f81c0fb098b5c8f98c8b82d534
Binary files /dev/null and b/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rshoulder.stl differ