diff --git a/CMakeLists.txt b/CMakeLists.txt index 010a5b5..1ebce4a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -167,6 +167,10 @@ include_directories( catkin_install_python(PROGRAMS scripts/getModels.py scripts/setModels.py + scripts/models.py + scripts/cmd_vel2motor.py + scripts/odometry.py + scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/config/diffdrive.yaml b/config/diffdrive.yaml index b7458d3..9c395fb 100644 --- a/config/diffdrive.yaml +++ b/config/diffdrive.yaml @@ -1,8 +1,8 @@ type: "diff_drive_controller/DiffDriveController" publish_rate: 50 -left_wheel: ['left_front_wheel_joint', 'left_back_wheel_joint'] -right_wheel: ['right_front_wheel_joint', 'right_back_wheel_joint'] +left_wheel: ['left_front_motor', 'left_back_motor'] +right_wheel: ['right_front_motor', 'right_back_motor'] wheel_separation: 0.2 @@ -19,12 +19,12 @@ base_frame_id: base_link linear: x: has_velocity_limits : true - max_velocity : 1.5 # m/s + max_velocity : 100 # m/s has_acceleration_limits: true - max_acceleration : 1.5 # m/s^2 + max_acceleration : 100 # m/s^2 angular: z: has_velocity_limits : true - max_velocity : 6.28 # rad/s + max_velocity : 100 # rad/s has_acceleration_limits: true - max_acceleration : 6.28 # rad/s^2 + max_acceleration : 100 # rad/s^2 diff --git a/config/v5_motor.yaml b/config/v5_motor.yaml new file mode 100644 index 0000000..c251e54 --- /dev/null +++ b/config/v5_motor.yaml @@ -0,0 +1,21 @@ +# topics +publish_velocity: true +publish_encoder: true +publish_current: true +publish_motor_joint_state: true +update_rate: 100.0 +# motor model +motor_nominal_voltage: 14.0 # Volts +# moment of inertia calculated by getting 17.5 inch / pounds of torque from a vex forum post and converting it to kgm https://www.vexforum.com/t/v5-motor-torque/80258/2 +moment_of_inertia: 0.4535924 # kgm^2 +armature_damping_ratio: 0.03 # Nm/(rad/s) +electromotive_force_constant: 0.03 # Nm/A +# ohms law 14V max batter 11W max power draw V/(W/V) = Ohms https://www.vexrobotics.com/276-4840.html#additional +electric_resistance: 17.818182 # Ohm +electric_inductance: 0.001 # Henry +# transmission +gear_ratio: 36.0 +joint_damping: 0.005 +joint_friction: 0.01 +# shaft encoder +encoder_ppr: 900 #50*_gear_ratio diff --git a/launch/changeup.launch b/launch/changeup.launch index 5959c77..a7157a3 100644 --- a/launch/changeup.launch +++ b/launch/changeup.launch @@ -1,19 +1,4 @@ - - - - - - - - - - - - - - - - - + + diff --git a/launch/changeup_field_only.launch b/launch/changeup_field_only.launch new file mode 100644 index 0000000..e2c3711 --- /dev/null +++ b/launch/changeup_field_only.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/launch/robot.launch b/launch/robot.launch index 82c258d..38e2593 100644 --- a/launch/robot.launch +++ b/launch/robot.launch @@ -1,75 +1,76 @@ - - + + + + - - - - - - - - + + args="-x $(arg x_start) -y $(arg y_start) -z $(arg z_start) -unpause -urdf -model robot -param robot_description" + respawn="false" output="screen"/> - - - + + - - - - - - - - - ["joint_states_copy"] - - - - - - - - - - - + + + + ns="robot_joint_state_controller"/> + ns="robot_diff_drive_controller"/> - + - - - + + + + + + + + + + + + + + + + + + + + - - - + + + + + + + + - + - + + + + + diff --git a/models/change_up_cad b/models/change_up_cad index 229a141..d8ac5f1 160000 --- a/models/change_up_cad +++ b/models/change_up_cad @@ -1 +1 @@ -Subproject commit 229a141c481a66c4c954dc6e3dbf9beafe2baa18 +Subproject commit d8ac5f166c408f87d3937618b70d268ead2ec42a diff --git a/rviz/changeup.rviz b/rviz/changeup.rviz index fbafd01..d510f61 100644 --- a/rviz/changeup.rviz +++ b/rviz/changeup.rviz @@ -6,7 +6,6 @@ Panels: Expanded: - /Global Options1 - /RobotModel1 - - /RobotModel1/Status1 Splitter Ratio: 0.5 Tree Height: 746 - Class: rviz/Selection @@ -28,7 +27,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -59,6 +58,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -216,6 +216,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + left_back_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false left_back_wheel: Alpha: 1 Show Axes: false @@ -341,6 +345,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + left_front_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false left_front_wheel: Alpha: 1 Show Axes: false @@ -466,6 +474,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + right_back_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false right_back_wheel: Alpha: 1 Show Axes: false @@ -591,6 +603,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + right_front_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false right_front_wheel: Alpha: 1 Show Axes: false @@ -598,7 +614,7 @@ Visualization Manager: Value: true Name: RobotModel Robot Description: robot_description - TF Prefix: GT + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -617,9 +633,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 @@ -632,6 +646,38 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /ground_truth_to_tf/pose + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -660,12 +706,13 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 7.970325946807861 + Distance: 6.271038055419922 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -675,10 +722,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.17479757964611053 + Pitch: 0.38479679822921753 Target Frame: - Value: Orbit (rviz) - Yaw: 3.6868176460266113 + Yaw: 5.736841678619385 Saved: - Class: rviz/Orbit Distance: 0.9657304286956787 @@ -687,6 +733,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -698,7 +745,6 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.28000056743621826 Target Frame: - Value: Orbit (rviz) Yaw: 4.598184108734131 Window Geometry: Displays: @@ -706,7 +752,7 @@ Window Geometry: Height: 1132 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000202000003b1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002b1000000ef0000000000000000fb0000000c00430061006d00650072006100000002200000018000000000000000000000000100000168000003b1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005bfc0100000002fb0000000800540069006d00650100000000000007800000026c00fffffffb0000000800540069006d006501000000000000045000000000000000000000040a000003b100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000202000003b1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002b1000000ef0000000000000000fb0000000c00430061006d00650072006100000002200000018000000000000000000000000100000168000003b1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005bfc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000040a000003b100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -716,5 +762,5 @@ Window Geometry: Views: collapsed: false Width: 1920 - X: 1920 - Y: 32 + X: 640 + Y: 27 diff --git a/rviz/robot.rviz b/rviz/robot.rviz index 33c76e0..9440a7a 100644 --- a/rviz/robot.rviz +++ b/rviz/robot.rviz @@ -28,7 +28,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -59,6 +59,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -216,6 +217,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + left_back_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false left_back_wheel: Alpha: 1 Show Axes: false @@ -341,6 +346,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + left_front_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false left_front_wheel: Alpha: 1 Show Axes: false @@ -466,6 +475,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + right_back_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false right_back_wheel: Alpha: 1 Show Axes: false @@ -591,6 +604,10 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + right_front_shaft: + Alpha: 1 + Show Axes: false + Show Trail: false right_front_wheel: Alpha: 1 Show Axes: false @@ -598,7 +615,7 @@ Visualization Manager: Value: true Name: RobotModel Robot Description: robot_description - TF Prefix: GT + TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true @@ -617,9 +634,7 @@ Visualization Manager: Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 @@ -632,41 +647,11 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /front_camera/front_camera/depth/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: world + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: @@ -696,6 +681,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -707,7 +693,6 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.3347975015640259 Target Frame: - Value: Orbit (rviz) Yaw: 5.938671588897705 Saved: - Class: rviz/Orbit @@ -717,6 +702,7 @@ Visualization Manager: Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: X: 0 Y: 0 @@ -728,7 +714,6 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.28000056743621826 Target Frame: - Value: Orbit (rviz) Yaw: 4.598184108734131 Window Geometry: Displays: @@ -736,7 +721,7 @@ Window Geometry: Height: 1132 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000031e000003b1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002b1000000ef0000000000000000fb0000000c00430061006d00650072006100000002200000018000000000000000000000000100000168000003b1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005bfc0100000002fb0000000800540069006d00650100000000000007800000026c00fffffffb0000000800540069006d00650100000000000004500000000000000000000002ee000003b100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000197000003b1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b1000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002b1000000ef0000000000000000fb0000000c00430061006d00650072006100000002200000018000000000000000000000000100000100000003b1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005830000005bfc0100000002fb0000000800540069006d0065010000000000000583000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e0000003b100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -745,6 +730,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 1920 - Y: 32 + Width: 1411 + X: 1149 + Y: 27 diff --git a/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb b/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb new file mode 100644 index 0000000..f094ae2 --- /dev/null +++ b/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb @@ -0,0 +1,143 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Quintic polynomials planner" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Quintic polynomials for one dimensional robot motion\n", + "\n", + "We assume a one-dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n", + "\n", + "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(1)\n", + "\n", + "$a_0, a_1. a_2, a_3, a_4, a_5$ are parameters of the quintic polynomial.\n", + "\n", + "It is assumed that terminal states (start and end) are known as boundary conditions.\n", + "\n", + "Start position, velocity, and acceleration are $x_s, v_s, a_s$ respectively.\n", + "\n", + "End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n", + "\n", + "So, when time is 0.\n", + "\n", + "$x(0) = a_0 = x_s$ -- (2)\n", + "\n", + "Then, differentiating the equation (1) with t, \n", + "\n", + "$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4$ -- (3)\n", + "\n", + "So, when time is 0,\n", + "\n", + "$x'(0) = a_1 = v_s$ -- (4)\n", + "\n", + "Then, differentiating the equation (3) with t again, \n", + "\n", + "$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n", + "\n", + "So, when time is 0,\n", + "\n", + "$x''(0) = 2a_2 = a_s$ -- (6)\n", + "\n", + "so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n", + "\n", + "$a_3, a_4, a_5$ are still unknown in eq(1).\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We assume that the end time for a maneuver is $T$, we can get these equations from eq (1), (3), (5):\n", + "\n", + "$x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e$ -- (7)\n", + "\n", + "$x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e$ -- (8)\n", + "\n", + "$x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e$ -- (9)\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "From eq (7), (8), (9), we can calculate $a_3, a_4, a_5$ to solve the linear equations.\n", + "\n", + "$Ax=b$\n", + "\n", + "$\\begin{bmatrix} T^3 & T^4 & T^5 \\\\ 3T^2 & 4T^3 & 5T^4 \\\\ 6T & 12T^2 & 20T^3 \\end{bmatrix}\n", + "\\begin{bmatrix} a_3\\\\ a_4\\\\ a_5\\end{bmatrix}=\\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\\\ v_e-v_s-a_sT\\\\ a_e-a_s\\end{bmatrix}$\n", + "\n", + "We can get all unknown parameters now" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Quintic polynomials for two dimensional robot motion (x-y)\n", + "\n", + "If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.\n", + "\n", + "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(10)\n", + "\n", + "$y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5$ --(11)\n", + "\n", + "It is assumed that terminal states (start and end) are known as boundary conditions.\n", + "\n", + "Start position, orientation, velocity, and acceleration are $x_s, y_s, \\theta_s, v_s, a_s$ respectively.\n", + "\n", + "End position, orientation, velocity, and acceleration are $x_e, y_e. \\theta_e, v_e, a_e$ respectively.\n", + "\n", + "Each velocity and acceleration boundary condition can be calculated with each orientation.\n", + "\n", + "$v_{xs}=v_scos(\\theta_s), v_{ys}=v_ssin(\\theta_s)$\n", + "\n", + "$v_{xe}=v_ecos(\\theta_e), v_{ye}=v_esin(\\theta_e)$\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.7.5" + }, + "pycharm": { + "stem_cell": { + "cell_type": "raw", + "metadata": { + "collapsed": false + }, + "source": [] + } + } + }, + "nbformat": 4, + "nbformat_minor": 1 +} diff --git a/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.py new file mode 100644 index 0000000..6ccc67a --- /dev/null +++ b/scripts/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -0,0 +1,309 @@ +""" + +Quintic Polynomials Planner + +author: Atsushi Sakai (@Atsushi_twi) + +Ref: + +- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/) + +""" + +import math + +import matplotlib.pyplot as plt +import numpy as np +import rospy +from std_msgs.msg import Float64, Header +from geometry_msgs.msg import Twist, PoseStamped, PoseArray, Pose +from nav_msgs.msg import Odometry, Path +import tf +import geometry_msgs + +# parameter +MAX_T = 100.0 # maximum time to the goal [s] +MIN_T = 0.5 # minimum time to the goal[s] + +show_animation = False + +class QuinticPolynomial: + def __init__(self, xs, vxs, axs, xe, vxe, axe, time): + # calc coefficient of quintic polynomial + # See jupyter notebook document for derivation of this equation. + self.a0 = xs + self.a1 = vxs + self.a2 = axs / 2.0 + + A = np.array([[time ** 3, time ** 4, time ** 5], + [3 * time ** 2, 4 * time ** 3, 5 * time ** 4], + [6 * time, 12 * time ** 2, 20 * time ** 3]]) + b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2, + vxe - self.a1 - 2 * self.a2 * time, + axe - 2 * self.a2]) + x = np.linalg.solve(A, b) + + self.a3 = x[0] + self.a4 = x[1] + self.a5 = x[2] + + def calc_point(self, t): + xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ + self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 + + return xt + + def calc_first_derivative(self, t): + xt = self.a1 + 2 * self.a2 * t + \ + 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 + + return xt + + def calc_second_derivative(self, t): + xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 + + return xt + + def calc_third_derivative(self, t): + xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 + + return xt + + +def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt): + """ + quintic polynomial planner + + input + s_x: start x position [m] + s_y: start y position [m] + s_yaw: start yaw angle [rad] + sa: start accel [m/ss] + gx: goal x position [m] + gy: goal y position [m] + gyaw: goal yaw angle [rad] + ga: goal accel [m/ss] + max_accel: maximum accel [m/ss] + max_jerk: maximum jerk [m/sss] + dt: time tick [s] + + return + time: time result + rx: x position result list + ry: y position result list + ryaw: yaw angle result list + rv: velocity result list + ra: accel result list + """ + + vxs = sv * math.cos(syaw) + vys = sv * math.sin(syaw) + vxg = gv * math.cos(gyaw) + vyg = gv * math.sin(gyaw) + + axs = sa * math.cos(syaw) + ays = sa * math.sin(syaw) + axg = ga * math.cos(gyaw) + ayg = ga * math.sin(gyaw) + + time, rx, ry, ryaw, yawv, rv, ra, rj = [], [], [], [], [], [], [], [] + + for T in np.arange(MIN_T, MAX_T, MIN_T): + xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T) + yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T) + + time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], [] + + for t in np.arange(0.0, T + dt, dt): + time.append(t) + rx.append(xqp.calc_point(t)) + ry.append(yqp.calc_point(t)) + + vx = xqp.calc_first_derivative(t) + vy = yqp.calc_first_derivative(t) + v = np.hypot(vx, vy) + yaw = math.atan2(vy, vx) + rv.append(v) + ryaw.append(yaw) + + ax = xqp.calc_second_derivative(t) + ay = yqp.calc_second_derivative(t) + a = np.hypot(ax, ay) + if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0: + a *= -1 + ra.append(a) + + jx = xqp.calc_third_derivative(t) + jy = yqp.calc_third_derivative(t) + j = np.hypot(jx, jy) + if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0: + j *= -1 + rj.append(j) + + if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk: + print("find path!!") + break + + if show_animation: # pragma: no cover + for i, _ in enumerate(time): + plt.cla() + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.grid(True) + plt.axis("equal") + plot_arrow(sx, sy, syaw) + plot_arrow(gx, gy, gyaw) + plot_arrow(rx[i], ry[i], ryaw[i]) + plt.title("Time[s]:" + str(time[i])[0:4] + + " v[m/s]:" + str(rv[i])[0:4] + + " a[m/ss]:" + str(ra[i])[0:4] + + " jerk[m/sss]:" + str(rj[i])[0:4], + ) + plt.pause(0.001) + + return time, rx, ry, ryaw, rv, ra, rj + + +def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover + """ + Plot arrow + """ + + if not isinstance(x, float): + for (ix, iy, iyaw) in zip(x, y, yaw): + plot_arrow(ix, iy, iyaw) + else: + plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), + fc=fc, ec=ec, head_width=width, head_length=width) + plt.plot(x, y) + + +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Mar 24 21:48:47 2021 + +@author: tabor +""" + + +def callback(odom): + global curOdom + curOdom = odom + + +def calculate(goal): + global curGoal + curGoal = goal + + curQuat = ( + curOdom.pose.pose.orientation.x,curOdom.pose.pose.orientation.y, + curOdom.pose.pose.orientation.z,curOdom.pose.pose.orientation.w) + curEul = tf.transformations.euler_from_quaternion(curQuat) + + goalQuat = ( + curGoal.pose.orientation.x,curGoal.pose.orientation.y, + curGoal.pose.orientation.z,curGoal.pose.orientation.w) + goalEul = tf.transformations.euler_from_quaternion(goalQuat) + + sx = curOdom.pose.pose.position.x # start x position [m] + print(sx) + sy = curOdom.pose.pose.position.y # start y position [m] + syaw = curEul[2] # start yaw angle [rad] + sv = 0.01 # start speed [m/s] + sa = 0.01 # start accel [m/ss] + gx = curGoal.pose.position.x # goal x position [m] + gy = curGoal.pose.position.y # goal y position [m] + gyaw = goalEul[2] # goal yaw angle [rad] + gv = 0.01 # goal speed [m/s] + ga = 0.01 # goal accel [m/ss] + max_accel = 1.0 # max accel [m/ss] + max_jerk = 0.5 # max jerk [m/sss] + dt = 0.1 # time tick [s] + + global time, x, y, yaw, yawv, v, a, j + time, x, y, yaw, v, a, j = quintic_polynomials_planner( + sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt) + + yawv = [] + cmd, cmd_vels = Twist(), [] + path, pose = Path(), PoseStamped() + + for t in time: + index = int(t/dt) + if yawv == []: + yawv.append((yaw[index]-syaw)) + else: + yawv.append((yaw[index]-yaw[index-1])) + + cmd.linear.x = v[index] + cmd.angular.z = yawv[index] + cmd_vels.append(cmd) + pose.header.stamp = rospy.Time.now() + pose.header.frame_id = 'world' + pose.pose.position.x = x[index] + pose.pose.position.y = y[index] + quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw[index]) + pose.pose.orientation.x = quaternion[0] + pose.pose.orientation.y = quaternion[1] + pose.pose.orientation.z = quaternion[2] + pose.pose.orientation.w = quaternion[3] + path.poses.append(pose) + + + for t in time: + index = int(t/dt) + cmd_vel.publish(cmd_vels[index]) + + cmd.linear.x = gv + cmd.linear.y = gv + cmd.angular.z = gv + + # if show_animation: # pragma: no cover + # plt.plot(x, y, "-r") + # + # plt.subplots() + # plt.plot(time, [np.rad2deg(i) for i in yaw], "-r") + # plt.xlabel("Time[s]") + # plt.ylabel("Yaw[deg]") + # plt.grid(True) + # + # plt.subplots() + # plt.plot(time, v, "-r") + # plt.xlabel("Time[s]") + # plt.ylabel("Speed[m/s]") + # plt.grid(True) + # + # plt.subplots() + # plt.plot(time, a, "-r") + # plt.xlabel("Time[s]") + # plt.ylabel("accel[m/ss]") + # plt.grid(True) + # + # plt.subplots() + # plt.plot(time, j, "-r") + # plt.xlabel("Time[s]") + # plt.ylabel("jerk[m/sss]") + # plt.grid(True) + # + # plt.show() + + +def main(): + + print("hi") + rospy.init_node('quintic_polynomials_planner', anonymous=True) + + global cmd_vel, path_pub + cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=10) + path_pub = rospy.Publisher('/robot/path', Path, queue_size=50) + + rospy.Subscriber("/ground_truth/state", Odometry, callback) + rospy.Subscriber("/move_base_simple/goal", PoseStamped, calculate) + + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/scripts/cmd_vel2motor.py b/scripts/cmd_vel2motor.py new file mode 100755 index 0000000..0f628b9 --- /dev/null +++ b/scripts/cmd_vel2motor.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Mar 24 21:48:47 2021 + +@author: tabor +""" + +# thanks to https://www.asu.edu/courses/kin335/documents/Linear-Angular%20Relationships.pdf + +import rospy +from std_msgs.msg import Float64, Bool +from geometry_msgs.msg import Twist + +wheel_radius = 0.041 +max_angular_velocity = 3.14 +max_voltage = 14 + + +def callback(twist): + # converts linear velocity input to angular velocity output by dividing by the wheel's radius + left = (twist.linear.x - twist.angular.z) + right = (twist.linear.x + twist.angular.z) + + global pid_left_set, pid_right_set, mode + left_msg = Float64(); + left_msg.data = left + right_msg = Float64(); + right_msg.data = right + + pid_left_set.publish(left_msg) + pid_right_set.publish(right_msg) + +def left_callback(vel): + global back_left_command, front_left_command + set = Float64(); + set.data = vel.data + back_left_command.publish(set) + front_left_command.publish(set) + +def right_callback(vel): + global back_right_command, front_right_command + set = Float64(); + set.data = vel.data + back_right_command.publish(set) + front_right_command.publish(set) + +def back_left_callback(vel): + global back_left_velocity + back_left_velocity = vel.data + +def front_left_callback(vel): + global front_left_velocity + front_left_velocity = vel.data + +def back_right_callback(vel): + global back_right_velocity + back_right_velocity = vel.data + + +def front_right_callback(vel): + global front_right_velocity + front_right_velocity = vel.data + + +def listener(): + rospy.init_node('cmd_vel2motor', anonymous=True) + + global \ + pid_left_set, \ + pid_right_set, \ + pid_left_state, \ + pid_right_state, \ + back_left_command, \ + front_left_command, \ + back_right_command, \ + front_right_command, \ + back_left_velocity, \ + front_left_velocity, \ + back_right_velocity, \ + front_right_velocity, \ + mode + + pid_left_set = rospy.Publisher('/pid_motor_left/setpoint', Float64, queue_size=10) + pid_right_set = rospy.Publisher('/pid_motor_right/setpoint', Float64, queue_size=10) + pid_left_state = rospy.Publisher('/pid_motor_left/state', Float64, queue_size=10) + pid_right_state = rospy.Publisher('/pid_motor_right/state', Float64, queue_size=10) + back_left_command = rospy.Publisher('/motor_back_left/command', Float64, queue_size=10) + front_left_command = rospy.Publisher('/motor_front_left/command', Float64, queue_size=10) + back_right_command = rospy.Publisher('/motor_back_right/command', Float64, queue_size=10) + front_right_command = rospy.Publisher('/motor_front_right/command', Float64, queue_size=10) + pid_enable = rospy.Publisher('/pid_enable', Bool, queue_size=10) + + rospy.Subscriber("/cmd_vel", Twist, callback) + rospy.Subscriber("/pid_motor_left/control_effort", Float64, left_callback) + rospy.Subscriber("/pid_motor_right/control_effort", Float64, right_callback) + rospy.Subscriber("/motor_back_left/velocity", Float64, front_left_callback) + rospy.Subscriber("/motor_front_left/velocity", Float64, front_right_callback) + rospy.Subscriber("/motor_back_right/velocity", Float64, back_left_callback) + rospy.Subscriber("/motor_front_right/velocity", Float64, back_right_callback) + + back_left_velocity = 0 + front_left_velocity = 0 + back_right_velocity = 0 + front_right_velocity = 0 + + rate = rospy.Rate(10) # in hz + while not rospy.is_shutdown(): + rate.sleep() + # enable = Bool() + # enable.data = True + # pid_enable.publish(enable) + left_velocity = Float64() + left_velocity.data = (back_left_velocity * front_left_velocity / 2) + pid_left_state.publish(left_velocity) + rate.sleep() + right_velocity = Float64() + right_velocity.data = (back_right_velocity * front_right_velocity / 2) + pid_right_state.publish(right_velocity) + +if __name__ == '__main__': + listener() diff --git a/scripts/models.py b/scripts/models.py new file mode 100755 index 0000000..f9ad882 --- /dev/null +++ b/scripts/models.py @@ -0,0 +1,254 @@ +#!/usr/bin/env python +import rospy + +import tf +from tf.transformations import euler_from_quaternion, quaternion_from_euler +from geometry_msgs.msg import TransformStamped, Twist, Pose +from gazebo_msgs.srv import GetModelState, GetWorldProperties, GetModelProperties, GetModelState, SetModelState +from gazebo_msgs.msg import ModelState +from std_msgs.msg import String, Bool, Float64, Int16 + + +def field_callback(state): + global game_state + game_state = state + +def robot_callback(state): + global robot_state + robot_state = state + +def set_state(state): + try: + rospy.wait_for_service('/gazebo/set_model_state') + set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) + resp = set_state(state) + + except rospy.ServiceException as e: + print("Service call failed: %s" % e) + +def main(): + # init ros + rospy.init_node('models_node', anonymous=True) + + # set global vars, publishers, and subs + global field_state_pub, field_state_set, set_model_state_pub, game_state, robot_state + field_state_pub = rospy.Publisher('/game/field/state', Int16, queue_size=10) + field_state_set = rospy.Publisher('/game/field/set_state', Int16, queue_size=10) + field_state_pub = rospy.Publisher('/game/robot/state', Int16, queue_size=10) + field_state_set = rospy.Publisher('/game/robot/set_state', Int16, queue_size=10) + set_model_state_pub = rospy.Publisher('/game/set_model_state', ModelState, queue_size=10) + rospy.Subscriber("/game/field/set_state", Int16, field_callback) + rospy.Subscriber("/game/robot/set_state", Int16, robot_callback) + rospy.Subscriber("/game/set_model_state", ModelState, set_state) + + # set the default states + game_state = Int16() + game_state.data = 3 + game_state_last = Int16() + game_state_last.data = 0 + field_state_set.publish(game_state) + + robot_state = Int16() + robot_state.data = 2 + robot_state_last = Int16() + robot_state_last.data = 0 + field_state_set.publish(robot_state) + + # call init methods + init_game_state_models() + init_robot_state_models() + + while not rospy.is_shutdown(): + # print(str(game_state.data) + str(game_state_last.data)) + if game_state != game_state_last: + if len(game_state_models) -1 >= game_state.data and game_state.data >= 0: + print("new field state set: " + str(game_state.data)) + + game_state_last = game_state + for m in game_state_models[game_state.data]: + set_state(m) + else: + print("invalid field state number") + + if robot_state != robot_state_last: + if len(robot_state_models) -1 >= robot_state.data and robot_state.data >= 0: + print("new robot state set: " + str(robot_state.data)) + + robot_state_last = robot_state + for m in robot_state_models[robot_state.data]: + set_state(m) + else: + print("invalid robot state number") + + # # wait for gazebo + # rospy.wait_for_service('/gazebo/get_world_properties') + # + # world = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties) + # model_names = world().model_names + # + # br = tf.TransformBroadcaster() + # + # for model_name in model_names: + # + # model = rospy.ServiceProxy('/gazebo/get_model_properties', GetModelProperties) + # body_names = model(model_name).body_names + # + # for body_name in body_names: + # find_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) + # + # state = find_state(model_name, body_name) + # br.sendTransform((state.twist.linear.x, state.twist.linear.y, state.twist.linear.z), + # (state.pose.orientation.x, state.pose.orientation.y, state.pose.orientation.z, + # state.pose.orientation.w), + # rospy.Time.now(), + # body_name, + # model_name) + + +def init_robot_state_models(): + # set robot_state_models to global + global robot_state_models + + # set the poses for all of the objects for a game + robot_state_models, default_models, skill_top_models, skill_bottom_models, comp_red_top_models, comp_red_bottom_models, comp_blue_top_models, comp_blue_bottom_models = [], [], [], [], [], [], [], [] + rp = 1.6; rn = -rp; p2 = .6096; n2 = -p2; p3 = .9144; n3 = -p3; p4 = 1.2192; n4 = -p4; op = 2; on = -op; r1 = .11; + + # robot_state 0 means that nothing is currently being set and is free to move + robot_state_models.append([]) + + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = p3; model.pose.position.y = p3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + robot_state_models.append(default_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rn; model.pose.position.y = p3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skill_top_models.append(model) + robot_state_models.append(skill_top_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rn; model.pose.position.y = n3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skill_bottom_models.append(model) + robot_state_models.append(skill_bottom_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rn; model.pose.position.y = p3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_red_top_models.append(model) + robot_state_models.append(comp_red_top_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rn; model.pose.position.y = n3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_red_bottom_models.append(model) + robot_state_models.append(comp_red_bottom_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rp; model.pose.position.y = p3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_blue_top_models.append(model) + robot_state_models.append(comp_blue_top_models) + model = ModelState(); model.model_name = 'robot'; model.reference_frame = 'world'; model.pose.position.x = rp; model.pose.position.y = n3; model.pose.position.z = r1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_blue_bottom_models.append(model) + robot_state_models.append(comp_blue_bottom_models) + + +def init_game_state_models(): + # set game_state_models to global + global game_state_models + + # set the poses for all of the objects for a game + game_state_models, default_models, comp_models, skills_models = [], [], [], [] + t0= 0; t1 = .08001; t2 = .34003; t3 = .50005; tp = 1.6275; tn = -tp; gp = 1.4393; gn = -gp; mp = .2487; mn = -mp; op = 2; on = -op; + p2 = .6096; n2 = -p2; p3 = .9144; n3 = -p3; p4 = 1.2192; n4 = -p4; fp = 1.7488; fn = -fp; + + # game_state 0 means that nothing is currently being set and is free to move + game_state_models.append([]) + + # game_state 1 means default setup elements not in field: all the positions for all the objects for a field game state going by color, top to bottom, left to right, low to high + model = ModelState(); model.model_name = 'blue1'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue2'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue3'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue4'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue5'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue6'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue7'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue8'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue9'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue10'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue11'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue12'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue13'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue14'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue15'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'blue16'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red1'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red2'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red3'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red4'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red5'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red6'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red7'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red8'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red9'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red10'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red11'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red12'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red13'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red14'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red15'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + model = ModelState(); model.model_name = 'red16'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; default_models.append(model) + game_state_models.append(default_models) + + # game_state 2 means competition setup: all the positions for all the objects for a field game state going by color, top to bottom, left to right, low to high + model = ModelState(); model.model_name = 'blue1'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tp; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue2'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tp; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue3'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue4'; model.reference_frame = 'world'; model.pose.position.x = gp; model.pose.position.y = gp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue5'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = t0; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue6'; model.reference_frame = 'world'; model.pose.position.x = mn; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue7'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue8'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = mn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue9'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = n3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue10'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tn; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue11'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue12'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tn; model.pose.position.z = t3; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue13'; model.reference_frame = 'world'; model.pose.position.x = gp; model.pose.position.y = gn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue14'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue15'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = p3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'blue16'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = n3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red1'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red2'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red3'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tp; model.pose.position.z = t3; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red4'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tp; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red5'; model.reference_frame = 'world'; model.pose.position.x = gn; model.pose.position.y = gp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red6'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = p3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red7'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = mp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red8'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red9'; model.reference_frame = 'world'; model.pose.position.x = mp; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red10'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = t0; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red11'; model.reference_frame = 'world'; model.pose.position.x = gn; model.pose.position.y = gn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red12'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red13'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tn; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red14'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tn; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red15'; model.reference_frame = 'world'; model.pose.position.x = on; model.pose.position.y = p3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + model = ModelState(); model.model_name = 'red16'; model.reference_frame = 'world'; model.pose.position.x = on; model.pose.position.y = n3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; comp_models.append(model) + game_state_models.append(comp_models) + + # game_state 3 means skills setup: all the positions for all the objects for a field game state going by color, top to bottom, left to right, low to high + model = ModelState(); model.model_name = 'blue1'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue2'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tp; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue3'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue4'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue5'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tp; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue6'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue7'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue8'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = t0; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue9'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = t0; model.pose.position.z = t3; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue10'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue11'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue12'; model.reference_frame = 'world'; model.pose.position.x = tn; model.pose.position.y = tn; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue13'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue14'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue15'; model.reference_frame = 'world'; model.pose.position.x = tp; model.pose.position.y = tn; model.pose.position.z = t2; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'blue16'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red1'; model.reference_frame = 'world'; model.pose.position.x = n3; model.pose.position.y = fp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red2'; model.reference_frame = 'world'; model.pose.position.x = p3; model.pose.position.y = fp; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red3'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = p4; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red4'; model.reference_frame = 'world'; model.pose.position.x = n4; model.pose.position.y = p3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red5'; model.reference_frame = 'world'; model.pose.position.x = p4; model.pose.position.y = p3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red6'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = p2; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red7'; model.reference_frame = 'world'; model.pose.position.x = n2; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red8'; model.reference_frame = 'world'; model.pose.position.x = p2; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red9'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = n2; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red10'; model.reference_frame = 'world'; model.pose.position.x = n4; model.pose.position.y = n3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red11'; model.reference_frame = 'world'; model.pose.position.x = p4; model.pose.position.y = n3; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red12'; model.reference_frame = 'world'; model.pose.position.x = t0; model.pose.position.y = n4; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red13'; model.reference_frame = 'world'; model.pose.position.x = n3; model.pose.position.y = fn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red14'; model.reference_frame = 'world'; model.pose.position.x = p3; model.pose.position.y = fn; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red15'; model.reference_frame = 'world'; model.pose.position.x = on; model.pose.position.y = t0; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + model = ModelState(); model.model_name = 'red16'; model.reference_frame = 'world'; model.pose.position.x = op; model.pose.position.y = op; model.pose.position.z = t1; model.pose.orientation.x = 0; model.pose.orientation.y = 0; model.pose.orientation.z = 0; model.pose.orientation.w = 1; skills_models.append(model) + game_state_models.append(skills_models) + + +if __name__ == '__main__': + main() diff --git a/scripts/odometry.py b/scripts/odometry.py new file mode 100755 index 0000000..c42a01e --- /dev/null +++ b/scripts/odometry.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Wed Mar 24 21:48:47 2021 + +@author: tabor +""" + +import rospy +from std_msgs.msg import Int32 +import numpy as np +import matplotlib.pyplot as plt + + +def left_back_callback(enc): + + global left_back, left_front, right_back, right_front + left_back = enc.data +def left_front_callback(enc): + + global left_back, left_front, right_back, right_front + left_front = enc.data +def right_back_callback(enc): + + global left_back, left_front, right_back, right_front + right_back = enc.data +def right_front_callback(enc): + + global left_back, left_front, right_back, right_front + right_front = enc.data + +class odomv1: + def __init__(self,chassis_width,ticks_per_rotation,wheel_diameter): + self.w = chassis_width + self.last_left_back = 0 + self.last_left_front = 0 + self.last_right_back = 0 + self.last_right_front =0 + + self.tpr = ticks_per_rotation + self.dia = wheel_diameter + self.x = 0 + self.y = 0 + self.theta = 0 + def update(self,left_back, left_front, right_back, right_front): + delta_left = ((left_back - self.last_left_back) + (left_front - self.last_left_front))/2 + delta_right = ((right_back - self.last_right_back) + (right_front - self.last_right_front))/2 + + leftDist = delta_left*np.pi*self.dia/self.tpr + rightDist = delta_right*np.pi*self.dia/self.tpr + + straightDist = (leftDist + rightDist) / 2 + outsideDist = (leftDist - rightDist) / 2 + + chassis_circumference = (self.w * np.pi) + deltaAngle = 2*np.pi * outsideDist/chassis_circumference + + self.x += np.cos(self.theta) *straightDist + self.y += np.sin(self.theta) *straightDist + self.theta += deltaAngle + + + self.last_left_back = left_back + self.last_left_front = left_front + self.last_right_back = right_back + self.last_right_front =right_front + + + return (self.x,self.y,self.theta) + + + + +def listener(): + rospy.init_node('odom', anonymous=True) + + + global left_back, left_front, right_back, right_front + left_back_sub = rospy.Subscriber('/left_back_motor/encoder', Int32, left_back_callback) + left_front_sub = rospy.Subscriber('/left_front_motor/encoder', Int32, left_front_callback) + right_back_sub = rospy.Subscriber('/right_back_motor/encoder', Int32, right_back_callback) + right_front_sub = rospy.Subscriber('/right_front_motor/encoder', Int32, right_front_callback) + left_back = left_front = right_back = right_front = 0 + rate = rospy.Rate(100) # 10hz + v1 = odomv1(0.2/1.018, 900, 0.1126) + + v1poses = [] + i = 0 + fig = plt.figure() + while not rospy.is_shutdown(): + i +=1 + v1poses.append(v1.update(left_back, left_front, right_back, right_front )) + rate.sleep() + if i % 200 == 0: + npv1poses = np.array(v1poses) + print(npv1poses[-1,:]) + plt.plot(npv1poses[:,0],npv1poses[:,1]) + plt.show() + +if __name__ == '__main__': + + listener() \ No newline at end of file diff --git a/urdf/chassis.urdf.xacro b/urdf/chassis.urdf.xacro index 5ea3559..9ee1903 100644 --- a/urdf/chassis.urdf.xacro +++ b/urdf/chassis.urdf.xacro @@ -1,5 +1,6 @@ + @@ -94,10 +95,8 @@ - - - - + + Gazebo/Grey @@ -120,39 +119,31 @@ - + + + + - - - + + + + - + - + + + + + - - - - Gazebo/Green - - - - transmission_interface/SimpleTransmission - - 1 - - - VelocityJointInterface - - - + + @@ -164,6 +155,7 @@ + @@ -179,25 +171,22 @@ - + - + - - - / - - true - 50.0 - chassis_link - ground_truth/state - 0.01 - world - 0 0 0 - 0 0 0 + true + 50.0 + chassis_link + ground_truth/state + 0.01 + world + 0 0 0 + 0 0 0 - + + \ No newline at end of file diff --git a/worlds/games/change_up.world b/worlds/games/change_up.world index 7862725..938c8b9 100644 --- a/worlds/games/change_up.world +++ b/worlds/games/change_up.world @@ -2,199 +2,50 @@ - - model://sun - - - - model://ground_plane - - - 1 - model://change_up_cad/ChangeUpField - 0 0 0 0 0 0 - - - - - 0 - model://change_up_cad/ChangeUpBallRed - 0 .2487 .08001 0 0 0 - red1 - - - 0 - model://change_up_cad/ChangeUpBallRed - .2487 0 .08001 0 0 0 - red2 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 0 -.2487 .08001 0 0 0 - blue1 - - - 0 - model://change_up_cad/ChangeUpBallBlue - -.2487 0 .08001 0 0 0 - blue2 - - - 0 - model://change_up_cad/ChangeUpBallRed - 0 .81375 .08001 0 0 0 - red3 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 0 -.81375 .08001 0 0 0 - blue3 - + model://sun - - - 0 - model://change_up_cad/ChangeUpBallRed - 0 1.6275 .40005 0 0 0 - red4 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 0 1.6275 .24003 0 0 0 - blue4 - - - 0 - model://change_up_cad/ChangeUpBallRed - 0 1.6275 .08001 0 0 0 - red5 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 0 -1.6275 .40005 0 0 0 - blue5 - - - 0 - model://change_up_cad/ChangeUpBallRed - 0 -1.6275 .24003 0 0 0 - red6 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 0 -1.6275 .08001 0 0 0 - blue6 - + + model://ground_plane - - - 0 - model://change_up_cad/ChangeUpBallBlue - -1.6275 -1.6275 .24003 0 0 0 - blue7 - - - 0 - model://change_up_cad/ChangeUpBallRed - -1.6275 -1.6275 .08001 0 0 0 - red14 - - - 0 - model://change_up_cad/ChangeUpBallBlue - -1.6275 0 .24003 0 0 0 - blue14 - - - 0 - model://change_up_cad/ChangeUpBallRed - -1.6275 0 .08001 0 0 0 - red7 - - - 0 - model://change_up_cad/ChangeUpBallBlue - -1.6275 1.6275 .24003 0 0 0 - blue8 - - - 0 - model://change_up_cad/ChangeUpBallRed - -1.6275 1.6275 .08001 0 0 0 - red8 - + + 1model://change_up_cad/ChangeUpField 0 0 0 0 0 0 - - - 0 - model://change_up_cad/ChangeUpBallRed - -1.4393 -1.4393 .08001 0 0 0 - red9 - - - 0 - model://change_up_cad/ChangeUpBallRed - -1.4393 1.4393 .08001 0 0 0 - red10 - + + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue1 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue2 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue3 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue4 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue5 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue6 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue7 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue8 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue9 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue10 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue11 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue12 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue13 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue14 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue15 + 0 model://change_up_cad/ChangeUpBallBlue 0 0 0 0 0 0 blue16 - - - 0 - model://change_up_cad/ChangeUpBallRed - 1.6275 -1.6275 .24003 0 0 0 - red11 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 1.6275 -1.6275 .08001 0 0 0 - blue9 - - - 0 - model://change_up_cad/ChangeUpBallRed - 1.6275 0 .24003 0 0 0 - red12 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 1.6275 0 .08001 0 0 0 - blue10 - - - 0 - model://change_up_cad/ChangeUpBallRed - 1.6275 1.6275 .24003 0 0 0 - red13 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 1.6275 1.6275 .08001 0 0 0 - blue11 - + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red1 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red2 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red3 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red4 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red5 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red6 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red7 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red8 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red9 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red10 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red11 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red12 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red13 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red14 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red15 + 0 model://change_up_cad/ChangeUpBallRed 0 0 0 0 0 0 red16 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 1.4393 -1.4393 .08001 0 0 0 - blue12 - - - 0 - model://change_up_cad/ChangeUpBallBlue - 1.4393 1.4393 .08001 0 0 0 - blue13 - - +