From 79c215389d5a0f2a4803e451608c81cb20d0c9b9 Mon Sep 17 00:00:00 2001
From: manx52 <jonathanspraggett@gmail.com>
Date: Mon, 2 Dec 2024 17:33:33 -0500
Subject: [PATCH] misc

---
 .idea/soccerbot.iml                           |  4 +
 .idea/workspace.xml                           | 77 ++++++++++---------
 .../pybullet_usage/pybullet_load_model.py     |  6 +-
 .../src/soccer_trajectories/pybullet_setup.py | 19 ++++-
 .../trajectory_manager_sim.py                 |  4 +-
 .../test/test_trajectory.py                   | 24 +++---
 .../trajectories/bez1/getupfront.csv          |  2 -
 .../trajectories/bez1/getupside.csv           |  4 +-
 .../trajectories/bez1_sim/getupback.csv       | 10 +--
 .../trajectories/bez1_sim/getupback_real.csv  | 10 +--
 .../trajectories/bez1_sim/getupfront.csv      | 10 +--
 .../trajectories/bez1_sim/getupside.csv       | 10 +--
 .../trajectories/bez1_sim/goalieleft.csv      | 10 +--
 .../trajectories/bez1_sim/goalieright.csv     | 10 +--
 .../trajectories/bez1_sim/rightkick.csv       |  6 +-
 .../trajectories/bez2_sim/fix_angle_test.csv  |  8 +-
 .../trajectories/bez2_sim/getupback.csv       |  8 +-
 .../trajectories/bez2_sim/getupback_real.csv  |  8 +-
 .../trajectories/bez2_sim/getupfront.csv      |  8 +-
 .../trajectories/bez2_sim/getupside.csv       |  8 +-
 .../trajectories/bez2_sim/goalieleft.csv      |  8 +-
 .../trajectories/bez2_sim/goalieright.csv     |  8 +-
 .../trajectories/bez2_sim/rightkick.csv       |  8 +-
 23 files changed, 142 insertions(+), 128 deletions(-)

diff --git a/.idea/soccerbot.iml b/.idea/soccerbot.iml
index a22db543c..5796eb0e1 100644
--- a/.idea/soccerbot.iml
+++ b/.idea/soccerbot.iml
@@ -7,6 +7,10 @@
       <sourceFolder url="file://$MODULE_DIR$/soccer_strategy/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/soccer_control/soccer_trajectories/src" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/soccer_common/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/soccer_perception/soccer_localization/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/soccer_perception/soccer_object_detection/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/soccer_perception/soccer_object_localization/src" isTestSource="false" />
+      <sourceFolder url="file://$MODULE_DIR$/soccer_webots/src" isTestSource="false" />
     </content>
     <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
index 2f939f9e7..cb46f465d 100644
--- a/.idea/workspace.xml
+++ b/.idea/workspace.xml
@@ -7,16 +7,12 @@
     <list default="true" id="1718606a-6ea1-4edd-a178-2f51136d492a" name="Changes" comment="">
       <change beforePath="$PROJECT_DIR$/.idea/soccerbot.iml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/soccerbot.iml" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/config/assembly/assembly_sim_pybullet.yaml" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/config/assembly/assembly_sim_pybullet.yaml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test/test_placo.py" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test/test_placo.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/test/test_trajectory.py" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/test/test_trajectory.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupback.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupback.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick_2.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick_2.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick_old.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/rightkick_old.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/sidekick.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1/sidekick.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv" afterDir="false" />
@@ -24,7 +20,6 @@
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv" afterDir="false" />
@@ -33,7 +28,6 @@
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/soccer_description/assembly_description/urdf/robot.urdf" beforeDir="false" afterPath="$PROJECT_DIR$/soccer_description/assembly_description/urdf/robot.urdf" afterDir="false" />
     </list>
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -73,40 +67,45 @@
   "keyToString": {
     "Python tests.pytest for test_firmware_interface.test_firmware_interface.executor": "Run",
     "Python tests.pytest for test_firmware_interface.test_firmware_interface_single_motor_range.executor": "Run",
+    "Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection.executor": "Run",
+    "Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection_vid.executor": "Run",
+    "Python tests.pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid.executor": "Run",
     "Python tests.pytest for test_placo.TestPlaco.test_bez1.executor": "Run",
     "Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range.executor": "Run",
     "Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range_single.executor": "Run",
     "Python tests.pytest for test_pybullet.TestPybullet.test_imu.executor": "Debug",
+    "Python tests.pytest for test_trajectory.TestTrajectory.test_trajectory.executor": "Run",
     "Python tests.pytest for test_trajectory.test_trajectory_sim.executor": "Run",
     "Python tests.pytest for test_walk_ros.TestPybullet.test_walk_ros_local.executor": "Run",
     "Python.dgfs.executor": "Run",
     "RunOnceActivity.ShowReadmeOnStart": "true",
     "git-widget-placeholder": "master",
-    "last_opened_file_path": "/home/manx52/catkin_ws/devel/lib/python3/dist-packages",
+    "last_opened_file_path": "/home/manx52/catkin_ws/src/soccerbot/soccer_control/soccer_trajectories/trajectories/bez2",
     "node.js.detected.package.eslint": "true",
     "node.js.detected.package.tslint": "true",
     "node.js.selected.package.eslint": "(autodetect)",
     "node.js.selected.package.tslint": "(autodetect)",
     "nodejs_package_manager_path": "npm",
-    "settings.editor.selected.configurable": "editor.preferences.fonts.default",
+    "settings.editor.selected.configurable": "preferences.fileTypes",
     "vue.rearranger.settings.migration": "true"
   }
 }]]></component>
   <component name="RecentsManager">
     <key name="CopyFile.RECENT_KEYS">
+      <recent name="$PROJECT_DIR$/soccer_control/soccer_trajectories/trajectories/bez2" />
       <recent name="$PROJECT_DIR$/soccer_description/assembly_description/urdf" />
       <recent name="$PROJECT_DIR$/soccer_control/soccer_pycontrol/config" />
       <recent name="$PROJECT_DIR$/tools/setup" />
     </key>
   </component>
-  <component name="RunManager" selected="Python tests.pytest for test_trajectory.test_trajectory_sim">
-    <configuration name="pytest for test_placo.TestPlaco.test_bez1" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
+  <component name="RunManager" selected="Python tests.pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid">
+    <configuration name="pytest for test_object_detection.TestObjectDetection.test_object_detection" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
       <module name="soccerbot" />
       <option name="ENV_FILES" value="" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_perception/soccer_object_detection/test" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
@@ -114,17 +113,17 @@
       <option name="_new_keywords" value="&quot;&quot;" />
       <option name="_new_parameters" value="&quot;&quot;" />
       <option name="_new_additionalArguments" value="&quot;-s&quot;" />
-      <option name="_new_target" value="&quot;test_placo.TestPlaco.test_bez1&quot;" />
+      <option name="_new_target" value="&quot;test_object_detection.TestObjectDetection.test_object_detection&quot;" />
       <option name="_new_targetType" value="&quot;PYTHON&quot;" />
       <method v="2" />
     </configuration>
-    <configuration name="pytest for test_pybullet.TestPybullet.test_bez_motor_range" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
+    <configuration name="pytest for test_object_detection.TestObjectDetection.test_object_detection_vid" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
       <module name="soccerbot" />
       <option name="ENV_FILES" value="" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_perception/soccer_object_detection/test" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
@@ -132,17 +131,17 @@
       <option name="_new_keywords" value="&quot;&quot;" />
       <option name="_new_parameters" value="&quot;&quot;" />
       <option name="_new_additionalArguments" value="&quot;&quot;" />
-      <option name="_new_target" value="&quot;test_pybullet.TestPybullet.test_bez_motor_range&quot;" />
+      <option name="_new_target" value="&quot;test_object_detection.TestObjectDetection.test_object_detection_vid&quot;" />
       <option name="_new_targetType" value="&quot;PYTHON&quot;" />
       <method v="2" />
     </configuration>
-    <configuration name="pytest for test_pybullet.TestPybullet.test_bez_motor_range_single" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
+    <configuration name="pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
       <module name="soccerbot" />
       <option name="ENV_FILES" value="" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_perception/soccer_object_localization/test" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
@@ -150,17 +149,17 @@
       <option name="_new_keywords" value="&quot;&quot;" />
       <option name="_new_parameters" value="&quot;&quot;" />
       <option name="_new_additionalArguments" value="&quot;&quot;" />
-      <option name="_new_target" value="&quot;test_pybullet.TestPybullet.test_bez_motor_range_single&quot;" />
+      <option name="_new_target" value="&quot;test_object_localization.TestObjectLocalization.test_fieldline_detection_vid&quot;" />
       <option name="_new_targetType" value="&quot;PYTHON&quot;" />
       <method v="2" />
     </configuration>
-    <configuration name="pytest for test_pybullet.TestPybullet.test_imu" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
+    <configuration name="pytest for test_trajectory.TestTrajectory.test_trajectory" type="tests" factoryName="py.test" temporary="true" nameIsGenerated="true">
       <module name="soccerbot" />
       <option name="ENV_FILES" value="" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/soccer_control/soccer_trajectories/test" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
@@ -168,7 +167,7 @@
       <option name="_new_keywords" value="&quot;&quot;" />
       <option name="_new_parameters" value="&quot;&quot;" />
       <option name="_new_additionalArguments" value="&quot;&quot;" />
-      <option name="_new_target" value="&quot;test_pybullet.TestPybullet.test_imu&quot;" />
+      <option name="_new_target" value="&quot;test_trajectory.TestTrajectory.test_trajectory&quot;" />
       <option name="_new_targetType" value="&quot;PYTHON&quot;" />
       <method v="2" />
     </configuration>
@@ -191,19 +190,19 @@
       <method v="2" />
     </configuration>
     <list>
+      <item itemvalue="Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection_vid" />
+      <item itemvalue="Python tests.pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid" />
+      <item itemvalue="Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection" />
+      <item itemvalue="Python tests.pytest for test_trajectory.TestTrajectory.test_trajectory" />
       <item itemvalue="Python tests.pytest for test_trajectory.test_trajectory_sim" />
-      <item itemvalue="Python tests.pytest for test_placo.TestPlaco.test_bez1" />
-      <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range" />
-      <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range_single" />
-      <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_imu" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python tests.pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid" />
+        <item itemvalue="Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection_vid" />
+        <item itemvalue="Python tests.pytest for test_object_detection.TestObjectDetection.test_object_detection" />
+        <item itemvalue="Python tests.pytest for test_trajectory.TestTrajectory.test_trajectory" />
         <item itemvalue="Python tests.pytest for test_trajectory.test_trajectory_sim" />
-        <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_imu" />
-        <item itemvalue="Python tests.pytest for test_placo.TestPlaco.test_bez1" />
-        <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range" />
-        <item itemvalue="Python tests.pytest for test_pybullet.TestPybullet.test_bez_motor_range_single" />
       </list>
     </recent_temporary>
   </component>
@@ -241,7 +240,7 @@
       <workItem from="1732403792633" duration="3377000" />
       <workItem from="1732997444701" duration="14964000" />
       <workItem from="1733019770661" duration="1047000" />
-      <workItem from="1733035011319" duration="9185000" />
+      <workItem from="1733035011319" duration="16229000" />
     </task>
     <servers />
   </component>
@@ -264,14 +263,18 @@
     </breakpoint-manager>
   </component>
   <component name="com.intellij.coverage.CoverageDataManagerImpl">
-    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_pybullet_TestPybullet_test_bez_motor_range_single.coverage" NAME="pytest for test_pybullet.TestPybullet.test_bez_motor_range_single Coverage Results" MODIFIED="1733013152155" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
-    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_placo_TestPlaco_test_bez1.coverage" NAME="pytest for test_placo.TestPlaco.test_bez1 Coverage Results" MODIFIED="1733163147251" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_trajectory_TestTrajectory_test_trajectory.coverage" NAME="pytest for test_trajectory.TestTrajectory.test_trajectory Coverage Results" MODIFIED="1733176109562" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_trajectories/test" />
     <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_firmware_interface_test_firmware_interface.coverage" NAME="pytest for test_firmware_interface.test_firmware_interface Coverage Results" MODIFIED="1732408857959" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_hardware/soccer_firmware_interface/test" />
-    <SUITE FILE_PATH="coverage/soccerbot$dgfs.coverage" NAME="dgfs Coverage Results" MODIFIED="1730141845344" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/tools/setup" />
     <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_pybullet_TestPybullet_test_imu.coverage" NAME="pytest for test_pybullet.TestPybullet.test_imu Coverage Results" MODIFIED="1733163515329" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
-    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_trajectory_test_trajectory_sim.coverage" NAME="pytest for test_trajectory.test_trajectory_sim Coverage Results" MODIFIED="1733164833446" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_trajectories/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_object_localization_TestObjectLocalization_test_fieldline_detection_vid.coverage" NAME="pytest for test_object_localization.TestObjectLocalization.test_fieldline_detection_vid Coverage Results" MODIFIED="1733177322454" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_perception/soccer_object_localization/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_trajectory_test_trajectory_sim.coverage" NAME="pytest for test_trajectory.test_trajectory_sim Coverage Results" MODIFIED="1733175993565" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_trajectories/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_object_detection_TestObjectDetection_test_object_detection_vid.coverage" NAME="pytest for test_object_detection.TestObjectDetection.test_object_detection_vid Coverage Results" MODIFIED="1733177240556" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_perception/soccer_object_detection/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_pybullet_TestPybullet_test_bez_motor_range_single.coverage" NAME="pytest for test_pybullet.TestPybullet.test_bez_motor_range_single Coverage Results" MODIFIED="1733013152155" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_placo_TestPlaco_test_bez1.coverage" NAME="pytest for test_placo.TestPlaco.test_bez1 Coverage Results" MODIFIED="1733174628799" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$dgfs.coverage" NAME="dgfs Coverage Results" MODIFIED="1730141845344" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/tools/setup" />
     <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_pybullet_TestPybullet_test_bez_motor_range.coverage" NAME="pytest for test_pybullet.TestPybullet.test_bez_motor_range Coverage Results" MODIFIED="1733015016961" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
     <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_firmware_interface_test_firmware_interface_single_motor_range.coverage" NAME="pytest for test_firmware_interface.test_firmware_interface_single_motor_range Coverage Results" MODIFIED="1732408905335" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_hardware/soccer_firmware_interface/test" />
+    <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_object_detection_TestObjectDetection_test_object_detection.coverage" NAME="pytest for test_object_detection.TestObjectDetection.test_object_detection Coverage Results" MODIFIED="1733177170818" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_perception/soccer_object_detection/test" />
     <SUITE FILE_PATH="coverage/soccerbot$pytest_for_test_walk_ros_TestPybullet_test_walk_ros_local.coverage" NAME="pytest for test_walk_ros.TestPybullet.test_walk_ros_local Coverage Results" MODIFIED="1733002179035" SOURCE_PROVIDER="com.intellij.coverage.DefaultCoverageFileProvider" RUNNER="coverage.py" COVERAGE_BY_TEST_ENABLED="false" COVERAGE_TRACING_ENABLED="false" WORKING_DIRECTORY="$PROJECT_DIR$/soccer_control/soccer_pycontrol/test" />
   </component>
 </project>
\ No newline at end of file
diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py
index 3af72757e..ac486bb78 100644
--- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py
+++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py
@@ -43,11 +43,7 @@ def set_pose(self, pose: Transformation = Transformation()) -> None:
         self.pose.position = (self.pose.position[0], self.pose.position[1], self.walking_torso_height)
 
         [y, _, _] = pose.orientation_euler
-        r = 0
 
-        if self.robot_model == "bez2":  # TODO need to fix this
-            r = -0.64
-
-        self.pose.orientation_euler = [y, 0, r]
+        self.pose.orientation_euler = [y, 0, 0]
         if pb.isConnected():
             pb.resetBasePositionAndOrientation(self.body, self.pose.position, self.pose.quaternion)
diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py
index 2af30161e..914f26ade 100644
--- a/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py
+++ b/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py
@@ -1,5 +1,6 @@
 import time
 from os.path import expanduser
+from typing import List
 
 import pybullet as pb
 from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld
@@ -38,12 +39,24 @@ def __init__(
             baseOrientation=pose.quaternion,
         )
 
-        self.motor_names = [pb.getJointInfo(self.body, i)[1].decode("utf-8") for i in range(20)]
+        self.motor_names = self.find_motor_names()
+
+        self.numb_of_motors = len(self.motor_names)
         self.max_forces = []
 
-        for i in range(0, 20):
+        for i in range(0, self.numb_of_motors):
             self.max_forces.append(pb.getJointInfo(self.body, i)[10] or 6)  # TODO why is this acting so weird
 
+    def find_motor_names(self) -> List[str]:
+
+        names = []
+        for i in range(pb.getNumJoints(self.body)):
+            joint_info = pb.getJointInfo(self.body, i)
+            if joint_info[2] == pb.JOINT_REVOLUTE:
+                names.append(joint_info[1].decode("utf-8"))
+
+        return names
+
     def wait(self, steps) -> None:
         """
         Make the robot wait for a few steps
@@ -62,7 +75,7 @@ def motor_control(self, target: list) -> None:
         pb.setJointMotorControlArray(
             bodyIndex=self.body,
             controlMode=pb.POSITION_CONTROL,
-            jointIndices=list(range(0, 20, 1)),
+            jointIndices=list(range(0, self.numb_of_motors, 1)),
             targetPositions=target,
             forces=self.max_forces,
         )
diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py
index eaca356d0..d42547e16 100755
--- a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py
+++ b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py
@@ -27,10 +27,10 @@ def __init__(
         self.sim = PybulletSetup(robot_model=robot_model, real_time=real_time, pose=pose, camera_yaw=camera_yaw)
 
     def read_joint_state(self) -> JointState:
-        return JointState(name=self.sim.motor_names, position=[0.0] * 20)
+        return JointState(name=self.sim.motor_names, position=[0.0] * self.sim.numb_of_motors)
 
     def send_joint_msg(self, timestamp: float) -> None:
-        states = [0.0] * 20
+        states = [0.0] * self.sim.numb_of_motors
         joints, angles = self.trajectory.create_joint_states(timestamp)
         for i, joint in enumerate(joints):
             states[self.sim.motor_names.index(joint)] = angles[i]
diff --git a/soccer_control/soccer_trajectories/test/test_trajectory.py b/soccer_control/soccer_trajectories/test/test_trajectory.py
index 342d15291..29633df5b 100644
--- a/soccer_control/soccer_trajectories/test/test_trajectory.py
+++ b/soccer_control/soccer_trajectories/test/test_trajectory.py
@@ -31,19 +31,19 @@ def test_trajectory(self):
             "right_hip_roll",
             "left_hip_roll",
             "right_hip_pitch",
-            "left_leg_motor_2",
+            "left_hip_pitch",
             "right_knee",
-            "left_leg_motor_3",
+            "left_knee",
             "right_ankle_pitch",
-            "left_leg_motor_4",
+            "left_ankle_pitch",
             "right_ankle_roll",
-            "left_leg_motor_5",
-            "right_arm_motor_0",
-            "left_arm_motor_0",
-            "right_arm_motor_1",
-            "left_arm_motor_1",
-            "head_motor_0",
-            "head_motor_1",
+            "left_ankle_roll",
+            "right_shoulder_pitch",
+            "left_shoulder_pitch",
+            "right_shoulder_roll",
+            "left_shoulder_roll",
+            "head_yaw",
+            "head_pitch",
         ]
         joint_state.position = [0.0] * 18
 
@@ -52,7 +52,7 @@ def test_trajectory(self):
         self.assertEqual(angles, [0.0, 0.0, 0.0, 0.0, 0.564, 0.564, -1.176, -1.176, 0.613, 0.613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
 
 
-@pytest.mark.parametrize("trajectory_name", ["rightkick_2"])  # , "getupback_full", "rightkick", "getupfront"])
+@pytest.mark.parametrize("trajectory_name", ["getupfront"])  # , "getupback_full", "rightkick", "getupfront"])
 @pytest.mark.parametrize("robot_model", ["assembly"])
 @pytest.mark.parametrize("real_time", [True])
 def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool):
@@ -64,7 +64,7 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool)
     if trajectory_name == "getupfront":
         pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 0])
         camera = 0
-    elif trajectory_name == "getupback_full" or trajectory_name == "getupback_roll" or trajectory_name == "getupback":
+    elif "getupback" in trajectory_name:
         pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0])
         camera = 0
     else:
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv b/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv
index 1b5a51bc6..04126728a 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1/getupfront.csv
@@ -15,5 +15,3 @@ right_shoulder_pitch,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0
 left_shoulder_pitch,0,-1.5,0,1.5,1.5,1.5,1.5,1,0.7,0.2,-0.5,0,0.3,0.8,0.7,0.6,0.5,0.6,0.75
 right_elbow,0,2.5,3,1.5,2,2,2,2.3,2.5,2.8,2.6,2.6,1.3,0,0,0,0,0,0
 left_elbow,0,2.5,3,1.5,2,2,2,2.3,2.5,2.8,2.6,2.6,1.3,0,0,0,0,0,0
-right_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-left_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv b/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv
index bbb12e473..c764f9a5d 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1/getupside.csv
@@ -15,5 +15,5 @@ right_shoulder_pitch,0,-1.5708
 left_shoulder_pitch,0,-1.5708
 right_elbow,0,0
 left_elbow,0,0
-head_motor_0,0,0
-head_motor_1,0,0
+head_yaw,0,0
+head_pitch,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv
index a14de3c17..dcff3fc8d 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 right_hip_roll,0,0,0,0,0,0.2,0.2,0,0,0,0,0,0,0,0
 left_hip_roll,0,0,0,0,0,0.2,0.2,0,0,0,0,0,0,0,0
 right_hip_pitch,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
-left_leg_motor_2,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
+left_hip_pitch,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
 right_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
-left_leg_motor_3,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
+left_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
 right_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
-left_leg_motor_4,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
+left_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
 right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 right_shoulder_pitch,0,0,0,0,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 left_shoulder_pitch,0,-1.57,-1.57,-1.57,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 right_elbow,0,0,0,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0
 left_elbow,0,1.57,1.57,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0
-head_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-head_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+head_pitch,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv
index fcc548397..c0cbae7a6 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupback_real.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0.0,-1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0
 right_hip_roll,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.2,0.0,0.0,0.0,0,0,0,0
 left_hip_roll,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.2,0.0,0.0,0.0,0,0,0,0
 right_hip_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7
-left_leg_motor_2,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7
+left_hip_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7
 right_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
-left_leg_motor_3,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
+left_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
 right_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
-left_leg_motor_4,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
+left_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
 right_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
 left_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
 right_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9
 left_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9
 right_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0
 left_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0
-head_motor_0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
-head_motor_1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
+head_yaw,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
+head_pitch,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv
index 0409c7c31..f94644bdc 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupfront.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0,0,0,0,0,0,0,0,0,0
 right_hip_roll,0,0,0,0,0,0,0,0,0,0
 left_hip_roll,0,0,0,0,0,0,0,0,0,0
 right_hip_pitch,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
-left_leg_motor_2,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
+left_hip_pitch,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
 right_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
-left_leg_motor_3,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
+left_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
 right_ankle_pitch,0,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
-left_leg_motor_4,0,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
+left_ankle_pitch,0,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
 right_ankle_roll,0,0,0,0,0,0,0,0,0,0
 left_ankle_roll,0,0,0,0,0,0,0,0,0,0
 right_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 left_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 right_elbow,0,3.14,3.14,2,2,1.3,0,0,0,0
 left_elbow,0,3.14,3.14,2,2,1.3,0,0,0,0
-head_motor_0,0,0,0,0,0,0,0,0,0,0
-head_motor_1,0,0,0,0,0,0,0,0,0,0
+head_yaw,0,0,0,0,0,0,0,0,0,0
+head_pitch,0,0,0,0,0,0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupside.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupside.csv
index 5a13f8e4c..c764f9a5d 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupside.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/getupside.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0,0
 right_hip_roll,0,0
 left_hip_roll,0,0
 right_hip_pitch,0,0
-left_leg_motor_2,0,0
+left_hip_pitch,0,0
 right_knee,0,0
-left_leg_motor_3,0,0
+left_knee,0,0
 right_ankle_pitch,0,0
-left_leg_motor_4,0,0
+left_ankle_pitch,0,0
 right_ankle_roll,0,0
 left_ankle_roll,0,0
 right_shoulder_pitch,0,-1.5708
 left_shoulder_pitch,0,-1.5708
 right_elbow,0,0
 left_elbow,0,0
-head_motor_0,0,0
-head_motor_1,0,0
+head_yaw,0,0
+head_pitch,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv
index aa299bcaa..a1ff9feb7 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieleft.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0,0,0
 right_hip_roll,0,0,0
 left_hip_roll,0,0,0
 right_hip_pitch,0.564,0.564,0.564
-left_leg_motor_2,0.564,0.564,0.564
+left_hip_pitch,0.564,0.564,0.564
 right_knee,-1.176,-1.176,-1.176
-left_leg_motor_3,-1.176,-2.5,-1.176
+left_knee,-1.176,-2.5,-1.176
 right_ankle_pitch,0.613,0.613,0.613
-left_leg_motor_4,0.613,1.2,0.613
+left_ankle_pitch,0.613,1.2,0.613
 right_ankle_roll,0,0,0
 left_ankle_roll,0,0,0
 right_shoulder_pitch,3.14,3.14,3.14
 left_shoulder_pitch,3.14,3.14,3.14
 right_elbow,0,0,0
 left_elbow,0,0,0
-head_motor_0,0,0,0
-head_motor_1,0,0,0
+head_yaw,0,0,0
+head_pitch,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv
index f309eadce..5f365ce1d 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/goalieright.csv
@@ -4,16 +4,16 @@ left_hip_yaw,0,0,0
 right_hip_roll,0,0,0
 left_hip_roll,0,0,0
 right_hip_pitch,0.564,0.564,0.564
-left_leg_motor_2,0.564,0.564,0.564
+left_hip_pitch,0.564,0.564,0.564
 right_knee,-1.176,-2.5,-1.176
-left_leg_motor_3,-1.176,-1.176,-1.176
+left_knee,-1.176,-1.176,-1.176
 right_ankle_pitch,0.613,1.2,0.613
-left_leg_motor_4,0.613,0.613,0.613
+left_ankle_pitch,0.613,0.613,0.613
 right_ankle_roll,0,0,0
 left_ankle_roll,0,0,0
 right_shoulder_pitch,3.14,3.14,3.14
 left_shoulder_pitch,3.14,3.14,3.14
 right_elbow,0,0,0
 left_elbow,0,0,0
-head_motor_0,0,0,0
-head_motor_1,0,0,0
+head_yaw,0,0,0
+head_pitch,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv
index e967e03fc..7dedc30e9 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv
@@ -1,9 +1,9 @@
 time,0.69,0.7,0.75,1,1.2,1.4,1.41,1.7,2
 left_hip_yaw,0,0,0,0,0,0,0,0,0
 left_hip_roll,0.1,0.1,-0.15,-0.15,-0.25,-0.25,-0.25,-0.25,0.1
-left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25
-left_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4
-left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
+left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25
+left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4
+left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
 left_ankle_roll,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25,-0.1
 left_shoulder_pitch,0,0,0,0,-1,-1,1,1,0
 left_elbow,2,2,2,2,2,2,2,2,2
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv
index 40aa19efd..0486a908c 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv
@@ -6,16 +6,16 @@ left_hip_roll,0,0,0
 right_hip_pitch,0,0,0
 left_leg_motor_2,0,0,0
 right_knee,0,0,0
-left_leg_motor_3,0,0,0
+left_knee,0,0,0
 right_ankle_pitch,0,0,0
-left_leg_motor_4,0,0,0
+left_ankle_pitch,0,0,0
 right_ankle_roll,0,0,0
 left_ankle_roll,0,0,0
 right_shoulder_pitch,0,3.14,3.14
 left_shoulder_pitch,0,3.14,3.14
 right_elbow,0,0,0
 left_elbow,0,0,0
-head_motor_0,0,1,-1
-head_motor_1,0,0,0
+head_yaw,0,1,-1
+head_pitch,0,0,0
 right_shoulder_roll,0,0,0
 left_shoulder_roll,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv
index f61faf9f1..f628d70ac 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv
@@ -6,16 +6,16 @@ left_hip_roll,0,0,0,0,0,0.2,0.2,0,0,0,0,0,0,0,0
 right_hip_pitch,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
 left_leg_motor_2,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564
 right_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
-left_leg_motor_3,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
+left_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
 right_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
-left_leg_motor_4,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
+left_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613
 right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 right_shoulder_pitch,0,0,0,0,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 left_shoulder_pitch,0,-1.57,-1.57,-1.57,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 right_shoulder_roll,0,0,0,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0
 left_shoulder_roll,0,1.57,1.57,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0
-head_motor_0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
-head_motor_1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+head_pitch,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 right_elbow,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 left_elbow,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv
index fcc548397..2ef76a947 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv
@@ -6,14 +6,14 @@ left_hip_roll,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.2,0.0,0.0,0.0,0,0,0,
 right_hip_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7
 left_leg_motor_2,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7
 right_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
-left_leg_motor_3,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
+left_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6
 right_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
-left_leg_motor_4,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
+left_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2
 right_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
 left_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
 right_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9
 left_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9
 right_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0
 left_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0
-head_motor_0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
-head_motor_1,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
+head_yaw,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
+head_pitch,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv
index 7c6aa86bf..74552c003 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv
@@ -6,16 +6,16 @@ left_hip_roll,0,0,0,0,0,0,0,0,0,0
 right_hip_pitch,0,0,0,1,1.5,2,2,2,1.7,0.564
 left_leg_motor_2,0,0,0,1,1.5,2,2,2,1.7,0.564
 right_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
-left_leg_motor_3,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
+left_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176
 right_ankle_pitch,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613
-left_leg_motor_4,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613
+left_ankle_pitch,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613
 right_ankle_roll,0,0,0,0,0,0,0,0,0,0
 left_ankle_roll,0,0,0,0,0,0,0,0,0,0
 right_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 left_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0
 right_shoulder_roll,0,3.14,3.14,2,2,1.3,0,0,0,0
 left_shoulder_roll,0,3.14,3.14,2,2,1.3,0,0,0,0
-head_motor_0,0,0,0,0,0,0,0,0,0,0
-head_motor_1,0,0,0,0,0,0,0,0,0,0
+head_yaw,0,0,0,0,0,0,0,0,0,0
+head_pitch,0,0,0,0,0,0,0,0,0,0
 right_elbow,0,0,0,0,0,0,0,0,0,0
 left_elbow,0,0,0,0,0,0,0,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv
index 5a13f8e4c..ceb190ca7 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv
@@ -6,14 +6,14 @@ left_hip_roll,0,0
 right_hip_pitch,0,0
 left_leg_motor_2,0,0
 right_knee,0,0
-left_leg_motor_3,0,0
+left_knee,0,0
 right_ankle_pitch,0,0
-left_leg_motor_4,0,0
+left_ankle_pitch,0,0
 right_ankle_roll,0,0
 left_ankle_roll,0,0
 right_shoulder_pitch,0,-1.5708
 left_shoulder_pitch,0,-1.5708
 right_elbow,0,0
 left_elbow,0,0
-head_motor_0,0,0
-head_motor_1,0,0
+head_yaw,0,0
+head_pitch,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv
index aa299bcaa..6c544846e 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv
@@ -6,14 +6,14 @@ left_hip_roll,0,0,0
 right_hip_pitch,0.564,0.564,0.564
 left_leg_motor_2,0.564,0.564,0.564
 right_knee,-1.176,-1.176,-1.176
-left_leg_motor_3,-1.176,-2.5,-1.176
+left_knee,-1.176,-2.5,-1.176
 right_ankle_pitch,0.613,0.613,0.613
-left_leg_motor_4,0.613,1.2,0.613
+left_ankle_pitch,0.613,1.2,0.613
 right_ankle_roll,0,0,0
 left_ankle_roll,0,0,0
 right_shoulder_pitch,3.14,3.14,3.14
 left_shoulder_pitch,3.14,3.14,3.14
 right_elbow,0,0,0
 left_elbow,0,0,0
-head_motor_0,0,0,0
-head_motor_1,0,0,0
+head_yaw,0,0,0
+head_pitch,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv
index f309eadce..8fba6b668 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv
@@ -6,14 +6,14 @@ left_hip_roll,0,0,0
 right_hip_pitch,0.564,0.564,0.564
 left_leg_motor_2,0.564,0.564,0.564
 right_knee,-1.176,-2.5,-1.176
-left_leg_motor_3,-1.176,-1.176,-1.176
+left_knee,-1.176,-1.176,-1.176
 right_ankle_pitch,0.613,1.2,0.613
-left_leg_motor_4,0.613,0.613,0.613
+left_ankle_pitch,0.613,0.613,0.613
 right_ankle_roll,0,0,0
 left_ankle_roll,0,0,0
 right_shoulder_pitch,3.14,3.14,3.14
 left_shoulder_pitch,3.14,3.14,3.14
 right_elbow,0,0,0
 left_elbow,0,0,0
-head_motor_0,0,0,0
-head_motor_1,0,0,0
+head_yaw,0,0,0
+head_pitch,0,0,0
diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv
index 988a0be17..6a75c4e68 100644
--- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv
+++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv
@@ -2,8 +2,8 @@ time,0.69,0.7,0.75,1,1.2,1.4,1.41,1.7,2
 left_hip_yaw,0,0,0,0,0,0,0,0,0
 left_hip_roll,0.1,0.1,-0.15,-0.15,-0.25,-0.25,-0.25,-0.25,0.1
 left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25
-left_leg_motor_3,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4
-left_leg_motor_4,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
+left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4
+left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2
 left_ankle_roll,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25,-0.1
 left_shoulder_pitch,0,0,0,0,-1,-1,1,1,0
 left_elbow,2,2,2,2,2,2,2,2,2
@@ -16,5 +16,5 @@ right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1
 right_shoulder_pitch,0,0,0,0,1,1,-1,-1,0
 right_elbow,2,2,2,2,2,2,2,2,2
 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance
-head_motor_0,0,0,0,0,0,0,0,0,0
-head_motor_1,0,0,0,0,0,0,0,0,0
+head_yaw,0,0,0,0,0,0,0,0,0
+head_pitch,0,0,0,0,0,0,0,0,0