From b00dc79bbe9d35596f4a717564f474809238bb93 Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Wed, 1 Apr 2026 23:42:50 -0600
Subject: [PATCH 1/3] feat(lab_sim): Add lab desk objects and update objectives

Add desk objects (microscope, bottle) to lab_sim scene, adjust collision
wall/tray positions, update waypoints and objective configurations. Switch
MPC pose tracking objective to use wrist camera and update square trajectory.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
---
 src/lab_sim/description/assets/bottle.stl     |     3 +
 src/lab_sim/description/assets/microscope.stl |     3 +
 src/lab_sim/description/keyboard-texture.png  |     3 +
 src/lab_sim/description/laptop-screen.png     |     3 +
 src/lab_sim/description/picknik_ur.xacro      |    12 +-
 src/lab_sim/description/scene.xml             |  1037 +-
 .../objectives/3_waypoint_pick_and_place.xml  |    18 +-
 .../objectives/classical_pick_and_place.xml   |   114 -
 .../computelinkposefromwaypoint.xml           |     2 +-
 .../constrained_pick_and_place_subtree.xml    |   327 +-
 .../objectives/constrained_pick_place.xml     |    53 -
 .../objectives/create_pcl_poses_vector.xml    |    12 +-
 src/lab_sim/objectives/grasp_planning.xml     |     4 +-
 .../grasp_pose_tuning_with_april_tag.xml      |     2 +-
 .../objectives/grasp_pose_using_yaml.xml      |     2 +-
 .../objectives/move_flasks_to_burners.xml     |    33 +
 src/lab_sim/objectives/mpc_pose_tracking.xml  |     4 +-
 ...loud_avoidance_with_sphere_down_sample.xml |     6 +-
 ...loud_avoidance_with_sphere_down_sample.xml |     8 +-
 ...th_static_sphere_point_cloud_avoidance.xml |     2 +-
 ...tle.xml => pick_1_pill_bottle_with_ml.xml} |    14 +-
 .../pick_all_bottles_with_apriltags.xml       |   571 +
 .../pick_april_tag_object_with_approval.xml   |     4 +-
 src/lab_sim/objectives/pick_object.xml        |     2 +-
 src/lab_sim/objectives/pick_up_cube.xml       |     4 +-
 src/lab_sim/objectives/place_object.xml       |     5 +-
 src/lab_sim/objectives/register_cad_part.xml  |    21 +-
 .../objectives/register_cad_part_subtree.xml  |    30 +-
 src/lab_sim/objectives/square_traj.yaml       | 10704 +++++++++++-----
 .../stitch_multiple_point_clouds_together.xml |    18 +-
 .../test/objectives_integration_test.py       |    45 +-
 src/lab_sim/waypoints/ur_waypoints.yaml       |   645 +-
 src/picknik_accessories                       |     2 +-
 33 files changed, 9540 insertions(+), 4173 deletions(-)
 create mode 100644 src/lab_sim/description/assets/bottle.stl
 create mode 100644 src/lab_sim/description/assets/microscope.stl
 create mode 100644 src/lab_sim/description/keyboard-texture.png
 create mode 100644 src/lab_sim/description/laptop-screen.png
 delete mode 100644 src/lab_sim/objectives/classical_pick_and_place.xml
 delete mode 100644 src/lab_sim/objectives/constrained_pick_place.xml
 create mode 100644 src/lab_sim/objectives/move_flasks_to_burners.xml
 rename src/lab_sim/objectives/{pick_1_pill_bottle.xml => pick_1_pill_bottle_with_ml.xml} (91%)
 create mode 100644 src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml

diff --git a/src/lab_sim/description/assets/bottle.stl b/src/lab_sim/description/assets/bottle.stl
new file mode 100644
index 000000000..e4a329ea0
--- /dev/null
+++ b/src/lab_sim/description/assets/bottle.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6358a28dd2f70d6552b14d2675c66b5bdf1ee1b5236cb7cca32b8c2e7560c602
+size 6284
diff --git a/src/lab_sim/description/assets/microscope.stl b/src/lab_sim/description/assets/microscope.stl
new file mode 100644
index 000000000..a7b48066d
--- /dev/null
+++ b/src/lab_sim/description/assets/microscope.stl
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b4739e5c8149f7f74f1765f5fecdfad4bbf566bb1d992662b46f6f9b6be4d67c
+size 19951234
diff --git a/src/lab_sim/description/keyboard-texture.png b/src/lab_sim/description/keyboard-texture.png
new file mode 100644
index 000000000..9e685eab3
--- /dev/null
+++ b/src/lab_sim/description/keyboard-texture.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f0b212c56658ff5e5936f82867413fb357322e432664609c6b496c190925d20c
+size 170370
diff --git a/src/lab_sim/description/laptop-screen.png b/src/lab_sim/description/laptop-screen.png
new file mode 100644
index 000000000..67982b9f1
--- /dev/null
+++ b/src/lab_sim/description/laptop-screen.png
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4045f46ccec667d8c317cde63758d9ab826f8990ebcdee163ca10a0241d52b09
+size 215006
diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro
index fe73e3e84..98d1a4342 100644
--- a/src/lab_sim/description/picknik_ur.xacro
+++ b/src/lab_sim/description/picknik_ur.xacro
@@ -663,7 +663,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <box size="5.0 0.6 0.30" />
+        <box size="5.0 0.2 0.30" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -672,7 +672,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0.14 0.125" />
       <geometry>
-        <box size="5.0 0.2 0.5" />
+        <box size="5.0 0.12 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -681,7 +681,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 -0.22 0.125" />
       <geometry>
-        <box size="5.0 0.12 0.5" />
+        <box size="5.0 0.08 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -690,7 +690,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0" />
       <geometry>
-        <box size="5.0 0.6 0.3" />
+        <box size="5.0 0.2 0.3" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -699,7 +699,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0.14 0.125" />
       <geometry>
-        <box size="5.0 0.2 0.5" />
+        <box size="5.0 0.12 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
@@ -708,7 +708,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 -0.22 0.125" />
       <geometry>
-        <box size="5.0 0.12 0.5" />
+        <box size="5.0 0.08 0.5" />
       </geometry>
       <material name="grey">
         <color rgba="0.1 0.1 0.1 1.0" />
diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml
index 7d896c290..b2e6dc99d 100644
--- a/src/lab_sim/description/scene.xml
+++ b/src/lab_sim/description/scene.xml
@@ -6,14 +6,96 @@
 
   <compiler angle="radian" autolimits="true" />
 
-  <option integrator="implicitfast" />
+  <option integrator="implicitfast" impratio="10" noslip_iterations="3" />
+
+  <!-- nuser_cam=1 enables per-camera user params: 0=RGB+Depth, 1=RGB-only, 2=3D-lidar -->
+  <size nuser_cam="1" />
 
   <asset>
-    <mesh name="cube" file="assets/Cube.stl" />
+    <!-- Laptop keyboard texture (dark checker pattern) -->
+    <texture
+      name="keyboard_tex"
+      type="2d"
+      builtin="checker"
+      rgb1="0.03 0.03 0.03"
+      rgb2="0.08 0.08 0.08"
+      width="256"
+      height="256"
+    />
+    <material
+      name="keyboard_mat"
+      texture="keyboard_tex"
+      texrepeat="10 6"
+      specular="0.2"
+      shininess="0.3"
+    />
+    <!-- Laptop screen texture (dark IDE-like gradient) -->
+    <texture
+      name="screen_tex"
+      type="2d"
+      builtin="gradient"
+      rgb1="0.05 0.12 0.18"
+      rgb2="0.02 0.06 0.10"
+      width="256"
+      height="256"
+    />
+    <material name="screen_mat" texture="screen_tex" emission="0.4" />
+    <texture
+      name="desktop_screenshot_tex"
+      type="cube"
+      filedown="laptop-screen.png"
+      rgb1="0.02 0.02 0.02"
+    />
+    <material
+      name="desktop_screenshot_mat"
+      texture="desktop_screenshot_tex"
+      emission="0.3"
+    />
+    <!-- Keyboard texture -->
+    <texture
+      name="keyboard_img_tex"
+      type="cube"
+      filefront="keyboard-texture.png"
+      rgb1="0.15 0.15 0.15"
+    />
+    <material
+      name="keyboard_img_mat"
+      texture="keyboard_img_tex"
+      specular="0.2"
+      shininess="0.3"
+    />
+    <!-- AprilTag material for tray is provided by desk_globals.xml (apriltag_material1) -->
+    <!-- Red plastic tray material -->
+    <material
+      name="red_plastic"
+      rgba="0.75 0.15 0.1 1"
+      specular="0.6"
+      shininess="0.7"
+    />
+    <!-- Light brown plastic pot material -->
+    <material
+      name="pot_plastic"
+      rgba="0.72 0.52 0.32 1"
+      specular="0.5"
+      shininess="0.6"
+    />
   </asset>
 
+  <!-- Set offscreen rendering resolution to match camera sensor resolution (640x480) so that
+       MuJoCo renders images at the correct size for the simulated wrist and scene cameras. -->
+  <visual>
+    <global offwidth="640" offheight="480" />
+    <headlight ambient="0.3 0.3 0.3" diffuse="0.5 0.5 0.5" />
+  </visual>
+
   <worldbody>
-    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+    <light
+      pos="0 0 1.5"
+      dir="0 0 -1"
+      directional="true"
+      diffuse="1 1 1"
+      ambient="0.3 0.3 0.3"
+    />
 
     <light
       name="spotlight"
@@ -31,51 +113,960 @@
     <!-- Scene Camera -->
     <site
       name="scene_camera_optical_frame"
-      pos="-1.0 -2.0 2.966"
-      euler="3.99 0.000 0"
+      pos="1.8 -0.495 1.0"
+      xyaxes="0.586 0.810 0.000 0.186 -0.135 -0.973"
     />
     <camera
       name="scene_camera"
-      pos="-1.0 -2.0 2.966"
+      pos="1.8 -0.495 1.0"
       fovy="58"
       mode="fixed"
       resolution="640 480"
-      euler="0.849 0.000 0.0"
+      xyaxes="0.586 0.810 0.000 -0.186 0.135 0.973"
+      user="0"
     />
     <!-- lab desk -->
     <include file="lab_desk/desk.xml" />
-    <!-- Define the block with AprilTags on each face -->
-    <body name="block1" pos="0.2 0.75 0.575">
-      <freejoint name="block1" />
+    <!-- Modern laptop (white Mac) -->
+    <body name="laptop" pos="-0.6 0.6718 0.474" euler="0 0 0.2618">
+      <freejoint />
+      <!-- Base / keyboard deck -->
       <geom
+        name="laptop_base"
         type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
-        material="apriltag_material1"
+        size="0.15 0.10 0.005"
+        pos="0 0 0.005"
+        material="keyboard_img_mat"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Screen - tilted back ~20 degrees past vertical, pushed forward past bezel -->
+      <geom
+        name="laptop_screen"
+        type="box"
+        size="0.15 0.003 0.10"
+        pos="0 0.138 0.104"
+        euler="-0.35 0 0"
+        material="desktop_screenshot_mat"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: base -->
+      <geom
+        name="laptop_base_col"
+        type="box"
+        size="0.15 0.10 0.005"
+        pos="0 0 0.005"
+        group="4"
+      />
+      <!-- Collision: screen -->
+      <geom
+        name="laptop_screen_col"
+        type="box"
+        size="0.155 0.004 0.105"
+        pos="0 0.133 0.104"
+        euler="-0.35 0 0"
+        group="4"
       />
     </body>
-    <body name="block2" pos="0.0 0.75 0.575">
-      <freejoint name="block2" />
-      <geom class="visual" mesh="cube" pos="0 0 -0.03" />
-      <geom class="collision" type="box" size="0.025 0.025 0.025" pos="0 0 0" />
+    <!-- Fake potted plant (left of microscope) -->
+    <body name="fake_plant" pos="-1.55 0.5768 0.474">
+      <freejoint />
+      <!-- Pot -->
+      <geom
+        name="plant_pot"
+        type="cylinder"
+        size="0.03 0.03"
+        pos="0 0 0.03"
+        material="pot_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_pot_rim"
+        type="cylinder"
+        size="0.033 0.004"
+        pos="0 0 0.056"
+        material="pot_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Thin stem -->
+      <geom
+        name="plant_stem"
+        type="cylinder"
+        size="0.004 0.06"
+        pos="0 0 0.12"
+        rgba="0.3 0.25 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Large foliage puff -->
+      <geom
+        name="plant_foliage_1"
+        type="sphere"
+        size="0.06"
+        pos="0 0 0.2"
+        rgba="0.15 0.45 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_2"
+        type="sphere"
+        size="0.045"
+        pos="0.035 0.02 0.22"
+        rgba="0.2 0.5 0.18 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_3"
+        type="sphere"
+        size="0.04"
+        pos="-0.03 0.025 0.23"
+        rgba="0.12 0.42 0.12 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_4"
+        type="sphere"
+        size="0.04"
+        pos="0.01 -0.035 0.22"
+        rgba="0.18 0.48 0.15 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="plant_foliage_5"
+        type="sphere"
+        size="0.035"
+        pos="-0.02 -0.02 0.25"
+        rgba="0.16 0.46 0.14 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: pot -->
+      <geom
+        name="plant_pot_col"
+        type="cylinder"
+        size="0.033 0.032"
+        pos="0 0 0.032"
+        group="4"
+      />
     </body>
-    <body name="block3" pos="-0.2 0.75 0.575">
-      <freejoint name="block3" />
+    <!-- Bronze alligator statue (top shelf) -->
+    <body name="alligator_statue" pos="-0.5 0.8768 1.365">
+      <freejoint />
+      <!-- Trophy base - dark wood with gold nameplate -->
       <geom
+        name="gator_base"
         type="box"
-        size="0.025 0.025 0.025"
-        rgba="1 1 1 1"
+        size="0.09 0.045 0.012"
+        pos="-0.01 0 -0.030"
+        rgba="0.12 0.06 0.02 1"
+      />
+      <geom
+        name="gator_base_top"
+        type="box"
+        size="0.08 0.04 0.003"
+        pos="-0.01 0 -0.015"
+        rgba="0.15 0.08 0.03 1"
+      />
+      <geom
+        name="gator_nameplate"
+        type="box"
+        size="0.03 0.001 0.008"
+        pos="-0.01 -0.041 -0.030"
+        rgba="0.75 0.65 0.25 1"
+      />
+      <!-- Body - three segments for organic shape -->
+      <geom
+        name="gator_body_front"
+        type="capsule"
+        size="0.013 0.02"
+        pos="0.02 0 0"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_body_mid"
+        type="capsule"
+        size="0.014 0.025"
+        pos="-0.015 0 0.001"
+        euler="0 1.5708 0"
+        rgba="0.18 0.65 0.18 1"
+      />
+      <geom
+        name="gator_body_rear"
+        type="capsule"
+        size="0.012 0.02"
+        pos="-0.045 0 0"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Head - wider, flatter -->
+      <geom
+        name="gator_head"
+        type="box"
+        size="0.022 0.012 0.007"
+        pos="0.06 0 -0.001"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Upper jaw -->
+      <geom
+        name="gator_upper_jaw"
+        type="box"
+        size="0.02 0.008 0.004"
+        pos="0.09 0 0.001"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Lower jaw -->
+      <geom
+        name="gator_lower_jaw"
+        type="box"
+        size="0.018 0.007 0.003"
+        pos="0.088 0 -0.005"
+        rgba="0.3 0.75 0.25 1"
+      />
+      <!-- Snout tip -->
+      <geom
+        name="gator_snout"
+        type="capsule"
+        size="0.005 0.008"
+        pos="0.108 0 -0.001"
+        euler="0 1.5708 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Eye ridges -->
+      <geom
+        name="gator_eye_l"
+        type="sphere"
+        size="0.004"
+        pos="0.055 0.01 0.007"
+        rgba="0.9 0.85 0.1 1"
+      />
+      <geom
+        name="gator_eye_r"
+        type="sphere"
+        size="0.004"
+        pos="0.055 -0.01 0.007"
+        rgba="0.9 0.85 0.1 1"
+      />
+      <!-- Dorsal ridges -->
+      <geom
+        name="gator_ridge_1"
+        type="sphere"
+        size="0.004"
+        pos="0.03 0 0.013"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_2"
+        type="sphere"
+        size="0.004"
+        pos="0.01 0 0.014"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_3"
+        type="sphere"
+        size="0.004"
+        pos="-0.01 0 0.014"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_4"
+        type="sphere"
+        size="0.003"
+        pos="-0.03 0 0.012"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <geom
+        name="gator_ridge_5"
+        type="sphere"
+        size="0.003"
+        pos="-0.045 0 0.011"
+        rgba="0.15 0.55 0.12 1"
+      />
+      <!-- Tail - three tapering segments with slight curve -->
+      <geom
+        name="gator_tail_1"
+        type="capsule"
+        size="0.009 0.02"
+        pos="-0.08 0 0.002"
+        euler="0 1.6 0"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_tail_2"
+        type="capsule"
+        size="0.006 0.02"
+        pos="-0.115 0 0.006"
+        euler="0 1.7 0.1"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_tail_3"
+        type="capsule"
+        size="0.003 0.015"
+        pos="-0.145 0.005 0.009"
+        euler="0 1.75 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Front legs with feet -->
+      <geom
+        name="gator_leg_fl"
+        type="capsule"
+        size="0.005 0.01"
+        pos="0.025 0.018 -0.008"
+        euler="0.5 0 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_fl"
+        type="box"
+        size="0.006 0.005 0.002"
+        pos="0.025 0.028 -0.016"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_leg_fr"
+        type="capsule"
+        size="0.005 0.01"
+        pos="0.025 -0.018 -0.008"
+        euler="-0.5 0 -0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_fr"
+        type="box"
+        size="0.006 0.005 0.002"
+        pos="0.025 -0.028 -0.016"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Rear legs with feet (slightly larger) -->
+      <geom
+        name="gator_leg_bl"
+        type="capsule"
+        size="0.005 0.012"
+        pos="-0.04 0.018 -0.008"
+        euler="0.5 0 0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_bl"
+        type="box"
+        size="0.007 0.005 0.002"
+        pos="-0.04 0.030 -0.018"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_leg_br"
+        type="capsule"
+        size="0.005 0.012"
+        pos="-0.04 -0.018 -0.008"
+        euler="-0.5 0 -0.2"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <geom
+        name="gator_foot_br"
+        type="box"
+        size="0.007 0.005 0.002"
+        pos="-0.04 -0.030 -0.018"
+        rgba="0.2 0.7 0.2 1"
+      />
+      <!-- Collision: simplified bounding box for entire statue -->
+      <geom
+        name="gator_col"
+        type="box"
+        size="0.16 0.035 0.025"
+        pos="-0.01 0 -0.005"
+        group="4"
+      />
+    </body>
+    <!-- Test tube rack -->
+    <body name="test_tube_rack" pos="0.3 0.5468 0.474">
+      <freejoint />
+      <!-- Base plate -->
+      <geom
+        name="rack_base"
+        type="box"
+        size="0.065 0.025 0.003"
+        pos="0 0 0.003"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Front wall -->
+      <geom
+        name="rack_front"
+        type="box"
+        size="0.065 0.003 0.025"
+        pos="0 -0.022 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Back wall -->
+      <geom
+        name="rack_back"
+        type="box"
+        size="0.065 0.003 0.025"
+        pos="0 0.022 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Left end -->
+      <geom
+        name="rack_left"
+        type="box"
+        size="0.003 0.025 0.025"
+        pos="-0.062 0 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Right end -->
+      <geom
+        name="rack_right"
+        type="box"
+        size="0.003 0.025 0.025"
+        pos="0.062 0 0.031"
+        rgba="0.6 0.6 0.65 1"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: overall rack box (heavy to keep tubes upright) -->
+      <geom
+        name="rack_col"
+        type="box"
+        size="0.065 0.025 0.028"
+        pos="0 0 0.028"
+        group="4"
+        mass="50.0"
+      />
+    </body>
+    <!-- Red plastic sorting tray -->
+    <body name="sorting_tray" pos="0.95 0.5568 0.474" euler="0 0 0.0873">
+      <freejoint />
+      <!-- Base plate -->
+      <geom
+        name="tray_base"
+        type="box"
+        size="0.1 0.2 0.003"
+        pos="0 0 0.003"
+        material="red_plastic"
+        mass="0.5"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Front lip -->
+      <geom
+        name="tray_front"
+        type="box"
+        size="0.097 0.003 0.013"
+        pos="0 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Back lip -->
+      <geom
+        name="tray_back"
+        type="box"
+        size="0.097 0.003 0.013"
+        pos="0 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Left lip -->
+      <geom
+        name="tray_left"
+        type="box"
+        size="0.003 0.197 0.013"
+        pos="-0.1 0 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Right lip -->
+      <geom
+        name="tray_right"
+        type="box"
+        size="0.003 0.197 0.013"
+        pos="0.1 0 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Rounded corners -->
+      <geom
+        name="tray_corner_fl"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="-0.1 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_fr"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="0.1 -0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_bl"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="-0.1 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tray_corner_br"
+        type="cylinder"
+        size="0.003 0.013"
+        pos="0.1 0.2 0.019"
+        material="red_plastic"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on tray floor -->
+      <geom
+        name="tray_apriltag"
+        type="box"
+        size="0.04 0.04 0.0005"
+        pos="0 -0.15 0.0065"
         material="apriltag_material2"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- Collision: tray base + lips as one box -->
+      <geom
+        name="tray_base_col"
+        type="box"
+        size="0.103 0.203 0.003"
+        pos="0 0 0.003"
+        group="4"
+        mass="0.5"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_front_col"
+        type="box"
+        size="0.103 0.003 0.016"
+        pos="0 -0.203 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_back_col"
+        type="box"
+        size="0.103 0.003 0.016"
+        pos="0 0.203 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_left_col"
+        type="box"
+        size="0.003 0.203 0.016"
+        pos="-0.103 0 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+      <geom
+        name="tray_right_col"
+        type="box"
+        size="0.003 0.203 0.016"
+        pos="0.103 0 0.016"
+        group="4"
+        friction="2.0 0.1 0.01"
+        condim="4"
+      />
+    </body>
+    <!-- Test tubes with colored liquids (manipulable) -->
+    <body name="tube_red" pos="0.25 0.5468 0.532">
+      <freejoint name="tube_red" />
+      <geom
+        name="tube_red_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_red_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.9 0.1 0.1 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom name="tube_red_col" type="capsule" size="0.0065 0.045" group="4" />
+    </body>
+    <body name="tube_blue" pos="0.27 0.5468 0.532">
+      <freejoint name="tube_blue" />
+      <geom
+        name="tube_blue_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_blue_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.1 0.2 0.9 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom name="tube_blue_col" type="capsule" size="0.0065 0.045" group="4" />
+    </body>
+    <body name="tube_green" pos="0.29 0.5468 0.532">
+      <freejoint name="tube_green" />
+      <geom
+        name="tube_green_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_green_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.1 0.8 0.2 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_green_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_yellow" pos="0.31 0.5468 0.532">
+      <freejoint name="tube_yellow" />
+      <geom
+        name="tube_yellow_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_yellow_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.95 0.9 0.1 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_yellow_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_purple" pos="0.33 0.5468 0.532">
+      <freejoint name="tube_purple" />
+      <geom
+        name="tube_purple_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_purple_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.6 0.1 0.8 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_purple_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <body name="tube_orange" pos="0.35 0.5468 0.532">
+      <freejoint name="tube_orange" />
+      <geom
+        name="tube_orange_glass"
+        type="capsule"
+        size="0.0065 0.045"
+        rgba="0.85 0.88 0.9 0.3"
+        mass="0.01"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="tube_orange_liquid"
+        type="capsule"
+        size="0.005 0.03"
+        pos="0 0 -0.008"
+        rgba="0.95 0.5 0.05 0.85"
+        contype="0"
+        conaffinity="0"
+        mass="0.03"
+      />
+      <geom
+        name="tube_orange_col"
+        type="capsule"
+        size="0.0065 0.045"
+        group="4"
+      />
+    </body>
+    <!-- Medicine bottles (manipulable) -->
+    <body name="bottle_white" pos="-0.16 0.5068 0.519">
+      <freejoint name="bottle_white" />
+      <geom
+        name="bottle_white_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.92 0.92 0.92 1"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_white_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.1 0.5 0.1 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_white_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_white_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_amber" pos="0.07 0.4868 0.519">
+      <freejoint name="bottle_amber" />
+      <geom
+        name="bottle_amber_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.55 0.33 0.08 0.75"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_amber_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.9 0.9 0.9 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on white cap (visible from above) -->
+      <geom
+        name="bottle_amber_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_amber_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_blue" pos="-0.04 0.5768 0.519">
+      <freejoint name="bottle_blue" />
+      <geom
+        name="bottle_blue_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.15 0.25 0.65 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_blue_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.1 0.2 0.8 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_blue_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_blue_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_green" pos="0.15 0.5968 0.519">
+      <freejoint name="bottle_green" />
+      <geom
+        name="bottle_green_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.15 0.5 0.25 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_green_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.8 0.1 0.1 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on cap (visible from above) -->
+      <geom
+        name="bottle_green_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_green_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
+      />
+    </body>
+    <body name="bottle_red" pos="-0.13 0.6268 0.519">
+      <freejoint name="bottle_red" />
+      <geom
+        name="bottle_red_body"
+        type="cylinder"
+        size="0.02 0.045"
+        rgba="0.7 0.15 0.1 0.85"
+        mass="0.08"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_red_cap"
+        type="cylinder"
+        size="0.021 0.008"
+        pos="0 0 0.053"
+        rgba="0.9 0.9 0.9 1"
+        mass="0.02"
+        contype="0"
+        conaffinity="0"
+      />
+      <!-- AprilTag on white cap (visible from above) -->
+      <geom
+        name="bottle_red_apriltag"
+        type="box"
+        size="0.018 0.018 0.003"
+        pos="0 0 0.065"
+        material="apriltag_material1"
+        contype="0"
+        conaffinity="0"
+      />
+      <geom
+        name="bottle_red_col"
+        type="cylinder"
+        size="0.021 0.053"
+        group="4"
+        friction="1.5 0.01 0.001"
+        condim="4"
       />
     </body>
+    <!-- Removed bottle_purple and bottle_yellow to reduce crowding -->
     <include file="ur5e/ur5e_linear_rail.xml" />
   </worldbody>
 
   <keyframe>
     <key
       name="default"
-      qpos="-1.168 0.776711 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 1.07777 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 1.07777 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.729603 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 -0.699998 0.85 0.472753 -0.759699 6.25044e-07 1.49157e-06 -0.650275 -0.5 0.69 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.609999 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.7 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 -3.70361e-09 0.9 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 -0.1 0.85 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.0952783 0.840677 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 0.2 0.75 0.498669 1 3.49711e-17 2.9976e-18 1.51884e-30 1.73912e-05 0.749992 0.498669 1 0.00016562 0.000360588 4.6626e-11 -0.2 0.75 0.498669 1 2.94315e-17 -7.92697e-18 -4.14434e-30 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
-      qvel="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 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 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"
+      qpos="-1.168 0.573511 0.469754 1 5.72696e-06 -4.26533e-08 -9.38821e-05 -1.34591 0.87457 0.902167 1 -2.70778e-08 7.60222e-07 -8.18971e-05 -1.1503 0.87457 0.902163 -0.873311 -1.32611e-06 -2.31428e-06 -0.487163 -0.871046 0.526403 0.473006 -0.839083 -3.76738e-06 -5.23752e-06 -0.544003 0.605 0.5578 0.473827 1 0 0 0 0.3 0.6468 0.472753 1 0 0 0 -0.5 0.4868 0.472779 -0.75966 -8.69884e-06 3.09906e-06 -0.65032 -0.360001 0.406799 0.472778 -0.759663 -6.29279e-06 3.61581e-06 -0.650317 -0.299999 0.4968 0.472779 -0.75966 -5.34077e-06 1.33814e-06 -0.65032 0.15 0.3468 0.47375 -0.839072 -1.82055e-05 -5.31435e-05 0.544021 0.12 0.4468 0.47375 0.283662 4.83891e-05 2.85336e-05 -0.958924 0.22 0.3968 0.47375 -0.852936 4.00776e-05 -3.93632e-05 -0.522015 -0.6 0.6718 0.474 0.9914 0 0 0.1305 -1.55 0.5768 0.474 1 0 0 0 -0.5 0.8768 1.365 1 0 0 0 0.3 0.5468 0.474 1 0 0 0 0.95 0.5568 0.474 0.999 0 0 0.0436 0.25 0.5468 0.532 1 0 0 0 0.27 0.5468 0.532 1 0 0 0 0.29 0.5468 0.532 1 0 0 0 0.31 0.5468 0.532 1 0 0 0 0.33 0.5468 0.532 1 0 0 0 0.35 0.5468 0.532 1 0 0 0 -0.16 0.5068 0.519 0.9945 0 0 0.1045 0.07 0.4868 0.519 0.9976 0 0 -0.0698 -0.04 0.5768 0.519 0.9990 0 0 0.0436 0.15 0.5968 0.519 0.9914 0 0 -0.1305 -0.13 0.6268 0.519 0.9848 0 0 0.1736 1.56909e-06 -3.71262e-07 -1.88491 1.10086 -1.25647 -1.57075 -3.63345e-07 0.0525545 0.000140183 0.0525603 -0.050625 0.0525545 0.000140184 0.0525567 -0.0506181"
+      qvel="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 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 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 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 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 0 0 0 0 0 0 0 0"
       ctrl="0 -1.8849 1.0997 -1.2566 -1.57075 0 0 0"
     />
   </keyframe>
diff --git a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
index c0f1f9756..c4698d040 100644
--- a/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
+++ b/src/lab_sim/objectives/3_waypoint_pick_and_place.xml
@@ -10,7 +10,7 @@
       <!--Setup the environment to its "home" configuration - move to center and open gripper-->
       <SubTree
         ID="Move to Waypoint"
-        waypoint_name="Look at Table"
+        waypoint_name="3 Waypoint Mid Point"
         joint_group_name="manipulator"
       />
       <Action
@@ -26,7 +26,7 @@
           <Control ID="Sequence">
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
+              waypoint_name="3 Waypoint Pick Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -37,13 +37,13 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
             <SubTree
               ID="Move to Waypoint"
               joint_group_name="manipulator"
-              waypoint_name="Place Cube"
+              waypoint_name="3 Waypoint Place Pose"
             />
             <Action
               ID="MoveGripperAction"
@@ -53,7 +53,7 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
           </Control>
@@ -61,7 +61,7 @@
           <Control ID="Sequence">
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Place Cube"
+              waypoint_name="3 Waypoint Place Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -72,12 +72,12 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Pick Cube"
+              waypoint_name="3 Waypoint Pick Pose"
               joint_group_name="manipulator"
             />
             <Action
@@ -88,7 +88,7 @@
             />
             <SubTree
               ID="Move to Waypoint"
-              waypoint_name="Look at Table"
+              waypoint_name="3 Waypoint Mid Point"
               joint_group_name="manipulator"
             />
           </Control>
diff --git a/src/lab_sim/objectives/classical_pick_and_place.xml b/src/lab_sim/objectives/classical_pick_and_place.xml
deleted file mode 100644
index 75b8066e1..000000000
--- a/src/lab_sim/objectives/classical_pick_and_place.xml
+++ /dev/null
@@ -1,114 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Classical Pick and Place">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Classical Pick and Place"
-    _description="Pick and place using classical (non-ML) algorithms to identify cuboid objects."
-    _favorite="false"
-  >
-    <Control ID="Sequence" name="TopLevelSequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        joint_group_name="manipulator"
-        waypoint_name="Look at Table"
-      />
-      <SubTree ID="Open Gripper" _collapsed="true" />
-      <Control ID="Sequence">
-        <SubTree ID="Clear Snapshot" _collapsed="true" />
-        <Action
-          ID="GetPointCloud"
-          message_out="{point_cloud}"
-          message_timeout_sec="5.000000"
-          topic_name="/wrist_camera/points"
-        />
-        <Action
-          ID="SendPointCloudToUI"
-          pcd_topic="/pcd_pointcloud_captures"
-          point_cloud="{point_cloud}"
-        />
-        <Action
-          ID="FindSingularCuboids"
-          base_frame="world"
-          cluster_distance_threshold="0.01"
-          crop_box_origin_xyz="0.0;0.6;0.0"
-          crop_box_size_xyz="0.3;0.3;0.8"
-          detected_shapes="{detected_shapes}"
-          plane_model_threshold="0.01"
-          point_cloud="{point_cloud}"
-        />
-        <Control ID="Fallback">
-          <Decorator
-            ID="ForEach"
-            index="{index}"
-            out="{shape}"
-            vector_in="{detected_shapes}"
-          >
-            <Control ID="Sequence">
-              <Action
-                ID="ExtractGraspableObjectPose"
-                graspable_object="{shape}"
-                pose="{shape_pose}"
-              />
-              <Action
-                ID="TransformPose"
-                input_pose="{shape_pose}"
-                output_pose="{shape_pose}"
-                quaternion_xyzw="1.0;0;0;0"
-              />
-              <Action
-                ID="VisualizePose"
-                marker_lifetime="0.000000"
-                marker_size="0.100000"
-                pose="{shape_pose}"
-                marker_name="Grasp Pose"
-              />
-              <SubTree
-                ID="Plan Move To Pose"
-                _collapsed="true"
-                move_to_pose_solution="{move_to_pose_solution}"
-                target_pose="{shape_pose}"
-              />
-              <Control ID="IfThenElse">
-                <Action
-                  ID="WaitForUserTrajectoryApproval"
-                  solution="{move_to_pose_solution}"
-                />
-                <Control ID="Sequence">
-                  <SubTree
-                    ID="Execute MTC Solution"
-                    solution="{move_to_pose_solution}"
-                  />
-                  <Action ID="AlwaysFailure" />
-                </Control>
-                <Action ID="AlwaysSuccess" />
-              </Control>
-            </Control>
-          </Decorator>
-          <Control ID="Sequence">
-            <SubTree ID="Close Gripper" _collapsed="true" />
-            <SubTree
-              ID="Move to Waypoint"
-              _collapsed="true"
-              joint_group_name="manipulator"
-              waypoint_name="Look at Table"
-            />
-            <SubTree
-              ID="Execute MTC Solution"
-              solution="{move_to_pose_solution}"
-            />
-            <SubTree ID="Open Gripper" _collapsed="true" />
-          </Control>
-        </Control>
-      </Control>
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Classical Pick and Place">
-      <MetadataFields>
-        <Metadata runnable="true" />
-        <Metadata subcategory="Application - Basic Examples" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/computelinkposefromwaypoint.xml b/src/lab_sim/objectives/computelinkposefromwaypoint.xml
index 14ee2e031..0a971c741 100644
--- a/src/lab_sim/objectives/computelinkposefromwaypoint.xml
+++ b/src/lab_sim/objectives/computelinkposefromwaypoint.xml
@@ -11,7 +11,7 @@
         ID="RetrieveWaypoint"
         joint_group_name="manipulator"
         waypoint_joint_state="{target_joint_state}"
-        waypoint_name="Above Pick Object"
+        waypoint_name="MPC Pose Tracking"
       />
       <Action
         ID="UnpackRobotJointStateMessage"
diff --git a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
index 28df92cd3..6c704ee5e 100644
--- a/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
+++ b/src/lab_sim/objectives/constrained_pick_and_place_subtree.xml
@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root
   BTCPP_format="4"
   main_tree_to_execute="Constrained Pick and Place Subtree"
@@ -6,81 +6,276 @@
   <!--//////////-->
   <BehaviorTree
     ID="Constrained Pick and Place Subtree"
-    _description="Takes in waypoints to run a constrained pick and place operation."
+    _description="Takes in pick and place waypoints, auto-computes pre-positions using forward kinematics with a height offset, and uses force-controlled lowering for placement."
     _favorite="false"
   >
-    <Control ID="Sequence">
-      <!--Move to pick location-->
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pre_pick}"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        waypoint_name="{pick}"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        keep_orientation_link_names="grasp_link"
-      />
-      <!--We force success as the gripper closes, since we are commanding a position it will never reach (fingers fully closed)-->
-      <Decorator ID="ForceSuccess">
-        <SubTree ID="Close Gripper" />
-      </Decorator>
-      <!--Move to place (drop) location-->
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_pick}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{place}"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Open Gripper" />
-      <SubTree
-        ID="Move to Waypoint"
-        joint_group_name="manipulator"
-        keep_orientation="true"
-        keep_orientation_tolerance="0.2"
-        _collapsed="true"
-        waypoint_name="{pre_place}"
-        keep_orientation_link_names="grasp_link"
-      />
+    <Control ID="Sequence" name="Main Sequence">
+      <!--Compute pre-pick and pre-place poses from waypoint FK + height offset (0.305m = 12 inches)-->
+      <Control ID="Sequence" name="Compute Pre-Positions">
+        <Action
+          ID="GetCurrentPlanningScene"
+          planning_scene_msg="{planning_scene}"
+        />
+        <!--Compute pre-pick pose: FK from pick waypoint, then offset upward-->
+        <Action
+          ID="RetrieveWaypoint"
+          joint_group_name="manipulator"
+          waypoint_joint_state="{pick_joint_state}"
+          waypoint_name="{pick}"
+        />
+        <Action
+          ID="UnpackRobotJointStateMessage"
+          joint_state="{pick_joint_state_msg}"
+          multi_dof_joint_state="{pick_multi_dof}"
+          robot_joint_state="{pick_joint_state}"
+        />
+        <Action
+          ID="ComputeLinkPoseForwardKinematics"
+          joint_state="{pick_joint_state_msg}"
+          link_name="grasp_link"
+          link_pose="{pick_link_pose}"
+          planning_scene="{planning_scene}"
+        />
+        <Action
+          ID="TransformPose"
+          input_pose="{pick_link_pose}"
+          output_pose="{pre_pick_pose}"
+          quaternion_xyzw="0;0;0;1"
+          translation_xyz="0;0;-0.305"
+          visualize_pose="true"
+          marker_text="Pre-Pick"
+        />
+        <!--Compute pre-place pose: FK from place waypoint, then offset upward-->
+        <Action
+          ID="RetrieveWaypoint"
+          joint_group_name="manipulator"
+          waypoint_joint_state="{place_joint_state}"
+          waypoint_name="{place}"
+        />
+        <Action
+          ID="UnpackRobotJointStateMessage"
+          joint_state="{place_joint_state_msg}"
+          multi_dof_joint_state="{place_multi_dof}"
+          robot_joint_state="{place_joint_state}"
+        />
+        <Action
+          ID="ComputeLinkPoseForwardKinematics"
+          joint_state="{place_joint_state_msg}"
+          link_name="grasp_link"
+          link_pose="{place_link_pose}"
+          planning_scene="{planning_scene}"
+        />
+        <Action
+          ID="TransformPose"
+          input_pose="{place_link_pose}"
+          output_pose="{pre_place_pose}"
+          quaternion_xyzw="0;0;0;1"
+          translation_xyz="0;0;-0.305"
+          marker_text="Pre-Place"
+          visualize_pose="true"
+        />
+      </Control>
+      <!--Move to pre-pick, then lower to pick using constrained motion-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Pick">
+          <Action
+            ID="InitializeMTCTask"
+            task_id="move_to_pre_pick"
+            controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+            task="{pre_pick_task}"
+          />
+          <Action ID="SetupMTCCurrentState" task="{pre_pick_task}" />
+          <Action
+            ID="SetupMTCPlanToPose"
+            ik_frame="grasp_link"
+            planning_group_name="manipulator"
+            target_pose="{pre_pick_pose}"
+            task="{pre_pick_task}"
+          />
+          <Action
+            ID="PlanMTCTask"
+            solution="{pre_pick_solution}"
+            task="{pre_pick_task}"
+            max_solutions="1"
+          />
+          <SubTree ID="Execute MTC Solution" solution="{pre_pick_solution}" />
+          <SubTree
+            ID="Move to Waypoint"
+            waypoint_name="{pick}"
+            joint_group_name="manipulator"
+            keep_orientation="true"
+            keep_orientation_tolerance="0.2"
+            keep_orientation_link_names="grasp_link"
+          />
+          <!--ForceSuccess because the gripper closes to a position it will never reach (fingers fully closed)-->
+          <SubTree ID="Close Gripper" />
+        </Control>
+      </Control>
+      <!--Retract upward after grasping (Cartesian path, 0.305m = 12 inches)-->
+      <Control ID="Sequence" name="Pick Retract">
+        <Action
+          ID="CreateStampedPose"
+          reference_frame="grasp_link"
+          stamped_pose="{pick_retract_pose}"
+          position_xyz="0;0;-0.305"
+        />
+        <Action ID="ResetPoseStampedVector" vector="{pick_retract_path}" />
+        <Action
+          ID="AddPoseStampedToVector"
+          input="{pick_retract_pose}"
+          vector="{pick_retract_path}"
+        />
+        <Action
+          ID="PlanCartesianPath"
+          acceleration_scale_factor="1.000000"
+          blending_radius="0.020000"
+          debug_solution="{debug_solution}"
+          ik_cartesian_space_density="0.010000"
+          ik_joint_space_density="0.100000"
+          joint_trajectory_msg="{joint_trajectory_msg}"
+          path="{pick_retract_path}"
+          planning_group_name="manipulator"
+          position_only="true"
+          tip_links="grasp_link"
+          trajectory_sampling_rate="100"
+          velocity_scale_factor="1.000000"
+        />
+        <Action ID="ExecuteTrajectory" />
+      </Control>
+      <!--Move to pre-place position-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Move to Pre-Place">
+          <Action
+            ID="InitializeMTCTask"
+            task_id="move_to_pre_place"
+            controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+            task="{pre_place_task}"
+          />
+          <Action ID="SetupMTCCurrentState" task="{pre_place_task}" />
+          <Action
+            ID="SetupMTCPlanToPose"
+            ik_frame="grasp_link"
+            planning_group_name="manipulator"
+            target_pose="{pre_place_pose}"
+            task="{pre_place_task}"
+          />
+          <Action
+            ID="PlanMTCTask"
+            solution="{pre_place_solution}"
+            task="{pre_place_task}"
+            max_solutions="1"
+          />
+          <SubTree ID="Execute MTC Solution" solution="{pre_place_solution}" />
+        </Control>
+      </Control>
+      <!--Force-controlled lower until contact (force threshold: 20N)-->
+      <Control ID="Sequence" name="Force-Controlled Place">
+        <Action
+          ID="SwitchController"
+          activate_controllers="velocity_force_controller"
+        />
+        <Action
+          ID="CallTriggerService"
+          service_name="/velocity_force_controller/zero_fts"
+        />
+        <Action
+          ID="CreateStampedWrench"
+          stamped_wrench="{desired_wrench}"
+          reference_frame="grasp_link"
+          torque_xyz="0;0;0"
+          force_xyz="0;0;0"
+        />
+        <Action
+          ID="CreateStampedTwist"
+          reference_frame="grasp_link"
+          stamped_twist="{desired_twist}"
+          angular_velocity_xyz="0;0;0"
+          linear_velocity_xyz="0.0;0;0.05"
+        />
+        <Action
+          ID="CreateStampedPose"
+          reference_frame="grasp_link"
+          stamped_pose="{initial_pose}"
+        />
+        <Action
+          ID="TransformPoseFrame"
+          output_pose="{initial_pose}"
+          input_pose="{initial_pose}"
+        />
+        <Control ID="Parallel" failure_count="1" success_count="1">
+          <Action
+            ID="ForceExceedsThreshold"
+            minimum_consecutive_wrench_values="5"
+            wrench_frame_name="tool0"
+            hand_frame_name="grasp_link"
+            force_threshold="100"
+          />
+          <Action
+            ID="PublishVelocityForceCommand"
+            force_controlled_axes="0;0;0;0;0;0"
+            wrench_gain="0.01"
+            velocity_controlled_axes="0;0;1;0;0;0"
+            publish_rate="50"
+            twist_stamped="{desired_twist}"
+            wrench_stamped="{desired_wrench}"
+          />
+        </Control>
+        <SubTree
+          ID="Force Relaxation"
+          _collapsed="true"
+          wrench_gain="0.01"
+          relaxation_time_ms="500"
+          tip_link="grasp_link"
+        />
+        <Action
+          ID="SwitchController"
+          activate_controllers="joint_trajectory_admittance_controller"
+        />
+      </Control>
+      <!--Release and retract upward-->
+      <Control ID="Sequence">
+        <Control ID="Sequence" name="Release and Retract">
+          <SubTree ID="Open Gripper" />
+          <Action
+            ID="CreateStampedPose"
+            reference_frame="grasp_link"
+            stamped_pose="{place_retract_pose}"
+            position_xyz="0;0;-0.08"
+          />
+          <Action ID="ResetPoseStampedVector" vector="{place_retract_path}" />
+          <Action
+            ID="AddPoseStampedToVector"
+            input="{place_retract_pose}"
+            vector="{place_retract_path}"
+          />
+          <Action
+            ID="PlanCartesianPath"
+            acceleration_scale_factor="1.000000"
+            blending_radius="0.020000"
+            debug_solution="{debug_solution}"
+            ik_cartesian_space_density="0.010000"
+            ik_joint_space_density="0.100000"
+            joint_trajectory_msg="{joint_trajectory_msg}"
+            path="{place_retract_path}"
+            planning_group_name="manipulator"
+            position_only="true"
+            tip_links="grasp_link"
+            trajectory_sampling_rate="100"
+            velocity_scale_factor="1.000000"
+          />
+          <Action ID="ExecuteTrajectory" />
+        </Control>
+      </Control>
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
     <SubTree ID="Constrained Pick and Place Subtree">
-      <inout_port name="pick" default="" />
-      <inout_port name="place" default="" />
-      <inout_port name="pre_pick" default="" />
-      <inout_port name="pre_place" default="" />
       <MetadataFields>
         <Metadata runnable="false" />
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
+      <input_port name="pick" default="" />
+      <input_port name="place" default="" />
     </SubTree>
   </TreeNodesModel>
 </root>
diff --git a/src/lab_sim/objectives/constrained_pick_place.xml b/src/lab_sim/objectives/constrained_pick_place.xml
deleted file mode 100644
index 0b435156e..000000000
--- a/src/lab_sim/objectives/constrained_pick_place.xml
+++ /dev/null
@@ -1,53 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Move Flasks to Burners">
-  <!--//////////-->
-  <BehaviorTree
-    ID="Move Flasks to Burners"
-    _description="Example of picking and placing objects with orientation constraints enforced on the arm's motion"
-    _favorite="true"
-  >
-    <Control ID="Sequence">
-      <SubTree
-        ID="Move to Waypoint"
-        _collapsed="true"
-        waypoint_name="Flask 1a - Pre Pick"
-        joint_group_name="manipulator"
-        keep_orientation_link_names="grasp_link"
-      />
-      <SubTree ID="Open Gripper" />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 1a - Pre Pick"
-        pick="Flask 1b - Pick"
-        pre_place="Flask 1c - Pre Place"
-        place="Flask 1d - Place"
-        name="Flask 1"
-      />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 3a - Pre Pick"
-        pick="Flask 3b - Pick"
-        pre_place="Flask 3c - Pre Place"
-        place="Flask 3d - Place"
-        name="Flask 3"
-      />
-      <SubTree
-        ID="Constrained Pick and Place Subtree"
-        pre_pick="Flask 2a - Pre Pick"
-        pick="Flask 2b - Pick"
-        pre_place="Flask 2c - Pre Place"
-        place="Flask 2d - Place"
-        name="Flask 2"
-        _collapsed="true"
-      />
-    </Control>
-  </BehaviorTree>
-  <TreeNodesModel>
-    <SubTree ID="Move Flasks to Burners">
-      <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
-        <Metadata runnable="true" />
-      </MetadataFields>
-    </SubTree>
-  </TreeNodesModel>
-</root>
diff --git a/src/lab_sim/objectives/create_pcl_poses_vector.xml b/src/lab_sim/objectives/create_pcl_poses_vector.xml
index a1df1862c..db794c4c7 100644
--- a/src/lab_sim/objectives/create_pcl_poses_vector.xml
+++ b/src/lab_sim/objectives/create_pcl_poses_vector.xml
@@ -11,7 +11,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="0.5;0.5;0.9"
+        position_xyz="0.5;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
@@ -23,7 +23,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="0.4;0.5;0.9"
+        position_xyz="0.4;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
@@ -35,7 +35,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="0.3;0.5;0.9"
+        position_xyz="0.3;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
@@ -47,7 +47,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="0.2;0.5;0.9"
+        position_xyz="0.2;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
@@ -59,7 +59,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="0.1;0.5;0.9"
+        position_xyz="0.1;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
@@ -71,7 +71,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0.000; 0.000; 0.000; 1.000"
-        position_xyz="-0.1;0.5;0.9"
+        position_xyz="-0.1;0.2;0.9"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
diff --git a/src/lab_sim/objectives/grasp_planning.xml b/src/lab_sim/objectives/grasp_planning.xml
index 597d3ca41..9f811e362 100644
--- a/src/lab_sim/objectives/grasp_planning.xml
+++ b/src/lab_sim/objectives/grasp_planning.xml
@@ -18,7 +18,7 @@
         <Action
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
-          position_xyz="0.01;.75;0.515"
+          position_xyz="-0.04;.575;0.58"
           stamped_pose="{grasp_candidate_1}"
           reference_frame="world"
         />
@@ -37,7 +37,7 @@
         <Action
           ID="CreateStampedPose"
           orientation_xyzw="0;0.9848078;0.1736482;0"
-          position_xyz="0.01;.75;0.515"
+          position_xyz="-0.04;.575;0.58"
           stamped_pose="{grasp_candidate_2}"
           reference_frame="world"
         />
diff --git a/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml b/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
index f026abe10..4cf0bb8d1 100644
--- a/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
+++ b/src/lab_sim/objectives/grasp_pose_tuning_with_april_tag.xml
@@ -35,7 +35,7 @@
         output_pose="{view_pose}"
         input_pose="{detection_pose}"
         quaternion_xyzw="1;0;0;0"
-        translation_xyz="0;0.1;0.15"
+        translation_xyz="0;0.1;0.25"
       />
       <SubTree
         ID="Plan Move To Pose"
diff --git a/src/lab_sim/objectives/grasp_pose_using_yaml.xml b/src/lab_sim/objectives/grasp_pose_using_yaml.xml
index f2c9808fe..b5f57951b 100644
--- a/src/lab_sim/objectives/grasp_pose_using_yaml.xml
+++ b/src/lab_sim/objectives/grasp_pose_using_yaml.xml
@@ -35,7 +35,7 @@
         output_pose="{view_pose}"
         input_pose="{detection_pose}"
         quaternion_xyzw="1;0;0;0"
-        translation_xyz="0;0.1;0.15"
+        translation_xyz="0;0.1;0.25"
       />
       <SubTree
         ID="Plan Move To Pose"
diff --git a/src/lab_sim/objectives/move_flasks_to_burners.xml b/src/lab_sim/objectives/move_flasks_to_burners.xml
new file mode 100644
index 000000000..6d9a23460
--- /dev/null
+++ b/src/lab_sim/objectives/move_flasks_to_burners.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Move Flasks to Burners">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Move Flasks to Burners"
+    _description="Example of picking and placing objects with orientation constraints and force-controlled placement"
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <SubTree ID="Open Gripper" />
+      <SubTree
+        ID="Constrained Pick and Place Subtree"
+        pick="Flask 1 - Pick"
+        place="Flask 1 - Place"
+        name="Flask 1"
+      />
+      <SubTree
+        ID="Constrained Pick and Place Subtree"
+        pick="Flask 2 - Pick"
+        place="Flask 2 - Place"
+        name="Flask 2"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Move Flasks to Burners">
+      <MetadataFields>
+        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/mpc_pose_tracking.xml b/src/lab_sim/objectives/mpc_pose_tracking.xml
index 8ee08b702..7c79d5d71 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking.xml
@@ -12,13 +12,13 @@
           ID="Move to Waypoint"
           _collapsed="true"
           joint_group_name="manipulator"
-          waypoint_name="Above Pick Object"
+          waypoint_name="Pose Tracking"
         />
         <Action
           ID="CreateStampedPose"
           reference_frame="pinch"
           stamped_pose="{stamped_pose}"
-          position_xyz="0;0.3;0.2"
+          position_xyz="0.2;0.2;0.0"
           orientation_xyzw="0;0;1;1"
         />
         <Action
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml b/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml
index 559fc4652..3a43dfd1f 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking_dynamic_point_cloud_avoidance_with_sphere_down_sample.xml
@@ -16,15 +16,15 @@
         waypoint_name="Home"
         joint_group_name="manipulator"
       />
-      <SubTree ID="Create PCL Poses Vector" _collapsed="false" />
+      <SubTree ID="Create PCL Poses Vector" _collapsed="true" />
       <SubTree
         ID="ComputeLinkPoseFromWaypoint"
-        _collapsed="false"
+        _collapsed="true"
         link_pose="{link_pose}"
       />
       <Decorator ID="ForEach" vector_in="{target_poses}" out="{stamped_pose}">
         <Control ID="Sequence">
-          <Action ID="VisualizePose" />
+          <Action ID="VisualizePose" pose="{stamped_pose}" />
           <Action ID="BreakpointSubscriber" _skipIf="true" />
           <SubTree
             ID="LoadAndVisualizePointCloud"
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml b/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml
index d3a78e57d..629a165c9 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking_static_point_cloud_avoidance_with_sphere_down_sample.xml
@@ -17,16 +17,16 @@
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
-      <Action ID="VisualizePose" />
+      <Action ID="VisualizePose" pose="{stamped_pose}" />
       <SubTree
         ID="LoadAndVisualizePointCloud"
-        _collapsed="false"
+        _collapsed="true"
         input_pose="{stamped_pose}"
         transform_cloud="{output_cloud}"
       />
       <SubTree
         ID="ComputeLinkPoseFromWaypoint"
-        _collapsed="false"
+        _collapsed="true"
         link_pose="{link_pose}"
       />
       <Action
@@ -38,7 +38,7 @@
       />
       <SubTree
         ID="GetPoseAtGripperandDisplay"
-        _collapsed="false"
+        _collapsed="true"
         output_pose="{gripper_pose}"
       />
       <Action
diff --git a/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml b/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
index eedf4f290..b5f33e03e 100644
--- a/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
+++ b/src/lab_sim/objectives/mpc_pose_tracking_with_static_sphere_point_cloud_avoidance.xml
@@ -15,7 +15,7 @@
         ID="GetPointCloud"
         message_out="{point_cloud}"
         message_timeout_sec="5.000000"
-        topic_name="/scene_camera/points"
+        topic_name="/wrist_camera/points"
         publisher_timeout_sec="5.000000"
       />
       <Action
diff --git a/src/lab_sim/objectives/pick_1_pill_bottle.xml b/src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
similarity index 91%
rename from src/lab_sim/objectives/pick_1_pill_bottle.xml
rename to src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
index 10d32a456..8d4e96bdf 100644
--- a/src/lab_sim/objectives/pick_1_pill_bottle.xml
+++ b/src/lab_sim/objectives/pick_1_pill_bottle_with_ml.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="Pick 1 Pill Bottle">
+<root BTCPP_format="4" main_tree_to_execute="Pick 1 Pill Bottle with ML">
   <!--//////////-->
   <BehaviorTree
-    ID="Pick 1 Pill Bottle"
+    ID="Pick 1 Pill Bottle with ML"
     _description="Captures and segments objects from a point cloud based on a text prompt. Then grasps the first object returned from the segmentation."
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence">
       <SubTree ID="Open Gripper" _collapsed="true" />
@@ -13,7 +13,7 @@
         _collapsed="true"
         joint_group_name="manipulator"
         keep_orientation_link_names="grasp_link"
-        waypoint_name="Above Pick Object"
+        waypoint_name="Look at Table"
       />
       <SubTree
         ID="Segment Point Cloud from Text Prompt Subtree"
@@ -28,7 +28,7 @@
         clip_model_path="models/clip.onnx"
         clipseg_model_path="models/clipseg.onnx"
         erosion_size="2"
-        prompts="a cylindrical object with a dark gray and silver metallic body, featuring a circular blue cap on top"
+        prompts="a cylindrical object"
         threshold="0.28"
         masks3d="{masks3d}"
         point_cloud="{point_cloud}"
@@ -105,12 +105,12 @@
         _collapsed="true"
         joint_group_name="manipulator"
         keep_orientation_link_names="grasp_link"
-        waypoint_name="Above Pick Object"
+        waypoint_name="Look at Table"
       />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="Pick 1 Pill Bottle">
+    <SubTree ID="Pick 1 Pill Bottle with ML">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - ML (GPU Recommended)" />
diff --git a/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
new file mode 100644
index 000000000..1af9da5aa
--- /dev/null
+++ b/src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml
@@ -0,0 +1,571 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Pick All Bottles with AprilTags">
+  <BehaviorTree
+    ID="Pick All Bottles with AprilTags"
+    _description="Detects AprilTags on medicine bottles, picks them up, and places them in a tray. Loops until no more tags are found."
+    _favorite="true"
+  >
+    <Control ID="Sequence">
+      <!--Add collision wall at the back of the table to prevent the arm from hitting the shelf-->
+      <Action
+        ID="GetCurrentPlanningScene"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="CreateStampedPose"
+        reference_frame="world"
+        stamped_pose="{back_wall_pose}"
+        position_xyz="0.15; 0.83; 0.85"
+      />
+      <Action
+        ID="CreateGraspableObject"
+        cuboid_object="{back_wall_object}"
+        dx="2.0"
+        dy="0.05"
+        dz="1.0"
+        generate_front_face="false"
+        generate_side_faces="false"
+        generate_top_face="false"
+        object_id="back_wall"
+        pose="{back_wall_pose}"
+      />
+      <Action
+        ID="AddVirtualObjectToPlanningScene"
+        apply_planning_scene_service="/apply_planning_scene"
+        object="{back_wall_object}"
+        planning_scene_msg="{planning_scene}"
+      />
+      <Action
+        ID="SwitchUIPrimaryView"
+        primary_view_name="/apriltag_detections"
+      />
+      <Control
+        ID="Sequence"
+        name="Compute Tray Place Positions"
+        _collapsed="true"
+      >
+        <SubTree ID="Open Gripper" _collapsed="true" />
+        <SubTree
+          ID="Move to Waypoint"
+          waypoint_name="Predrop Bottles Right"
+          _collapsed="true"
+        />
+        <Action
+          ID="GetCameraInfo"
+          topic_name="/wrist_camera/camera_info"
+          message_out="{camera_info}"
+          message_timeout_sec="5.0"
+          publisher_timeout_sec="5.0"
+        />
+        <Action
+          ID="GetImage"
+          topic_name="/wrist_camera/color"
+          message_out="{tray_image}"
+          message_timeout_sec="5.0"
+        />
+        <Action
+          ID="DetectAprilTags"
+          image="{tray_image}"
+          camera_info="{camera_info}"
+          detections="{tray_detections}"
+          apriltag_family_name="36h11"
+          max_hamming="0"
+          n_threads="1"
+          quad_decimate="2.0"
+          quad_sigma="0.0"
+          refine_edges="true"
+          tag_size="0.08"
+        />
+        <Action
+          ID="ComputeTrayPlacePositionsUsingAprilTags"
+          detections="{tray_detections}"
+          camera_info="{camera_info}"
+          input_image="{tray_image}"
+          tray_apriltag_id="1"
+          num_rows="3"
+          columns_per_row="2"
+          row_spacing="0.10"
+          column_spacing="0.09"
+          tag_to_tray_center="0.125"
+          bottle_diameter="0.04"
+          place_positions="{place_positions}"
+        />
+        <!--Visualize coordinate frames at every place location-->
+        <Action ID="Script" code="place_marker_count := 0" />
+        <Decorator
+          ID="ForEach"
+          vector_in="{place_positions}"
+          out="{place_viz_pose}"
+          index="{place_viz_index}"
+        >
+          <Control ID="Sequence">
+            <Action
+              ID="VisualizePose"
+              pose="{place_viz_pose}"
+              marker_size="0.05"
+              marker_lifetime="0"
+              marker_name="{place_marker_count}"
+              marker_text="Place"
+            />
+            <Action ID="Script" code="place_marker_count += 1" />
+          </Control>
+        </Decorator>
+      </Control>
+      <Control ID="Sequence">
+        <SubTree ID="Clear Snapshot" _collapsed="true" />
+        <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
+        <SubTree ID="Look at Table" _collapsed="true" />
+        <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
+      </Control>
+      <!--Warm up velocity_force_controller so the first placement is not a cold start-->
+      <Control ID="Sequence">
+        <Action
+          ID="SwitchController"
+          activate_controllers="velocity_force_controller"
+        />
+        <Action
+          ID="CallTriggerService"
+          service_name="/velocity_force_controller/zero_fts"
+        />
+        <Action
+          ID="SwitchController"
+          activate_controllers="joint_trajectory_admittance_controller"
+        />
+      </Control>
+      <Decorator ID="ForceSuccess">
+        <Decorator ID="KeepRunningUntilFailure">
+          <Control ID="Sequence" name="TopLevelSequence" _collapsed="true">
+            <Control ID="Sequence" name="Detect AprilTag" _collapsed="true">
+              <SubTree ID="Look at Table" _collapsed="true" />
+              <SubTree ID="Open Gripper" _collapsed="false" />
+              <Action
+                ID="GetCameraInfo"
+                topic_name="/wrist_camera/camera_info"
+                message_out="{camera_info}"
+                message_timeout_sec="5.000000"
+                publisher_timeout_sec="5.000000"
+              />
+              <Action
+                ID="GetImage"
+                topic_name="/wrist_camera/color"
+                message_out="{image}"
+                message_timeout_sec="5.000000"
+              />
+              <!--Detect AprilTag and compute a pre-grasp viewpoint offset above it-->
+              <Action
+                ID="DetectAprilTags"
+                image="{image}"
+                camera_info="{camera_info}"
+                detections="{detections}"
+                apriltag_family_name="36h11"
+                max_hamming="0"
+                n_threads="1"
+                quad_decimate="2.0"
+                quad_sigma="0.0"
+                refine_edges="true"
+                tag_size="0.036"
+              />
+              <Action
+                ID="GetCenterMostAprilTag"
+                detection_pose="{detection_pose}"
+                detections="{detections}"
+                camera_info="{camera_info}"
+                image="{image}"
+                tag_size="0.036"
+              />
+              <Action
+                ID="TransformPoseFrame"
+                input_pose="{detection_pose}"
+                output_pose="{detection_pose}"
+                target_frame_id="world"
+              />
+              <Action
+                ID="TransformPose"
+                output_pose="{view_pose}"
+                input_pose="{detection_pose}"
+                quaternion_xyzw="1;0;0;0"
+                translation_xyz="0.0;0.0;0.3"
+                visualize_pose="true"
+                marker_size="0.1"
+                marker_lifetime="0"
+                marker_text="Pre-Grasp"
+              />
+            </Control>
+            <Control ID="Sequence" name="Refine Pose" _collapsed="true">
+              <Control ID="Sequence">
+                <!--Move to the pre-grasp viewpoint above the detected tag-->
+                <SubTree
+                  ID="Plan Move To Pose"
+                  target_pose="{view_pose}"
+                  move_to_pose_solution="{move_to_pose_solution}"
+                  _collapsed="true"
+                />
+                <SubTree
+                  ID="Execute MTC Solution"
+                  solution="{move_to_pose_solution}"
+                  _collapsed="true"
+                />
+                <!--Re-detect the tag from closer range for a more accurate pose-->
+                <Action
+                  ID="GetImage"
+                  topic_name="/wrist_camera/color"
+                  message_out="{image}"
+                  message_timeout_sec="5.000000"
+                />
+                <Action
+                  ID="DetectAprilTags"
+                  image="{image}"
+                  camera_info="{camera_info}"
+                  detections="{detections}"
+                  apriltag_family_name="36h11"
+                  max_hamming="0"
+                  n_threads="1"
+                  quad_decimate="2.0"
+                  quad_sigma="0.0"
+                  refine_edges="true"
+                  tag_size="0.036"
+                />
+                <Action
+                  ID="GetCenterMostAprilTag"
+                  detection_pose="{detection_pose}"
+                  detections="{detections}"
+                  camera_info="{camera_info}"
+                  image="{image}"
+                  tag_size="0.036"
+                />
+                <Action
+                  ID="TransformPoseFrame"
+                  input_pose="{detection_pose}"
+                  output_pose="{detection_pose}"
+                  target_frame_id="world"
+                />
+                <Action
+                  ID="VisualizePose"
+                  pose="{detection_pose}"
+                  marker_size="0.05"
+                  marker_lifetime="0"
+                  marker_name="detection_raw"
+                  marker_text="Detection Raw"
+                />
+                <!--Override tag orientation with a fixed downward grasp and offset Z to the bottle body-->
+                <Action
+                  ID="OverridePoseOrientation"
+                  input_pose="{detection_pose}"
+                  output_pose="{offset_pose}"
+                  orientation_xyzw="1;0;0;0"
+                />
+                <Action
+                  ID="TransformPose"
+                  input_pose="{offset_pose}"
+                  output_pose="{offset_pose}"
+                  translation_xyz="0;0;-0.03"
+                />
+                <!--Calibration offset: tune translation_xyz to correct systematic XY error after table move. Local X = World +X, Local Y = World -Y (orientation is 180° around X).-->
+                <Action
+                  ID="TransformPose"
+                  input_pose="{offset_pose}"
+                  output_pose="{offset_pose}"
+                  quaternion_xyzw="0.000; 0.000; 1.000; 0.000"
+                  translation_xyz="0;0;0"
+                />
+                <Action
+                  ID="VisualizeLine"
+                  start_pose="{detection_pose}"
+                  end_pose="{offset_pose}"
+                  line_thickness="0.005"
+                  marker_lifetime="0"
+                />
+                <Action
+                  ID="VisualizeLine"
+                  start_pose="{view_pose}"
+                  end_pose="{offset_pose}"
+                  line_thickness="0.005"
+                  marker_lifetime="0"
+                />
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Pick from Pose" _collapsed="true">
+              <Control ID="Sequence">
+                <Control ID="Sequence">
+                  <!--Initialize MTC pick task-->
+                  <Action
+                    ID="InitializeMTCTask"
+                    controller_names="joint_trajectory_admittance_controller;robotiq_gripper_controller"
+                    task="{mtc_task}"
+                    task_id="pick_object"
+                    trajectory_monitoring="false"
+                    timeout="-1.000000"
+                  />
+                  <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+                  <Action
+                    ID="SetupMTCConnectWithProRRT"
+                    acceleration_scale_factor="1.000000"
+                    keep_orientation="false"
+                    keep_orientation_link_names="grasp_link"
+                    keep_orientation_tolerance="0.100000"
+                    link_padding="0.000000"
+                    max_iterations="20000"
+                    planning_group_name="manipulator"
+                    seed="0"
+                    task="{mtc_task}"
+                    trajectory_sampling_rate="100"
+                    velocity_scale_factor="1.000000"
+                  />
+                  <!--Approach along grasp_link Z axis-->
+                  <Action
+                    ID="SetupMTCMoveAlongFrameAxis"
+                    acceleration_scale="1.000000"
+                    axis_frame="grasp_link"
+                    axis_x="0.000000"
+                    axis_y="0.000000"
+                    axis_z="1.000000"
+                    hand_frame="grasp_link"
+                    max_distance="0.1"
+                    min_distance="0.1"
+                    planning_group_name="manipulator"
+                    task="{mtc_task}"
+                    velocity_scale="1.000000"
+                    ignore_environment_collisions="false"
+                  />
+                  <!--Solve IK for the grasp pose-->
+                  <Action
+                    ID="ResetPoseStampedVector"
+                    vector="{grasp_pose_vector}"
+                  />
+                  <Action
+                    ID="AddPoseStampedToVector"
+                    input="{offset_pose}"
+                    vector="{grasp_pose_vector}"
+                  />
+                  <Action
+                    ID="SetupMTCBatchPoseIK"
+                    end_effector_group="moveit_ee"
+                    end_effector_link="grasp_link"
+                    ik_group="manipulator"
+                    ik_timeout_s="0.1"
+                    max_ik_solutions="16"
+                    monitored_stage="current state"
+                    target_poses="{grasp_pose_vector}"
+                    task="{mtc_task}"
+                  />
+                  <Action
+                    ID="VisualizePose"
+                    pose="{offset_pose}"
+                    marker_size="0.05"
+                    marker_lifetime="0"
+                    marker_name="grasp_target"
+                    marker_text="Grasp Target"
+                  />
+                  <Action ID="BreakpointSubscriber" _skipIf="true" />
+                  <Action
+                    ID="PlanMTCTask"
+                    solution="{mtc_solution}"
+                    task="{mtc_task}"
+                    max_solutions="4"
+                  />
+                  <SubTree
+                    ID="Execute MTC Solution"
+                    solution="{mtc_solution}"
+                    _collapsed="true"
+                  />
+                  <Decorator ID="ForceSuccess">
+                    <Action
+                      ID="MoveGripperAction"
+                      gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd"
+                      position="0.58"
+                      timeout="15.0"
+                    />
+                  </Decorator>
+                </Control>
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Retract" _collapsed="true">
+              <!--Retract away from the grasp-->
+              <Action
+                ID="CreateStampedPose"
+                reference_frame="grasp_link"
+                stamped_pose="{retract_pose}"
+                position_xyz="0;0;-0.25"
+              />
+              <Action ID="ResetPoseStampedVector" vector="{retract_path}" />
+              <Action
+                ID="AddPoseStampedToVector"
+                input="{retract_pose}"
+                vector="{retract_path}"
+              />
+              <Action
+                ID="PlanCartesianPath"
+                acceleration_scale_factor="1.000000"
+                blending_radius="0.020000"
+                debug_solution="{debug_solution}"
+                ik_cartesian_space_density="0.010000"
+                ik_joint_space_density="0.100000"
+                joint_trajectory_msg="{joint_trajectory_msg}"
+                path="{retract_path}"
+                planning_group_name="manipulator"
+                position_only="true"
+                tip_links="grasp_link"
+                trajectory_sampling_rate="100"
+                velocity_scale_factor="1.000000"
+              />
+              <Action ID="ExecuteTrajectory" />
+            </Control>
+            <Control ID="Sequence" name="Place in Tray" _collapsed="true">
+              <!--Pop the next place pose from the front of the computed positions vector-->
+              <Action
+                ID="GetElementOfVector"
+                vector_in="{place_positions}"
+                index="0"
+                element="{current_place_pose}"
+              />
+              <!--Pause here to inspect the place pose before lowering-->
+              <!--Compute a pre-place hover pose above the target-->
+              <Action
+                ID="TransformPose"
+                input_pose="{current_place_pose}"
+                output_pose="{pre_place_pose}"
+                quaternion_xyzw="0;0;1;0"
+                translation_xyz="0;0;-0.4"
+                visualize_pose="true"
+                marker_text="Pre-Place Pose"
+              />
+              <!--Move to the pre-place hover position above the tray-->
+              <SubTree
+                ID="Plan Move To Pose"
+                target_pose="{pre_place_pose}"
+                move_to_pose_solution="{place_solution}"
+                _collapsed="true"
+              />
+              <SubTree
+                ID="Execute MTC Solution"
+                solution="{place_solution}"
+                _collapsed="true"
+              />
+              <!--Lower until contact: flattened from lab_sim Push Button (velocity/force controller)-->
+              <Control ID="Sequence">
+                <Action
+                  ID="SwitchController"
+                  activate_controllers="velocity_force_controller"
+                />
+                <Action
+                  ID="CallTriggerService"
+                  service_name="/velocity_force_controller/zero_fts"
+                />
+                <Action
+                  ID="CreateStampedWrench"
+                  stamped_wrench="{desired_wrench}"
+                  reference_frame="grasp_link"
+                  torque_xyz="0;0;0"
+                  force_xyz="0;0;0"
+                />
+                <Action
+                  ID="CreateStampedTwist"
+                  reference_frame="grasp_link"
+                  stamped_twist="{desired_twist}"
+                  angular_velocity_xyz="0;0;0"
+                  linear_velocity_xyz="0.0;0;0.05"
+                />
+                <Action
+                  ID="CreateStampedPose"
+                  reference_frame="grasp_link"
+                  stamped_pose="{initial_pose}"
+                />
+                <Action
+                  ID="TransformPoseFrame"
+                  output_pose="{initial_pose}"
+                  input_pose="{initial_pose}"
+                />
+                <Control ID="Parallel" failure_count="1" success_count="1">
+                  <Action
+                    ID="ForceExceedsThreshold"
+                    minimum_consecutive_wrench_values="5"
+                    wrench_frame_name="tool0"
+                    hand_frame_name="grasp_link"
+                    force_threshold="30"
+                  />
+                  <Action
+                    ID="PublishVelocityForceCommand"
+                    force_controlled_axes="0;0;0;0;0;0"
+                    wrench_gain="0.01"
+                    velocity_controlled_axes="0;0;1;0;0;0"
+                    publish_rate="50"
+                    twist_stamped="{desired_twist}"
+                    wrench_stamped="{desired_wrench}"
+                  />
+                </Control>
+                <SubTree
+                  ID="Force Relaxation"
+                  _collapsed="true"
+                  wrench_gain="0.01"
+                  relaxation_time_ms="500"
+                  tip_link="grasp_link"
+                />
+                <Action
+                  ID="SwitchController"
+                  activate_controllers="joint_trajectory_admittance_controller"
+                />
+              </Control>
+            </Control>
+            <Control ID="Sequence" name="Release and Retract" _collapsed="true">
+              <Control ID="Sequence">
+                <Control ID="Sequence">
+                  <Control ID="Sequence">
+                    <!--Release the bottle-->
+                    <SubTree ID="Open Gripper" _collapsed="true" />
+                    <!--Retract upward after releasing the bottle-->
+                    <Action
+                      ID="CreateStampedPose"
+                      reference_frame="grasp_link"
+                      stamped_pose="{place_retract_pose}"
+                      position_xyz="0;0;-0.08"
+                    />
+                    <Action
+                      ID="ResetPoseStampedVector"
+                      vector="{place_retract_path}"
+                    />
+                    <Action
+                      ID="AddPoseStampedToVector"
+                      input="{place_retract_pose}"
+                      vector="{place_retract_path}"
+                    />
+                    <Action
+                      ID="PlanCartesianPath"
+                      acceleration_scale_factor="1.000000"
+                      blending_radius="0.020000"
+                      debug_solution="{debug_solution}"
+                      ik_cartesian_space_density="0.010000"
+                      ik_joint_space_density="0.100000"
+                      joint_trajectory_msg="{joint_trajectory_msg}"
+                      path="{place_retract_path}"
+                      planning_group_name="manipulator"
+                      position_only="true"
+                      tip_links="grasp_link"
+                      trajectory_sampling_rate="100"
+                      velocity_scale_factor="1.000000"
+                    />
+                    <Action ID="ExecuteTrajectory" />
+                    <!--Remove the used pose so the next iteration gets a fresh position-->
+                    <Action
+                      ID="RemoveFromVector"
+                      input_vector="{place_positions}"
+                      index="0"
+                      output_vector="{place_positions}"
+                    />
+                  </Control>
+                </Control>
+              </Control>
+            </Control>
+          </Control>
+        </Decorator>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Pick All Bottles with AprilTags">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="AprilTag" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>
diff --git a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
index 83d262b31..3aa7e5548 100644
--- a/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
+++ b/src/lab_sim/objectives/pick_april_tag_object_with_approval.xml
@@ -41,7 +41,7 @@
         output_pose="{view_pose}"
         input_pose="{detection_pose}"
         quaternion_xyzw="1;0;0;0"
-        translation_xyz="0;0.1;0.15"
+        translation_xyz="0;0.1;0.25"
       />
       <SubTree
         ID="Plan Move To Pose"
@@ -72,7 +72,7 @@
       />
       <SubTree
         ID="Pick from Pose With Approval"
-        _collapsed="false"
+        _collapsed="true"
         grasp_pose="{grasp_pose}"
         approach_distance="0.1"
         retract_xyz="0;0;-0.1"
diff --git a/src/lab_sim/objectives/pick_object.xml b/src/lab_sim/objectives/pick_object.xml
index 3fbce5948..8485d6f1d 100644
--- a/src/lab_sim/objectives/pick_object.xml
+++ b/src/lab_sim/objectives/pick_object.xml
@@ -12,7 +12,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;1;0;0"
-        position_xyz="0.01;.75;0.515"
+        position_xyz="-0.04;.58;0.57"
         stamped_pose="{stamped_pose}"
         reference_frame="world"
       />
diff --git a/src/lab_sim/objectives/pick_up_cube.xml b/src/lab_sim/objectives/pick_up_cube.xml
index 3186a75e9..03903efd1 100644
--- a/src/lab_sim/objectives/pick_up_cube.xml
+++ b/src/lab_sim/objectives/pick_up_cube.xml
@@ -8,10 +8,10 @@
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Clear Snapshot" _collapsed="true" />
-      <SubTree ID="Look at Table" _collapsed="false" />
+      <SubTree ID="Look at Table" _collapsed="true" />
       <!--Take a snapshot so we have a visualization of the objects to use in grasp pose correction-->
       <SubTree ID="Take Wrist Camera Snapshot" _collapsed="true" />
-      <SubTree ID="Pick object" _collapsed="false" />
+      <SubTree ID="Pick object" _collapsed="true" />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/lab_sim/objectives/place_object.xml b/src/lab_sim/objectives/place_object.xml
index 12949c19a..b150071d4 100644
--- a/src/lab_sim/objectives/place_object.xml
+++ b/src/lab_sim/objectives/place_object.xml
@@ -1,6 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Place Object">
-  <!--//////////-->
   <BehaviorTree
     ID="Place Object"
     _description="Place an object in the gripper at a manually specified location"
@@ -12,7 +11,7 @@
         <Action
           ID="CreateStampedPose"
           orientation_xyzw="0;1;0;0"
-          position_xyz="0.01;.75;0.5.15"
+          position_xyz="-0.15905; 0.51283; 0.55"
           stamped_pose="{stamped_pose}"
           reference_frame="world"
         />
@@ -32,8 +31,8 @@
   <TreeNodesModel>
     <SubTree ID="Place Object">
       <MetadataFields>
-        <Metadata subcategory="Application - Basic Examples" />
         <Metadata runnable="true" />
+        <Metadata subcategory="Manipulation" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index 6b33e6706..a760fbdfd 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -1,6 +1,5 @@
-<?xml version="1.0" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Register CAD Part">
-  <!--//////////-->
   <BehaviorTree
     ID="Register CAD Part"
     _description="Load an STL from disk and use registration to align the pointcloud with a matching shape in the environment"
@@ -12,7 +11,7 @@
       <Action
         ID="CreateStampedPose"
         reference_frame="world"
-        position_xyz="0.02;.75;0.58"
+        position_xyz="-1.0;.5;0.7"
         orientation_xyzw="0;0;0;1"
         stamped_pose="{stamped_pose}"
         name="Initial Guess"
@@ -21,7 +20,7 @@
         ID="LoadPointCloudFromFile"
         color="255;0;0"
         package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
+        file_path="description/assets/microscope.stl"
         point_cloud="{red_cloud}"
       />
       <Action
@@ -36,11 +35,12 @@
         output_cloud="{red_cloud}"
       />
       <Action ID="SendPointCloudToUI" point_cloud="{red_cloud}" />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_camera/points"
-        message_out="{wrist_point_cloud}"
+      <SubTree
+        ID="Move to Waypoint"
+        _collapsed="true"
+        waypoint_name="Look at Microscope"
       />
+      <Action ID="GetPointCloud" message_out="{wrist_point_cloud}" />
       <Action
         ID="TransformPointCloudFrame"
         input_cloud="{wrist_point_cloud}"
@@ -58,7 +58,7 @@
         ID="LoadPointCloudFromFile"
         color="0;255;0"
         package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
+        file_path="description/assets/microscope.stl"
         point_cloud="{green_cloud}"
       />
       <Action
@@ -84,7 +84,8 @@
   <TreeNodesModel>
     <SubTree ID="Register CAD Part">
       <MetadataFields>
-        <Metadata subcategory="Application - Advanced Examples" />
+        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud" />
       </MetadataFields>
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/objectives/register_cad_part_subtree.xml b/src/lab_sim/objectives/register_cad_part_subtree.xml
index cdb43a48f..5ce242657 100644
--- a/src/lab_sim/objectives/register_cad_part_subtree.xml
+++ b/src/lab_sim/objectives/register_cad_part_subtree.xml
@@ -1,6 +1,5 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Register CAD Part Subtree">
-  <!--//////////-->
   <BehaviorTree
     ID="Register CAD Part Subtree"
     _description="Load an STL from disk and use registration to align the pointcloud with a matching shape in the environment. Return the pose of the object in the world frame."
@@ -11,7 +10,7 @@
       <SubTree ID="Look at Table" _collapsed="true" />
       <Action
         ID="CreateStampedPose"
-        position_xyz="0.2;0.75;0.6"
+        position_xyz="-0.15;.5;0.7"
         orientation_xyzw="0;0;0;1"
         stamped_pose="{stamped_pose}"
         name="Initial Guess"
@@ -21,7 +20,7 @@
         ID="LoadPointCloudFromFile"
         color="255;0;0"
         package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
+        file_path="description/assets/bottle.stl"
         point_cloud="{red_cloud}"
       />
       <Action
@@ -35,17 +34,13 @@
         transform_pose="{stamped_pose}"
         output_cloud="{red_cloud}"
       />
-      <Action
-        ID="SendPointCloudToUI"
-        point_cloud="{red_cloud}"
-        pcd_topic="/pcd_pointcloud_captures"
-      />
-      <Action
-        ID="GetPointCloud"
-        topic_name="/wrist_camera/points"
-        message_out="{wrist_point_cloud}"
-        publisher_timeout_sec="5.000000"
-        message_timeout_sec="5.000000"
+      <SubTree
+        ID="Fuse Multiple Views Subtree"
+        _collapsed="true"
+        merged_cloud="{wrist_point_cloud}"
+        waypoint1="Look at Objects - Right"
+        waypoint2="Look at Objects - Top"
+        waypoint3="Look at Objects - Left"
       />
       <Action
         ID="TransformPointCloudFrame"
@@ -58,6 +53,11 @@
         point_cloud="{wrist_point_cloud}"
         pcd_topic="/pcd_pointcloud_captures"
       />
+      <Action
+        ID="SendPointCloudToUI"
+        point_cloud="{red_cloud}"
+        pcd_topic="/pcd_pointcloud_captures"
+      />
       <Action
         ID="RegisterPointClouds"
         target_point_cloud="{wrist_point_cloud}"
@@ -70,7 +70,7 @@
         ID="LoadPointCloudFromFile"
         color="0;255;0"
         package_name="lab_sim"
-        file_path="description/assets/Cube.stl"
+        file_path="description/assets/bottle.stl"
         point_cloud="{green_cloud}"
       />
       <Action
diff --git a/src/lab_sim/objectives/square_traj.yaml b/src/lab_sim/objectives/square_traj.yaml
index 22c573aac..ee629c1cc 100644
--- a/src/lab_sim/objectives/square_traj.yaml
+++ b/src/lab_sim/objectives/square_traj.yaml
@@ -13,21 +13,21 @@ joint_names:
   - wrist_1_joint
 points:
   - positions:
-      - 0.66661510412389058
-      - -0.018166145236789152
-      - -0.48600842659561388
-      - -2.2312025029230922
-      - 0.10077181577992345
-      - 0.10614107859737329
-      - -2.4610341822106103
+      - 1.5707696176332915
+      - -1.8862819411605685
+      - 1.1085169994644841
+      - -1.5708148497877352
+      - -7.0240255610305765e-08
+      - 0.00032577951410150925
+      - -0.74001807804968911
     velocities:
-      - 0.0040204555268496048
-      - 0.011674919513726036
-      - -0.010576750650883515
-      - -0.0043384246937160703
-      - 0.00065997965562653168
-      - 0.00031280602299771696
-      - -0.0079064188092546929
+      - 0.00054076415503849029
+      - 0.027291086621521327
+      - -0.013970744281599267
+      - 0.00023924550153468194
+      - 1.4191579364062033e-06
+      - -0.01027668032544142
+      - -0.01349508519402294
     accelerations:
       - 0
       - 0
@@ -41,21 +41,21 @@ points:
       sec: 0
       nanosec: 0
   - positions:
-      - 0.66556257418082654
-      - -0.019989477736440539
-      - -0.48519320062726429
-      - -2.2301945553060878
-      - 0.10053366848149191
-      - 0.10736119220116616
-      - -2.4571825394770648
+      - 1.5668727177980559
+      - -1.8856255426663566
+      - 1.1081843510176355
+      - -1.5710153500058226
+      - -0.0039090981362475902
+      - -0.00040673496319184005
+      - -0.74034591001311845
     velocities:
-      - -0.016959655180729886
-      - -0.067469761321244534
-      - 0.008267372661495874
-      - 0.016773025227106799
-      - -0.0037653699176076602
-      - 0.017639084188497418
-      - 0.059047525855116946
+      - -0.063569154011656456
+      - -0.00034535526338790246
+      - 0.00022787082237794637
+      - -0.0033820202811485726
+      - -0.063364009865911466
+      - -0.0087145483190840343
+      - 0.00010717744598068316
     accelerations:
       - 0
       - 0
@@ -67,23 +67,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 490212987
+      nanosec: 455085584
   - positions:
-      - 0.66463053682128814
-      - -0.023716742816971278
-      - -0.48475671408140708
-      - -2.2292764252210273
-      - 0.10032750553741153
-      - 0.10832619270736395
-      - -2.4539658619709401
+      - 1.5622903418463794
+      - -1.8856254562350547
+      - 1.108186532457452
+      - -1.5712581162387658
+      - -0.0084861069297345344
+      - -0.0010317557189444709
+      - -0.74035161290492246
     velocities:
-      - -0.023212991949810236
-      - -0.092920354130411609
-      - 0.010342070580406135
-      - 0.022661850840673767
-      - -0.0050874949811692662
-      - 0.024060159772345199
-      - 0.07892785094209083
+      - -0.098264877843298792
+      - 0.00014418771705484309
+      - -8.173735063038541e-05
+      - -0.0051695254036487406
+      - -0.098551043366166366
+      - -0.013322106164582679
+      - -0.0002376212487892599
     accelerations:
       - 0
       - 0
@@ -95,23 +95,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 538948883
+      nanosec: 518322584
   - positions:
-      - 0.66368314445572041
-      - -0.027486090350276143
-      - -0.48432112114836562
-      - -2.2283428446039482
-      - 0.10011810441688056
-      - 0.10929947485345941
-      - -2.4507413759740335
+      - 1.5577200980142609
+      - -1.8856093282321871
+      - 1.1081805719562328
+      - -1.5715001837177709
+      - -0.013049996632617711
+      - -0.0016500794402778145
+      - -0.74036508395616241
     velocities:
-      - -0.028942862485503991
-      - -0.11453704617712146
-      - 0.01333626448383554
-      - 0.028547337229298881
-      - -0.0063963672060596442
-      - 0.029638738986689773
-      - 0.097831920094532135
+      - -0.12476440938901392
+      - 0.00052164766386514566
+      - -0.00020059252264617984
+      - -0.0066312148752807627
+      - -0.12436433045645751
+      - -0.01704500579342914
+      - -0.00040327747054294148
     accelerations:
       - 0
       - 0
@@ -123,23 +123,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 575289253
+      nanosec: 561653667
   - positions:
-      - 0.66274557596411621
-      - -0.031175130862394772
-      - -0.48389630410460421
-      - -2.227420254365172
-      - 0.099911443335200273
-      - 0.11025985232900475
-      - -2.447606040771563
+      - 1.5528644365657105
+      - -1.885582943653261
+      - 1.1081681171748605
+      - -1.5717568237247985
+      - -0.017904232899144931
+      - -0.0023032286253952378
+      - -0.74038475924054203
     velocities:
-      - -0.033093049056101533
-      - -0.12961284526869327
-      - 0.014687283366225941
-      - 0.032438399277573957
-      - -0.0072627973911438826
-      - 0.03396313983850479
-      - 0.1094277621442212
+      - -0.16506251142004444
+      - 0.0012421187093069213
+      - -0.00063203265668313716
+      - -0.0086397578871388013
+      - -0.16588419812110178
+      - -0.02195384456649651
+      - -0.00087704105673882084
     accelerations:
       - 0
       - 0
@@ -151,23 +151,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 607213247
+      nanosec: 605053542
   - positions:
-      - 0.66182065255776457
-      - -0.03476386962608833
-      - -0.48347624574760845
-      - -2.2265091593278137
-      - 0.099707651277534254
-      - 0.11120150596520602
-      - -2.4445597251823714
+      - 1.5480866415655623
+      - -1.8855222844120494
+      - 1.1081397029228048
+      - -1.5720102364919932
+      - -0.022670795047217744
+      - -0.0029664242104394944
+      - -0.74042013893845304
     velocities:
-      - -0.037364730519922978
-      - -0.14411665959090392
-      - 0.016525405941943697
-      - 0.036565868864361931
-      - -0.0081762750502620673
-      - 0.03807585272935346
-      - 0.12139955682649853
+      - -0.17596873963653725
+      - 0.0027094212111631014
+      - -0.0014563386152910774
+      - -0.009334384562714652
+      - -0.17560664676593823
+      - -0.024781566531729259
+      - -0.0016304277119160444
     accelerations:
       - 0
       - 0
@@ -179,23 +179,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 633391000
+      nanosec: 635055001
   - positions:
-      - 0.66086640252513285
-      - -0.038414025772051642
-      - -0.4830435443306979
-      - -2.2255693647418
-      - 0.099497746655709526
-      - 0.11216852769513484
-      - -2.4414678471568294
+      - 1.5433000913721118
+      - -1.8854223170756572
+      - 1.1080871805065309
+      - -1.5722637117418066
+      - -0.027451153139129301
+      - -0.0036460985012052266
+      - -0.74047709729451427
     velocities:
-      - -0.041606652481534549
-      - -0.15801252316916276
-      - 0.018348284182981397
-      - 0.040637576974767546
-      - -0.0090721694582381537
-      - 0.042163703053564171
-      - 0.13274728379580292
+      - -0.19399593667308529
+      - 0.0051428747331520525
+      - -0.0025987875917275909
+      - -0.010282960145978411
+      - -0.19357065795563419
+      - -0.027826017231179129
+      - -0.0027867398852668072
     accelerations:
       - 0
       - 0
@@ -207,23 +207,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 658879566
+      nanosec: 664970334
   - positions:
-      - 0.65989382441945266
-      - -0.042077610030018267
-      - -0.48260309941236307
-      - -2.2246123180137256
-      - 0.099284308685638145
-      - 0.11314923169142933
-      - -2.4383703971915134
+      - 1.5388679247702159
+      - -1.8852811472341025
+      - 1.1080155305160155
+      - -1.5724983502907308
+      - -0.031876896723675843
+      - -0.0042931819803163854
+      - -0.74055263045391639
     velocities:
-      - -0.045873075530288548
-      - -0.17185342132462461
-      - 0.020090156042997343
-      - 0.044752571855267584
-      - -0.009976160022768266
-      - 0.04641956377403126
-      - 0.14382804353281181
+      - -0.21002372723204879
+      - 0.0070208186987347843
+      - -0.0038863885615610665
+      - -0.010907142723597303
+      - -0.21216557137107114
+      - -0.030762838234720688
+      - -0.0040230126231908811
     accelerations:
       - 0
       - 0
@@ -235,23 +235,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 682045725
+      nanosec: 691582334
   - positions:
-      - 0.65894058936107469
-      - -0.045631024311629159
-      - -0.48217442520055137
-      - -2.2236731249819175
-      - 0.099075143566841481
-      - 0.11411310867233009
-      - -2.4353785060464492
+      - 1.5340225909723424
+      - -1.8850762187567431
+      - 1.1079076042785969
+      - -1.5727542951559594
+      - -0.03672083577971394
+      - -0.0050181413552447532
+      - -0.74066205732375479
     velocities:
-      - -0.049519512112747355
-      - -0.18347662329014114
-      - 0.022989917035114747
-      - 0.049302018534849168
-      - -0.010974281864585435
-      - 0.049824308283825836
-      - 0.15591248311188124
+      - -0.24060146263508131
+      - 0.011497052921340499
+      - -0.0063506496613079172
+      - -0.012701061889640004
+      - -0.24049727561560638
+      - -0.035742577907449551
+      - -0.0062136258944493457
     accelerations:
       - 0
       - 0
@@ -263,23 +263,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 702181570
+      nanosec: 722328251
   - positions:
-      - 0.65791421463998612
-      - -0.049402041710159217
-      - -0.48171263264449121
-      - -2.2226620459711595
-      - 0.098850343296883345
-      - 0.11514665397579128
-      - -2.4322092874073942
+      - 1.5289156569240359
+      - -1.8847828254858354
+      - 1.1077599050161222
+      - -1.57302436767642
+      - -0.041818365520333245
+      - -0.0058073006584313759
+      - -0.74081173461446581
     velocities:
-      - -0.053237163225235459
-      - -0.19410105105727948
-      - 0.024227389023689528
-      - 0.052548790452705586
-      - -0.011671023697399162
-      - 0.053368210750969446
-      - 0.16345013540302264
+      - -0.26329911972278247
+      - 0.016414989149074008
+      - -0.0082509331753050207
+      - -0.013895487318724954
+      - -0.26324748323700564
+      - -0.041967099454318778
+      - -0.0083862228097358743
     accelerations:
       - 0
       - 0
@@ -291,23 +291,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 723927244
+      nanosec: 746960459
   - positions:
-      - 0.65692563007050087
-      - -0.052983810923255707
-      - -0.48126920941839124
-      - -2.2216886997201604
-      - 0.098634255115682037
-      - 0.11613893851010998
-      - -2.4292066104708376
+      - 1.5242095624722916
+      - -1.8844681631282838
+      - 1.1075979686983506
+      - -1.5732740429896419
+      - -0.046511696424287735
+      - -0.0065798712710125832
+      - -0.74097368274396003
     velocities:
-      - -0.056598001814487979
-      - -0.20391085607516585
-      - 0.026007493029972684
-      - 0.056121455890734202
-      - -0.012445313675730591
-      - 0.056613938735634507
-      - 0.17188196123847641
+      - -0.26039817407431781
+      - 0.018385221097973459
+      - -0.0096012933753045187
+      - -0.013840962289371353
+      - -0.25948079204631497
+      - -0.043449074803099667
+      - -0.0095070586636420013
     accelerations:
       - 0
       - 0
@@ -319,23 +319,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 741962698
+      nanosec: 766918417
   - positions:
-      - 0.65587061525578561
-      - -0.056750262695530504
-      - -0.48079519954499739
-      - -2.2206503437058012
-      - 0.098404116817880263
-      - 0.11719279259800158
-      - -2.4260533654403864
+      - 1.5194600928643427
+      - -1.8841098727504868
+      - 1.1074069239409654
+      - -1.57352529404944
+      - -0.051257726083465099
+      - -0.0073756928044559538
+      - -0.74116092574726056
     velocities:
-      - -0.060218515613863349
-      - -0.21357296039287002
-      - 0.026275313620417448
-      - 0.058749388554014591
-      - -0.013015815889994436
-      - 0.060217155132503301
-      - 0.17722368136245711
+      - -0.27533099116896675
+      - 0.021885408099660664
+      - -0.012006674701775391
+      - -0.01444967521404238
+      - -0.27633486355673037
+      - -0.045701139850142007
+      - -0.011613602307870292
     accelerations:
       - 0
       - 0
@@ -347,23 +347,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 760206204
+      nanosec: 790194251
   - positions:
-      - 0.65487182839725078
-      - -0.06026987446190555
-      - -0.48034909149635141
-      - -2.2196681605988786
-      - 0.098186740759728122
-      - 0.11818899053048793
-      - -2.4231164830628549
+      - 1.5148469390888291
+      - -1.8837010307420088
+      - 1.1071920743712746
+      - -1.5737688589081174
+      - -0.05586744535721605
+      - -0.0081534441447162504
+      - -0.7413705019201684
     velocities:
-      - -0.063290825575518586
-      - -0.22229437975551075
-      - 0.027526891715018917
-      - 0.061860344988430481
-      - -0.01368503029506728
-      - 0.063450417896333419
-      - 0.18387100456404717
+      - -0.29851189981119258
+      - 0.02813981613439246
+      - -0.01469219832082724
+      - -0.015803449434792675
+      - -0.2977136913571049
+      - -0.050903384143111713
+      - -0.014294376222745707
     accelerations:
       - 0
       - 0
@@ -375,23 +375,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 777097690
+      nanosec: 808262459
   - positions:
-      - 0.65382049200051584
-      - -0.063930943208073215
-      - -0.47987886765280324
-      - -2.2186326028178196
-      - 0.097957924217295253
-      - 0.11923692406980586
-      - -2.4200669491226821
+      - 1.5106652183871567
+      - -1.883284942546686
+      - 1.1069773470217155
+      - -1.5739902895509008
+      - -0.060038556882855275
+      - -0.0088853033312910352
+      - -0.74158034607993117
     velocities:
-      - -0.066826592776958016
-      - -0.23155175270269621
-      - 0.029226475859677803
-      - 0.065428147196531047
-      - -0.014449863109849968
-      - 0.0667725331410845
-      - 0.19145205757219516
+      - -0.29951705190547312
+      - 0.031002224319305275
+      - -0.015931054520730435
+      - -0.01589870004914315
+      - -0.29835665654745125
+      - -0.053315119366065832
+      - -0.015557040272964503
     accelerations:
       - 0
       - 0
@@ -403,23 +403,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 793831421
+      nanosec: 825557751
   - positions:
-      - 0.65272050918101288
-      - -0.067714325148405793
-      - -0.47938744051456078
-      - -2.2175481710648062
-      - 0.097718711616012935
-      - 0.12033220746885119
-      - -2.4169234963084092
+      - 1.5062410247827316
+      - -1.8828025502605543
+      - 1.1067248773374556
+      - -1.5742240061920127
+      - -0.06445691401869634
+      - -0.0096749184473172902
+      - -0.74182428452070326
     velocities:
-      - -0.07043526895512929
-      - -0.24034548345493859
-      - 0.031111890333885468
-      - 0.069065066298833314
-      - -0.015221997480951125
-      - 0.069947640801708663
-      - 0.19881447737952898
+      - -0.31477219896229147
+      - 0.03586274336720368
+      - -0.018432849622722174
+      - -0.016706089303895506
+      - -0.31350807464649438
+      - -0.057471871374271538
+      - -0.017900142824866194
     accelerations:
       - 0
       - 0
@@ -431,23 +431,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 810844219
+      nanosec: 841898542
   - positions:
-      - 0.65156464993709795
-      - -0.071632224732274874
-      - -0.47887081515742791
-      - -2.2164085366708592
-      - 0.097467759103593718
-      - 0.12147840402942751
-      - -2.4136738348608424
+      - 1.5018195404229377
+      - -1.8822819113672911
+      - 1.1064517074106421
+      - -1.5744580421519792
+      - -0.068869246226341677
+      - -0.010492317731690045
+      - -0.74208698290281472
     velocities:
-      - -0.07446959208205163
-      - -0.25027149103231539
-      - 0.03377823313264755
-      - 0.073592246541122883
-      - -0.016185135244388017
-      - 0.073392845606183352
-      - 0.20822970370780111
+      - -0.31491277527533729
+      - 0.038364623814380473
+      - -0.020116134535220473
+      - -0.016709770765487152
+      - -0.31380894230681861
+      - -0.058933884161634906
+      - -0.019292391099264054
     accelerations:
       - 0
       - 0
@@ -459,23 +459,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 827219172
+      nanosec: 856933001
   - positions:
-      - 0.65049693259627095
-      - -0.075195883384443149
-      - -0.47839279789676625
-      - -2.2153565419981458
-      - 0.097236513834802113
-      - 0.12253116580984171
-      - -2.4107215416125838
+      - 1.4973622621251133
+      - -1.881716537891966
+      - 1.1061522779375945
+      - -1.5746938170596445
+      - -0.073318857297765647
+      - -0.011332009419422955
+      - -0.74237248395289623
     velocities:
-      - -0.078495821979276764
-      - -0.26002816500009102
-      - 0.035609037979201182
-      - 0.077513986853949821
-      - -0.017020389902888004
-      - 0.076988640625461027
-      - 0.21605117303455867
+      - -0.31625166037055363
+      - 0.04151417760442596
+      - -0.021835996004971228
+      - -0.016749807023365114
+      - -0.31546025649813808
+      - -0.060319068389123896
+      - -0.020843793239532525
     accelerations:
       - 0
       - 0
@@ -487,23 +487,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 840086030
+      nanosec: 873398626
   - positions:
-      - 0.64938262171887895
-      - -0.078871660771030666
-      - -0.47790162624193483
-      - -2.2142628294480482
-      - 0.096996456220052638
-      - 0.12363100342740622
-      - -2.4076967788486487
+      - 1.4928206159644797
+      - -1.881094697401126
+      - 1.1058209224506292
+      - -1.5749335392787494
+      - -0.077856815632394799
+      - -0.012199093637843867
+      - -0.74268621695933079
     velocities:
-      - -0.08037210057786999
-      - -0.26424637449052207
-      - 0.034779392096802343
-      - 0.078578786028470821
-      - -0.017240035180559752
-      - 0.079654961557560966
-      - 0.21605090031939109
+      - -0.32817922166381147
+      - 0.046595578983833347
+      - -0.024761290334357314
+      - -0.017345856504899712
+      - -0.32754274195342137
+      - -0.063096290923064374
+      - -0.023401495969992282
     accelerations:
       - 0
       - 0
@@ -515,23 +515,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 855383461
+      nanosec: 888312334
   - positions:
-      - 0.64822007848840457
-      - -0.08266089989855864
-      - -0.47738385117036602
-      - -2.2131169136937077
-      - 0.096745395121469416
-      - 0.12477605658236103
-      - -2.4045757171426239
+      - 1.4882343770342148
+      - -1.880416378831574
+      - 1.1054617212002247
+      - -1.5751758165893963
+      - -0.082435552207170693
+      - -0.013094331063824691
+      - -0.7430257322983993
     velocities:
-      - -0.084306910031584845
-      - -0.27323660718304821
-      - 0.036901981744702016
-      - 0.082592615572556954
-      - -0.018081618036433302
-      - 0.08310611424889025
-      - 0.22356677369866335
+      - -0.33064204220645094
+      - 0.050427297958426175
+      - -0.026761245165498611
+      - -0.017481787235335357
+      - -0.32986155817005364
+      - -0.064986635475792823
+      - -0.025205795958121877
     accelerations:
       - 0
       - 0
@@ -543,23 +543,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 870484135
+      nanosec: 906618751
   - positions:
-      - 0.64701041251571345
-      - -0.086554241054619618
-      - -0.47684584631122612
-      - -2.2119242535950749
-      - 0.096484550366594743
-      - 0.12596534312523291
-      - -2.4013770215648726
+      - 1.4840693453183729
+      - -1.8797529805064199
+      - 1.1051094135186792
+      - -1.5753952553968722
+      - -0.086596669220312908
+      - -0.01391433105504346
+      - -0.74335656547729789
     velocities:
-      - -0.088354181362141793
-      - -0.28237372973904457
-      - 0.039529147820054754
-      - 0.087138990623815055
-      - -0.019035983565646911
-      - 0.086547149321104008
-      - 0.23207821607910475
+      - -0.34611963004179724
+      - 0.05702868499100798
+      - -0.029636636293312476
+      - -0.018332487044256537
+      - -0.3446432916080937
+      - -0.069528982966102715
+      - -0.028019710295430027
     accelerations:
       - 0
       - 0
@@ -571,23 +571,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 883800470
+      nanosec: 919990542
   - positions:
-      - 0.64593314194864138
-      - -0.089977408006908718
-      - -0.47637017734319737
-      - -2.2108647021768362
-      - 0.096253210566793604
-      - 0.12702208462172362
-      - -2.3985765651672963
+      - 1.4798498733719725
+      - -1.879041371780495
+      - 1.1047340067133373
+      - -1.5756178833007086
+      - -0.090808772678601643
+      - -0.014770227837316225
+      - -0.74370889847144672
     velocities:
-      - -0.090763588032784573
-      - -0.28720955551944294
-      - 0.039591691221278313
-      - 0.088981795077267667
-      - -0.019418632421988425
-      - 0.089146672923779174
-      - 0.23391582990971127
+      - -0.35135202204762372
+      - 0.060604518296312797
+      - -0.031887321785338561
+      - -0.018591629002070918
+      - -0.35014336153732867
+      - -0.072199199085574595
+      - -0.029885501784674758
     accelerations:
       - 0
       - 0
@@ -599,23 +599,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 897146831
+      nanosec: 933612876
   - positions:
-      - 0.64482499871483978
-      - -0.093464141867754175
-      - -0.47587957631395611
-      - -2.209771783797581
-      - 0.09601496624322671
-      - 0.12810879931083183
-      - -2.3957276077822143
+      - 1.4741911270135064
+      - -1.8780364898471054
+      - 1.1041981613796983
+      - -1.5759161417633445
+      - -0.096462403875423153
+      - -0.015946195083181996
+      - -0.74420732119891719
     velocities:
-      - -0.094146532656416634
-      - -0.29463857662234011
-      - 0.04278632328298123
-      - 0.093718597113936208
-      - -0.020414967251587993
-      - 0.091868196435466926
-      - 0.24272830942878412
+      - -0.36526853725054009
+      - 0.0655720344982227
+      - -0.036178553068519206
+      - -0.0189273998955822
+      - -0.36871363476550434
+      - -0.075305392964289536
+      - -0.033249112030361364
     accelerations:
       - 0
       - 0
@@ -627,23 +627,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 908804279
+      nanosec: 952520792
   - positions:
-      - 0.6436697832342575
-      - -0.097046155068052684
-      - -0.47536691382467511
-      - -2.2086339128233545
-      - 0.095767396813624839
-      - 0.12923428809867279
-      - -2.3928042063053154
+      - 1.4702653204971166
+      - -1.8772916289546946
+      - 1.103794233597907
+      - -1.576122151311659
+      - -0.10038841114084573
+      - -0.016755015141570042
+      - -0.744576714777471
     velocities:
-      - -0.097671776800922608
-      - -0.30120686540754082
-      - 0.042850843072200273
-      - 0.095803505437781947
-      - -0.02083051609602837
-      - 0.09513318670089542
-      - 0.24471909741700887
+      - -0.39320397008134367
+      - 0.077906168037138443
+      - -0.04035449299534024
+      - -0.02091910430512401
+      - -0.38966435436932556
+      - -0.08321012227169243
+      - -0.037426835297432147
     accelerations:
       - 0
       - 0
@@ -655,23 +655,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 922376843
+      nanosec: 968271542
   - positions:
-      - 0.6424839757140498
-      - -0.10068681648662954
-      - -0.47484676933302983
-      - -2.2074679514556967
-      - 0.09551410315603337
-      - 0.13039179966332309
-      - -2.3898503218086162
+      - 1.4654829338540976
+      - -1.8763284962373592
+      - 1.1032867371461625
+      - -1.5763741642964173
+      - -0.10515938912425075
+      - -0.017785663031682435
+      - -0.74504488526048307
     velocities:
-      - -0.099609366708907632
-      - -0.30448735964398221
-      - 0.042981265857162249
-      - 0.097437936602026348
-      - -0.021154865324155347
-      - 0.097413867412552452
-      - 0.24550303486276748
+      - -0.39903997547856684
+      - 0.081244717436176173
+      - -0.043376941937096083
+      - -0.020953387078053637
+      - -0.39917525626101868
+      - -0.087105122338277871
+      - -0.039789542325950063
     accelerations:
       - 0
       - 0
@@ -683,23 +683,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 933543738
+      nanosec: 980080084
   - positions:
-      - 0.64125826354049964
-      - -0.10440455938807063
-      - -0.47430474366891628
-      - -2.206258962875999
-      - 0.095251936507095986
-      - 0.13158409300417187
-      - -2.3868320197741033
+      - 1.4614907469062597
+      - -1.8754959313644972
+      - 1.1028413407933411
+      - -1.5765848104534244
+      - -0.10914141695376647
+      - -0.018669637427549155
+      - -0.74545132872675046
     velocities:
-      - -0.10354823797821054
-      - -0.31287145023976282
-      - 0.04680037545667888
-      - 0.10309712035432467
-      - -0.022340761690951855
-      - 0.10050062892285143
-      - 0.25571654238987285
+      - -0.39465781786883469
+      - 0.083625880630139524
+      - -0.044509705255840894
+      - -0.020871253872103996
+      - -0.39318698351449949
+      - -0.088604411020574564
+      - -0.040647897862610277
     accelerations:
       - 0
       - 0
@@ -711,23 +711,23 @@ points:
     effort: []
     time_from_start:
       sec: 0
-      nanosec: 947284785
+      nanosec: 991623626
   - positions:
-      - 0.63998280301527677
-      - -0.10821646136056177
-      - -0.47373924108935078
-      - -2.2050023209866194
-      - 0.094979995822539465
-      - 0.13281696183878944
-      - -2.3837413983594287
+      - 1.4574727363085809
+      - -1.8746298008484337
+      - 1.1023722141739127
+      - -1.5767963907451574
+      - -0.11315450645626912
+      - -0.019571395482387729
+      - -0.74587560667054598
     velocities:
-      - -0.10763131097397359
-      - -0.32001856853132576
-      - 0.046784973715907938
-      - 0.10539286099030058
-      - -0.022799012412567606
-      - 0.10416551170411793
-      - 0.25772970463344413
+      - -0.40019345272814644
+      - 0.087934580203671481
+      - -0.047267542953179122
+      - -0.021150631775759238
+      - -0.39875214191493791
+      - -0.090705052100697015
+      - -0.042774500310411488
     accelerations:
       - 0
       - 0
@@ -738,24 +738,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 0
-      nanosec: 959026739
+      sec: 1
+      nanosec: 1637501
   - positions:
-      - 0.63864836197362662
-      - -0.11214850648071016
-      - -0.47314682969565069
-      - -2.2036874469916823
-      - 0.094695992367778842
-      - 0.13410074507675449
-      - -2.3805575062108177
+      - 1.4534781448664371
+      - -1.8737368845989235
+      - 1.1018892221345824
+      - -1.5770069872745607
+      - -0.11714104381568065
+      - -0.020482884808909321
+      - -0.74631092020924428
     velocities:
-      - -0.11292431133686447
-      - -0.33091161505876782
-      - 0.049344605024294268
-      - 0.11064126191840117
-      - -0.023881059447638797
-      - 0.108734582141344
-      - 0.26628350877859258
+      - -0.39588582430157876
+      - 0.089818829893944074
+      - -0.048382408177754611
+      - -0.02091603205349931
+      - -0.39457045869299778
+      - -0.091130014979090637
+      - -0.043604050043248244
     accelerations:
       - 0
       - 0
@@ -766,24 +766,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 0
-      nanosec: 970481685
+      sec: 1
+      nanosec: 13538001
   - positions:
-      - 0.63751090420704837
-      - -0.11547329153605686
-      - -0.47264872909439387
-      - -2.202568424940357
-      - 0.094454680225565343
-      - 0.13520054171550336
-      - -2.3778846685126878
+      - 1.4494519881926573
+      - -1.8728091370037547
+      - 1.1013824888196753
+      - -1.5772186302560525
+      - -0.12116507469678002
+      - -0.021406455428015114
+      - -0.74676395870444168
     velocities:
-      - -0.11535503521627014
-      - -0.33547156713944165
-      - 0.050848818213963129
-      - 0.11362467457895072
-      - -0.024479857969025243
-      - 0.1112613247319208
-      - 0.26994250335400077
+      - -0.40525427596892272
+      - 0.094692840479446122
+      - -0.051809471223666199
+      - -0.021315263133936828
+      - -0.40478055164599469
+      - -0.093096345999388916
+      - -0.046170091951326653
     accelerations:
       - 0
       - 0
@@ -794,24 +794,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 0
-      nanosec: 981983354
+      sec: 1
+      nanosec: 23401459
   - positions:
-      - 0.63634439680717025
-      - -0.11884782959687858
-      - -0.47213882637392218
-      - -2.2014213102503986
-      - 0.094207744740801458
-      - 0.13632728722916398
-      - -2.3751798404913371
+      - 1.4454296924330572
+      - -1.8718492225210386
+      - 1.100860092232411
+      - -1.577430396338197
+      - -0.12517983919571068
+      - -0.02233991024997508
+      - -0.74722934167126631
     velocities:
-      - -0.11723889440694678
-      - -0.3380068851624502
-      - 0.050450438274166655
-      - 0.1147339270156839
-      - -0.024685840964551682
-      - 0.11350923462180412
-      - 0.26926928141589451
+      - -0.40157626600762186
+      - 0.097210308008220961
+      - -0.052787325486975775
+      - -0.021178247666207739
+      - -0.40033709349518676
+      - -0.093688917190115475
+      - -0.046953141854425652
     accelerations:
       - 0
       - 0
@@ -822,24 +822,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 0
-      nanosec: 991741922
+      sec: 1
+      nanosec: 34942126
   - positions:
-      - 0.63514218773688969
-      - -0.12228656558558589
-      - -0.47160746943335047
-      - -2.2002341473259359
-      - 0.093952658975807374
-      - 0.13748364017442744
-      - -2.3724185939300395
+      - 1.4412304373790701
+      - -1.8708112426448551
+      - 1.1002893614221265
+      - -1.5776504004113781
+      - -0.12937933769280943
+      - -0.023311471868540164
+      - -0.74773231844030807
     velocities:
-      - -0.12188367057809554
-      - -0.34717515734645793
-      - 0.054736054267540642
-      - 0.12118670543392718
-      - -0.026022278104224816
-      - 0.11690330327121952
-      - 0.28022104142255638
+      - -0.42225620286748189
+      - 0.10648147494657609
+      - -0.057988690529722435
+      - -0.022207468658802847
+      - -0.42112266778050167
+      - -0.098581343911220934
+      - -0.051133195972088916
     accelerations:
       - 0
       - 0
@@ -851,23 +851,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 1662277
+      nanosec: 46659168
   - positions:
-      - 0.63391108642035987
-      - -0.1257708901644545
-      - -0.47106901647616989
-      - -2.1990224096746176
-      - 0.093692745806924382
-      - 0.13866732405037543
-      - -2.3696371176788511
+      - 1.4370319912352061
+      - -1.8697317122240709
+      - 1.0997021546920414
+      - -1.5778711259464078
+      - -0.13356806920071812
+      - -0.024305886950597128
+      - -0.74824896208784786
     velocities:
-      - -0.12433483537402147
-      - -0.35094799327098741
-      - 0.055601569866535243
-      - 0.12367878787545371
-      - -0.026512510013172667
-      - 0.11935328188588731
-      - 0.28220842260043466
+      - -0.41862525758983482
+      - 0.10919545063483567
+      - -0.059301902405882401
+      - -0.022059618480391181
+      - -0.4170461200440499
+      - -0.10006164086794268
+      - -0.052056818846120963
     accelerations:
       - 0
       - 0
@@ -879,23 +879,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 12085468
+      nanosec: 56916001
   - positions:
-      - 0.63265543897652587
-      - -0.12929045249123938
-      - -0.47051952353665843
-      - -2.1977848120712551
-      - 0.093427774069330588
-      - 0.1398738953357814
-      - -2.3668341073246872
+      - 1.4327536957810116
+      - -1.8685979536485606
+      - 1.0990757533721209
+      - -1.5780950858679361
+      - -0.13784638991026826
+      - -0.02532718707958578
+      - -0.74879288267721633
     velocities:
-      - -0.12701121324865905
-      - -0.35474701294856498
-      - 0.05704016416731187
-      - 0.12658312315309109
-      - -0.02708110734142884
-      - 0.12163772295443882
-      - 0.28494154114075299
+      - -0.43771498781278761
+      - 0.1177616354680933
+      - -0.065456942064184231
+      - -0.022845585449681637
+      - -0.43829367787099316
+      - -0.10435758702502546
+      - -0.056506646082931954
     accelerations:
       - 0
       - 0
@@ -907,23 +907,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 22568967
+      nanosec: 70337251
   - positions:
-      - 0.63136229798760335
-      - -0.13286806345527452
-      - -0.46994898290127018
-      - -2.1965083061418946
-      - 0.093155042593610363
-      - 0.14110855941367889
-      - -2.3639831980167392
+      - 1.428404165874567
+      - -1.8673954693440571
+      - 1.0984163563086837
+      - -1.5783234024510773
+      - -0.1421855432268877
+      - -0.026385016834679819
+      - -0.74936271032348434
     velocities:
-      - -0.13124529741052551
-      - -0.36090650711041966
-      - 0.058008230916581575
-      - 0.12943362719858775
-      - -0.027625865917087582
-      - 0.12487151341227709
-      - 0.28742946704611044
+      - -0.43471575587500333
+      - 0.1212556951485244
+      - -0.066916161067054239
+      - -0.022691874841967594
+      - -0.43528858099050799
+      - -0.10644763856499762
+      - -0.057642562217177576
     accelerations:
       - 0
       - 0
@@ -935,23 +935,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 33817531
+      nanosec: 81584626
   - positions:
-      - 0.63002134133389098
-      - -0.13652534367672758
-      - -0.46935971340957111
-      - -2.195188672531776
-      - 0.09287362625876755
-      - 0.14238081622066712
-      - -2.3610773820137205
+      - 1.4248459883551843
+      - -1.8663882637376739
+      - 1.0978540062071112
+      - -1.5785090976485809
+      - -0.14574713397674083
+      - -0.027255332431029893
+      - -0.74984243583215859
     velocities:
-      - -0.13500206497266268
-      - -0.36685847934087318
-      - 0.058155870812554085
-      - 0.13203181660123064
-      - -0.028144765119102175
-      - 0.12844678494003375
-      - 0.28929162381995971
+      - -0.45869840763238096
+      - 0.13067815616528741
+      - -0.074012136518086857
+      - -0.023824505069238285
+      - -0.46025267171734285
+      - -0.11125787311528545
+      - -0.062610161330671044
     accelerations:
       - 0
       - 0
@@ -963,23 +963,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 43909341
+      nanosec: 91720584
   - positions:
-      - 0.62865278612792053
-      - -0.14022572257766719
-      - -0.46876353811763144
-      - -2.1938428855027814
-      - 0.092587143070850988
-      - 0.14368269985306373
-      - -2.3581540627429316
+      - 1.4211951740132951
+      - -1.8653201559137096
+      - 1.097263870477905
+      - -1.578700420601348
+      - -0.14938897288219988
+      - -0.028159769687795412
+      - -0.75034485514971039
     velocities:
-      - -0.13731969862112883
-      - -0.37011227468970476
-      - 0.05886957204759926
-      - 0.1343524261323949
-      - -0.028583228993957269
-      - 0.13104063988789119
-      - 0.29041534632944782
+      - -0.45807625089593978
+      - 0.13479819977526919
+      - -0.074668651139730965
+      - -0.02390365761623274
+      - -0.45821815238197056
+      - -0.11404952751233782
+      - -0.063503531565496096
     accelerations:
       - 0
       - 0
@@ -991,23 +991,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 54017213
+      nanosec: 100064251
   - positions:
-      - 0.62724816841619413
-      - -0.14398155351005337
-      - -0.46815009654776957
-      - -2.1924599944600676
-      - 0.09229336302409144
-      - 0.14501596087855159
-      - -2.3551919232685279
+      - 1.417535751095206
+      - -1.8642296652139032
+      - 1.0966597985756736
+      - -1.5788920741834604
+      - -0.15304166544983386
+      - -0.029080697978654649
+      - -0.75085690641668812
     velocities:
-      - -0.14033522307748378
-      - -0.37450412077614009
-      - 0.060086103589804866
-      - 0.13745546945006687
-      - -0.029190109773793778
-      - 0.13389106016179786
-      - 0.29304937714550544
+      - -0.45963603125856856
+      - 0.13788170207374062
+      - -0.076614404918942794
+      - -0.024098975179868816
+      - -0.4584502389858825
+      - -0.11603120733384691
+      - -0.064721124567167707
     accelerations:
       - 0
       - 0
@@ -1019,23 +1019,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 63779625
+      nanosec: 108536834
   - positions:
-      - 0.62581028155697693
-      - -0.14778582379813684
-      - -0.46751611153739314
-      - -2.1910383630797874
-      - 0.091992007169774195
-      - 0.14637705675308563
-      - -2.3521897207077829
+      - 1.4138339593232709
+      - -1.8631044161705252
+      - 1.0960345685155493
+      - -1.5790860545019556
+      - -0.15673471696384289
+      - -0.030021597135083261
+      - -0.75138362091429267
     velocities:
-      - -0.14599621467895865
-      - -0.38372403080463013
-      - 0.064366114241674072
-      - 0.14402687186459265
-      - -0.030496987935570704
-      - 0.13766645546625758
-      - 0.30241509545240669
+      - -0.45644755237148704
+      - 0.1401353576966923
+      - -0.077227411768088089
+      - -0.02400378537351619
+      - -0.45444399372240241
+      - -0.11744115711353349
+      - -0.065182866264831121
     accelerations:
       - 0
       - 0
@@ -1047,23 +1047,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 73529128
+      nanosec: 118518626
   - positions:
-      - 0.62434678053536175
-      - -0.15161820723398309
-      - -0.4668804376893238
-      - -2.1895982709606914
-      - 0.091687285736366864
-      - 0.1477633229019345
-      - -2.3491889239977848
+      - 1.410122735231945
+      - -1.861958412848173
+      - 1.095391972335455
+      - -1.5792799650106439
+      - -0.16044418214089468
+      - -0.030973001659112289
+      - -0.75192059137045741
     velocities:
-      - -0.14777378339417616
-      - -0.38524808955828094
-      - 0.064833509591950611
-      - 0.14587852956445718
-      - -0.03083090134661589
-      - 0.13965099966666053
-      - 0.30219795280394907
+      - -0.46700377216663103
+      - 0.14495215412196294
+      - -0.081817749511645832
+      - -0.024320554669134884
+      - -0.46769811590107208
+      - -0.11967993283683925
+      - -0.068069124616842933
     accelerations:
       - 0
       - 0
@@ -1075,23 +1075,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 85225264
+      nanosec: 126612751
   - positions:
-      - 0.62284973539913024
-      - -0.15549206080206537
-      - -0.4662285281882787
-      - -2.1881245337565747
-      - 0.091376120884608739
-      - 0.14917565344291417
-      - -2.3461609826686196
+      - 1.4063779260290614
+      - -1.8607806062882921
+      - 1.0947256170494095
+      - -1.5794751286186155
+      - -0.16419203440024682
+      - -0.031937173107508411
+      - -0.75247212811269193
     velocities:
-      - -0.14992169764607591
-      - -0.38664948777293062
-      - 0.064010188090733569
-      - 0.14663566031626624
-      - -0.030945819197510806
-      - 0.14187132360639459
-      - 0.29981907779551492
+      - -0.48801907009866496
+      - 0.15294566919301439
+      - -0.089306051796213981
+      - -0.024982334751349883
+      - -0.49359375125508348
+      - -0.12316635973062259
+      - -0.072899872904353208
     accelerations:
       - 0
       - 0
@@ -1103,23 +1103,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 95548673
+      nanosec: 138409334
   - positions:
-      - 0.62134265848778181
-      - -0.1593613593254104
-      - -0.4655769192506688
-      - -2.1866409303431031
-      - 0.091063457403998835
-      - 0.15060119417762402
-      - -2.343153466528225
+      - 1.4024426520426134
+      - -1.8595055733511736
+      - 1.0940026975446957
+      - -1.5796804982958987
+      - -0.16812087017161687
+      - -0.032947611446594006
+      - -0.75306399722778961
     velocities:
-      - -0.15042262604722659
-      - -0.38503765820100966
-      - 0.064146305904252993
-      - 0.14744322410346691
-      - -0.031054526423388407
-      - 0.14266260703038403
-      - 0.29744063267223808
+      - -0.49322072184566834
+      - 0.16195817413348626
+      - -0.091009452020578058
+      - -0.025799849679656324
+      - -0.49156229844234728
+      - -0.12779869663823895
+      - -0.074590040791038953
     accelerations:
       - 0
       - 0
@@ -1131,23 +1131,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 105316869
+      nanosec: 146680668
   - positions:
-      - 0.61981247951289364
-      - -0.16324515992422192
-      - -0.46490812403568976
-      - -2.1851284241209235
-      - 0.090745419071826761
-      - 0.15204104509645736
-      - -2.3401309222929174
+      - 1.3984976492692294
+      - -1.8581932713907081
+      - 1.093264704863262
+      - -1.5798865960775492
+      - -0.1720562799700194
+      - -0.03398165371993813
+      - -0.75366666845659092
     velocities:
-      - -0.15400584740916967
-      - -0.38927680348454047
-      - 0.068026202593413646
-      - 0.15304193729906179
-      - -0.032156961030483149
-      - 0.14454077934499895
-      - 0.30400222545999211
+      - -0.49157286968807523
+      - 0.1650021458068564
+      - -0.09259523011533359
+      - -0.02574709346004252
+      - -0.48960882722001436
+      - -0.12992321861436917
+      - -0.075479454558183792
     accelerations:
       - 0
       - 0
@@ -1159,23 +1159,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 115373498
+      nanosec: 155204168
   - positions:
-      - 0.61826874357420758
-      - -0.16712085757647499
-      - -0.46423778466185572
-      - -2.1836063354292148
-      - 0.090426040673292171
-      - 0.15349018884229595
-      - -2.3371305019252868
+      - 1.3945794206763316
+      - -1.8568661565061353
+      - 1.0925146000280068
+      - -1.5800913548260438
+      - -0.17596527077952159
+      - -0.035024987849618348
+      - -0.75427430193471179
     velocities:
-      - -0.1550363170418643
-      - -0.3874438606834652
-      - 0.067441441598125401
-      - 0.15289279207995049
-      - -0.032048708272149014
-      - 0.14525645194329581
-      - 0.29970826367898057
+      - -0.48653212582584487
+      - 0.16609709522782284
+      - -0.093770379126582273
+      - -0.025482936898947327
+      - -0.48472476607015785
+      - -0.13046088827517624
+      - -0.075803238231846856
     accelerations:
       - 0
       - 0
@@ -1187,23 +1187,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 125411192
+      nanosec: 165133084
   - positions:
-      - 0.61672069250146122
-      - -0.17096978843136473
-      - -0.46356691686301216
-      - -2.1820796511706249
-      - 0.090106347462056674
-      - 0.15494086963725062
-      - -2.3341613204898137
+      - 1.3887529553296674
+      - -1.8548476506695419
+      - 1.0913676389524007
+      - -1.5803960985535517
+      - -0.1817744773892688
+      - -0.036597348101470834
+      - -0.75519397231274032
     velocities:
-      - -0.15543504180290191
-      - -0.38453129744260045
-      - 0.067535760464794231
-      - 0.1533578219924045
-      - -0.032084292033782158
-      - 0.14525070914073451
-      - 0.29661121810050256
+      - -0.46606755454404059
+      - 0.16394950508248601
+      - -0.091755669630620013
+      - -0.024609950907277071
+      - -0.46213506185932934
+      - -0.12861481404902514
+      - -0.073794642233598232
     accelerations:
       - 0
       - 0
@@ -1215,23 +1215,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 137201677
+      nanosec: 179480001
   - positions:
-      - 0.61516774236508709
-      - -0.17478939311063627
-      - -0.46289500231444786
-      - -2.1805492255239214
-      - 0.089786540092115327
-      - 0.15639067330205739
-      - -2.3312243509052264
+      - 1.3848550142008993
+      - -1.8534840539370538
+      - 1.090567867727428
+      - -1.5805977345757876
+      - -0.18568700346236849
+      - -0.037640957462249679
+      - -0.75582226362307836
     velocities:
-      - -0.15489468729981257
-      - -0.37953955078395846
-      - 0.066331020908587127
-      - 0.15208188402880915
-      - -0.031759264880409262
-      - 0.14468198710754193
-      - 0.29042887993667632
+      - -0.50610872024374909
+      - 0.17944045680477114
+      - -0.10535957205772527
+      - -0.02633212580718049
+      - -0.50539967730243363
+      - -0.13393716443107118
+      - -0.082154301968227109
     accelerations:
       - 0
       - 0
@@ -1243,23 +1243,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 147317340
+      nanosec: 186612418
   - positions:
-      - 0.61362351434285922
-      - -0.17855404388048787
-      - -0.46222791620667697
-      - -2.179026539321705
-      - 0.089468982881219128
-      - 0.15783010015956073
-      - -2.3283396569378834
+      - 1.3807940288252423
+      - -1.8520130027926731
+      - 1.0897198848123484
+      - -1.5808090237934518
+      - -0.18973962025417895
+      - -0.038729692604606458
+      - -0.75648613524886676
     velocities:
-      - -0.15366027333409957
-      - -0.37320573512152466
-      - 0.065695905709283353
-      - 0.15091033793237851
-      - -0.031451323600447355
-      - 0.14328591554719364
-      - 0.28455513540901878
+      - -0.50680771087935828
+      - 0.18550297704821522
+      - -0.10618120667116336
+      - -0.026415938899430298
+      - -0.50511973997704984
+      - -0.13701792117172931
+      - -0.083165475611346268
     accelerations:
       - 0
       - 0
@@ -1271,23 +1271,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 157131716
+      nanosec: 196882918
   - positions:
-      - 0.61207404978211644
-      - -0.18228674666914305
-      - -0.46155192607616119
-      - -2.1774942034805589
-      - 0.089150115272005351
-      - 0.15926296684765812
-      - -2.325475369368486
+      - 1.3766691811778666
+      - -1.8504900340061656
+      - 1.0888395425356718
+      - -1.5810230807872137
+      - -0.19386090937006192
+      - -0.039845802214891944
+      - -0.75716970089531321
     velocities:
-      - -0.15532840217922025
-      - -0.37255364620635945
-      - 0.068464665025379984
-      - 0.15434477601673291
-      - -0.032093355795158852
-      - 0.14310110433565876
-      - 0.28688061595905151
+      - -0.51815820428265413
+      - 0.1927940336919354
+      - -0.11145841832166027
+      - -0.02693907894195343
+      - -0.51700918207296875
+      - -0.14077288101194488
+      - -0.086250863655046264
     accelerations:
       - 0
       - 0
@@ -1299,23 +1299,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 166906592
+      nanosec: 205105209
   - positions:
-      - 0.61052542524350017
-      - -0.18597604826973851
-      - -0.46087885019148594
-      - -2.1759661951810454
-      - 0.088832805330222253
-      - 0.1606878768058711
-      - -2.322655554761389
+      - 1.3725611110712785
+      - -1.848945492218381
+      - 1.0879471772252598
+      - -1.5812367208583182
+      - -0.197959924571916
+      - -0.040974463650074761
+      - -0.75785788863200043
     velocities:
-      - -0.15520436009501259
-      - -0.36792003218973529
-      - 0.067754262338517141
-      - 0.15324971334053747
-      - -0.031790294647569259
-      - 0.1423102275435485
-      - 0.2812988276963741
+      - -0.5149760827727109
+      - 0.1946029358616517
+      - -0.11293846236950343
+      - -0.026803065159757365
+      - -0.51352630303416735
+      - -0.14175515851047582
+      - -0.086658896983289754
     accelerations:
       - 0
       - 0
@@ -1327,23 +1327,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 177284452
+      nanosec: 213440043
   - positions:
-      - 0.60897645881444618
-      - -0.1896253852558534
-      - -0.46020630009309282
-      - -2.1744393624291987
-      - 0.088516382453836584
-      - 0.16210563440058906
-      - -2.3198738696207091
+      - 1.3683950649117329
+      - -1.8473594071970483
+      - 1.0870173265843845
+      - -1.581452282979336
+      - -0.20212856544528726
+      - -0.042120216794090545
+      - -0.75856534752994986
     velocities:
-      - -0.15467041485817737
-      - -0.36281640637515644
-      - 0.066696267069777104
-      - 0.15197761490239722
-      - -0.031472349086785228
-      - 0.14143597689220869
-      - 0.2755394267079383
+      - -0.53638397465474164
+      - 0.2043651011505537
+      - -0.12200598474020156
+      - -0.027462836182322658
+      - -0.53997715202777119
+      - -0.14577605400462407
+      - -0.091867148561919146
     accelerations:
       - 0
       - 0
@@ -1355,23 +1355,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 188780617
+      nanosec: 225002959
   - positions:
-      - 0.60743362319766858
-      - -0.19322507401619315
-      - -0.45953737948874934
-      - -2.1729189408780685
-      - 0.088201914248618424
-      - 0.16351338286317929
-      - -2.3171385982807875
+      - 1.3651738709664289
+      - -1.846109336651234
+      - 1.0862832094542232
+      - -1.5816192051121634
+      - -0.20534493131485129
+      - -0.043006275841487966
+      - -0.75911850746283538
     velocities:
-      - -0.15371141724270415
-      - -0.35731438992071346
-      - 0.066210075574101496
-      - 0.1511190390698203
-      - -0.031235065352796791
-      - 0.14024611966617756
-      - 0.27057295164213385
+      - -0.54034387081788271
+      - 0.21054215260606993
+      - -0.1238520680994717
+      - -0.027890637511176205
+      - -0.54080074535447697
+      - -0.14877989304124281
+      - -0.093132328562595484
     accelerations:
       - 0
       - 0
@@ -1383,23 +1383,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 198630300
+      nanosec: 231594626
   - positions:
-      - 0.60590711330033398
-      - -0.196756692871043
-      - -0.45887706321377808
-      - -2.1714147365264105
-      - 0.087891412090218987
-      - 0.16490447516770451
-      - -2.3144652699607979
+      - 1.3619468539849904
+      - -1.8448380551828416
+      - 1.0855392238595709
+      - -1.5817864963393107
+      - -0.20856567110960025
+      - -0.043904157246087622
+      - -0.75967684460420026
     velocities:
-      - -0.15171269194328513
-      - -0.3497524926420445
-      - 0.065122759488552021
-      - 0.14905407108058563
-      - -0.030748200463256655
-      - 0.13828462421592438
-      - 0.26370130341638998
+      - -0.54125605615998673
+      - 0.21357199556509943
+      - -0.12578966960357402
+      - -0.027935474112590626
+      - -0.54169972575008929
+      - -0.15048475232739777
+      - -0.094014154715857154
     accelerations:
       - 0
       - 0
@@ -1411,23 +1411,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 208581577
+      nanosec: 238484209
   - positions:
-      - 0.60438332295642394
-      - -0.2002426464021933
-      - -0.45821535907443106
-      - -2.1699125398689287
-      - 0.08758198170284609
-      - 0.16628425076308317
-      - -2.3118287532963753
+      - 1.3587139903579941
+      - -1.8435501444689848
+      - 1.0847824171150442
+      - -1.5819540304415083
+      - -0.21179274072561666
+      - -0.044810508972725163
+      - -0.76024056663930895
     velocities:
-      - -0.15059775364892039
-      - -0.34380158200656508
-      - 0.064231587112619754
-      - 0.14756969016419094
-      - -0.030384503747717014
-      - 0.1368719483819644
-      - 0.25794651265994967
+      - -0.54176844443292738
+      - 0.21611738556068724
+      - -0.12785666017561104
+      - -0.027946202063686532
+      - -0.54236536366136645
+      - -0.15169902476638059
+      - -0.094829959604721473
     accelerations:
       - 0
       - 0
@@ -1439,23 +1439,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 218616263
+      nanosec: 245079543
   - positions:
-      - 0.60287432134393282
-      - -0.20366451281967832
-      - -0.45756005395629873
-      - -2.1684240640898804
-      - 0.087275983508321026
-      - 0.16764716529761062
-      - -2.3092476289141302
+      - 1.3555059287447286
+      - -1.8422564737594092
+      - 1.0840222756489633
+      - -1.5821206988713434
+      - -0.21498987448818133
+      - -0.04571895413619631
+      - -0.76080348930732056
     velocities:
-      - -0.14947923902678026
-      - -0.33812971391228325
-      - 0.064385295507784879
-      - 0.14709368476183016
-      - -0.030221408124373322
-      - 0.1352331909636606
-      - 0.25395982392779159
+      - -0.52932029122019209
+      - 0.21437214353544692
+      - -0.1256460581078154
+      - -0.027529915603822161
+      - -0.52723634268788289
+      - -0.1508089846660472
+      - -0.093013933256808928
     accelerations:
       - 0
       - 0
@@ -1467,23 +1467,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 228859536
+      nanosec: 251603543
   - positions:
-      - 0.60135999140428065
-      - -0.20705620446364409
-      - -0.45689485994435181
-      - -2.1669264879536638
-      - 0.086968829421166349
-      - 0.16900202166532266
-      - -2.3066834587417775
+      - 1.3512228181674095
+      - -1.84051589659629
+      - 1.0829843993020039
+      - -1.5823417013711321
+      - -0.21927649050545961
+      - -0.046931836179022295
+      - -0.76156257587592802
     velocities:
-      - -0.15161427685459039
-      - -0.3376526663519212
-      - 0.06564015327898487
-      - 0.14880648325329934
-      - -0.030500842202560281
-      - 0.13540973609186974
-      - 0.25365194494642451
+      - -0.54926961607996805
+      - 0.22243504611104803
+      - -0.13591668077922917
+      - -0.027860879075696163
+      - -0.55542919521164913
+      - -0.15322700300229883
+      - -0.098191434647503378
     accelerations:
       - 0
       - 0
@@ -1495,23 +1495,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 240541856
+      nanosec: 261585376
   - positions:
-      - 0.59985540935897197
-      - -0.21039581389102249
-      - -0.4562418571069205
-      - -2.1654453436462417
-      - 0.086665580719934146
-      - 0.17034706709794686
-      - -2.3041767136769606
+      - 1.3479077115631486
+      - -1.8391486102356944
+      - 1.0821615205578288
+      - -1.5825127310070486
+      - -0.22258992787495896
+      - -0.047863159679792724
+      - -0.76215596470652958
     velocities:
-      - -0.15011646605413492
-      - -0.33173590739775016
-      - 0.064514737485584728
-      - 0.14698039254782469
-      - -0.03006729283841271
-      - 0.13414185367755255
-      - 0.24767073964117459
+      - -0.55663723312369784
+      - 0.23069062680408522
+      - -0.13888716198616752
+      - -0.028622648804113714
+      - -0.55735037502622786
+      - -0.15634915628411991
+      - -0.099933286503736035
     accelerations:
       - 0
       - 0
@@ -1523,23 +1523,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 250578484
+      nanosec: 269968043
   - positions:
-      - 0.59836286991635457
-      - -0.21368144214890589
-      - -0.45559496684763118
-      - -2.163976054305865
-      - 0.086365341074584392
-      - 0.17167980785555248
-      - -2.3017187414212819
+      - 1.3445231824486414
+      - -1.837728758312178
+      - 1.0813098219936073
+      - -1.5826873480341841
+      - -0.2259692173898955
+      - -0.048817060150539257
+      - -0.76276605435008482
     velocities:
-      - -0.1481456320141025
-      - -0.32518640678746663
-      - 0.063513046713341029
-      - 0.14521235721923442
-      - -0.029654839785253673
-      - 0.13251832193935134
-      - 0.24191997978829444
+      - -0.56014746416915528
+      - 0.23683631160791718
+      - -0.14083100434433019
+      - -0.029017454773612512
+      - -0.5577967693871263
+      - -0.15923333440989074
+      - -0.1010237821331518
     accelerations:
       - 0
       - 0
@@ -1551,23 +1551,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 260364591
+      nanosec: 275144959
   - positions:
-      - 0.59688630611329796
-      - -0.21690703507099343
-      - -0.45495109777501452
-      - -2.1625175572636985
-      - 0.08606790568425321
-      - 0.17299582172235287
-      - -2.2993068027548045
+      - 1.341165130093231
+      - -1.8363015073744078
+      - 1.0804563902524094
+      - -1.5828607195556972
+      - -0.22932062404177064
+      - -0.049775797256955044
+      - -0.76337451409266166
     velocities:
-      - -0.14814372071603088
-      - -0.32215524851061333
-      - 0.065607760306099827
-      - 0.14727226174218941
-      - -0.030005808656379866
-      - 0.13135480116363588
-      - 0.24227864043977945
+      - -0.55951274399166462
+      - 0.23873810446765395
+      - -0.14285284550731495
+      - -0.028956182779157544
+      - -0.55750094143475604
+      - -0.16015564407105851
+      - -0.10154231667044028
     accelerations:
       - 0
       - 0
@@ -1579,23 +1579,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 270215060
+      nanosec: 281671501
   - positions:
-      - 0.59541188746874385
-      - -0.22009330493977286
-      - -0.45430805497509641
-      - -2.1610635488104197
-      - 0.085771996563394792
-      - 0.17430236958396558
-      - -2.2969297253814198
+      - 1.3378129149727087
+      - -1.8348634985337995
+      - 1.0795912868949615
+      - -1.5830336732423052
+      - -0.2326673338050303
+      - -0.050738141598980484
+      - -0.76398559497729313
     velocities:
-      - -0.14753107569193336
-      - -0.31750529604172967
-      - 0.064628117973303201
-      - 0.1456550747904084
-      - -0.029613309930372495
-      - 0.13037817024264761
-      - 0.23699451739399177
+      - -0.55937556881497941
+      - 0.24077079693968012
+      - -0.14513993952877421
+      - -0.028893878470165113
+      - -0.55795803607582228
+      - -0.16072126296265335
+      - -0.10215963625891057
     accelerations:
       - 0
       - 0
@@ -1607,23 +1607,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 280479940
+      nanosec: 288571626
   - positions:
-      - 0.59395077627014281
-      - -0.22322537354921365
-      - -0.45367345587985225
-      - -2.1596246288281833
-      - 0.085479695176540374
-      - 0.17559553204105019
-      - -2.2946031809207761
+      - 1.3345110749519076
+      - -1.8334309846768131
+      - 1.0787305798579112
+      - -1.5832045241088704
+      - -0.23595764753591852
+      - -0.051695525091936971
+      - -0.76459005411611825
     velocities:
-      - -0.14561705281209128
-      - -0.3109974346073463
-      - 0.062966647542136989
-      - 0.14306478850442469
-      - -0.029039998308093364
-      - 0.12879714576160636
-      - 0.2303282731061424
+      - -0.54478988628368785
+      - 0.23710924694400112
+      - -0.14239067966213106
+      - -0.028203654982448249
+      - -0.54279588235770337
+      - -0.15865953854198594
+      - -0.099855828484599063
     accelerations:
       - 0
       - 0
@@ -1635,23 +1635,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 290644303
+      nanosec: 295067209
   - positions:
-      - 0.59251176978223696
-      - -0.22628970939386989
-      - -0.45304915463352496
-      - -2.1582064397412974
-      - 0.085192132560003311
-      - 0.17686940951601401
-      - -2.2923347190156509
+      - 1.3312027912814475
+      - -1.8319874431368608
+      - 1.077851138569061
+      - -1.5833748672750754
+      - -0.23926368791303371
+      - -0.052651437479717171
+      - -0.76519993431450717
     velocities:
-      - -0.14330826892334964
-      - -0.30401088491724398
-      - 0.062076849844128472
-      - 0.14103997358654127
-      - -0.028575403465212475
-      - 0.12668093158652124
-      - 0.22464626151582562
+      - -0.54734139219707545
+      - 0.24021412278368706
+      - -0.14566103103084668
+      - -0.028306681162707178
+      - -0.54535858805348625
+      - -0.15883883943637053
+      - -0.10095046785333325
     accelerations:
       - 0
       - 0
@@ -1663,23 +1663,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 302212889
+      nanosec: 301915626
   - positions:
-      - 0.59109517554081981
-      - -0.22928396485021074
-      - -0.45243275362412938
-      - -2.156808146714718
-      - 0.084909146148800685
-      - 0.17812068909573298
-      - -2.2901217293081375
+      - 1.3279158332471084
+      - -1.830538880587828
+      - 1.0769656331549309
+      - -1.5835439914052305
+      - -0.24254849332970804
+      - -0.053602609644554509
+      - -0.76580898066231162
     velocities:
-      - -0.14155879103519109
-      - -0.29772931183538115
-      - 0.061681866416906574
-      - 0.13958470236831119
-      - -0.02822397768810957
-      - 0.12455506527548771
-      - 0.21995798371867664
+      - -0.54951284197277539
+      - 0.24298062942866699
+      - -0.14888831363135843
+      - -0.028305327995338855
+      - -0.54858135718153433
+      - -0.15874188585898596
+      - -0.10199997527410297
     accelerations:
       - 0
       - 0
@@ -1691,23 +1691,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 312639182
+      nanosec: 306887876
   - positions:
-      - 0.58968441499878177
-      - -0.23223484940073516
-      - -0.4518155540513567
-      - -2.1554145006119589
-      - 0.084627652361231195
-      - 0.17935747193531745
-      - -2.2879400884276433
+      - 1.3234645658027053
+      - -1.828556417354813
+      - 1.0757404785930216
+      - -1.583771878267819
+      - -0.24700613637215596
+      - -0.054880699503329418
+      - -0.76664001186526487
     velocities:
-      - -0.14158963973273458
-      - -0.29489524088760449
-      - 0.062823298508766917
-      - 0.14074478688400718
-      - -0.028404481130163795
-      - 0.12347374032327348
-      - 0.21924024237724252
+      - -0.57319399390891501
+      - 0.25553646635535698
+      - -0.16048401482445379
+      - -0.029045096582760058
+      - -0.57726101336747659
+      - -0.16230347806109291
+      - -0.10763600033758158
     accelerations:
       - 0
       - 0
@@ -1719,23 +1719,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 322162983
+      nanosec: 320077751
   - positions:
-      - 0.58799810119831486
-      - -0.23572541126946298
-      - -0.45108151506614552
-      - -2.153754302802394
-      - 0.084293003003144684
-      - 0.18082908826746905
-      - -2.2853713022471749
+      - 1.3199575503943299
+      - -1.8269669746538995
+      - 1.0747546415813458
+      - -1.5839514669045802
+      - -0.25050987552361625
+      - -0.055877168393538908
+      - -0.76729932753771335
     velocities:
-      - -0.13939106705637141
-      - -0.28760664804938335
-      - 0.060123301509159677
-      - 0.13673323081358965
-      - -0.027540194209193933
-      - 0.12183196234915931
-      - 0.21053388293385161
+      - -0.58019006957501928
+      - 0.26547645858345448
+      - -0.16250284260361444
+      - -0.029872580486315203
+      - -0.57751107001008073
+      - -0.1665330059904864
+      - -0.1090103433041939
     accelerations:
       - 0
       - 0
@@ -1747,23 +1747,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 335495283
+      nanosec: 326546418
   - positions:
-      - 0.5863496754047649
-      - -0.23911632639180597
-      - -0.45036648382999167
-      - -2.1521304882778023
-      - 0.083966344886091754
-      - 0.18227114831593408
-      - -2.2828886028372115
+      - 1.3153003344084642
+      - -1.8248196198550777
+      - 1.0734359209021775
+      - -1.5841903764204173
+      - -0.25515597310267646
+      - -0.05722215295149103
+      - -0.76817772935121842
     velocities:
-      - -0.13600756928167718
-      - -0.27882745639786477
-      - 0.05860953484261025
-      - 0.13362976202726862
-      - -0.0268630479949457
-      - 0.11904755349666474
-      - 0.2033545587267192
+      - -0.57777403327843535
+      - 0.26774581012610449
+      - -0.16426467756580687
+      - -0.029657762000448739
+      - -0.57624601206791815
+      - -0.16805698641656128
+      - -0.10912067679869417
     accelerations:
       - 0
       - 0
@@ -1775,23 +1775,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 347184023
+      nanosec: 334932001
   - positions:
-      - 0.58472020816233083
-      - -0.24243554224912878
-      - -0.44965430697898506
-      - -2.1505211808649807
-      - 0.08364334207384988
-      - 0.18368895802817173
-      - -2.280457690086656
+      - 1.3118295442278882
+      - -1.8232058801500848
+      - 1.0724379175406735
+      - -1.5843681966804692
+      - -0.25862163198708793
+      - -0.058232676051226485
+      - -0.76883520433857466
     velocities:
-      - -0.13517306814336114
-      - -0.27412528422460164
-      - 0.059034942646120289
-      - 0.13330209312768562
-      - -0.026726646772479101
-      - 0.11735841204939906
-      - 0.20038232078665394
+      - -0.57907690755061714
+      - 0.26967451195407893
+      - -0.16747025541690802
+      - -0.029613028307716109
+      - -0.57888447521033171
+      - -0.16859001804097651
+      - -0.10987113318393804
     accelerations:
       - 0
       - 0
@@ -1803,23 +1803,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 360409856
+      nanosec: 340108293
   - positions:
-      - 0.58311383129660965
-      - -0.24567795437847972
-      - -0.44895252367813432
-      - -2.1489355109012278
-      - 0.083325739696203285
-      - 0.18508163905084696
-      - -2.278090018744988
+      - 1.3084055561757151
+      - -1.8216011728103569
+      - 1.0714428485772225
+      - -1.5845439320399517
+      - -0.26203669565660043
+      - -0.059236654515419658
+      - -0.76948548527178451
     velocities:
-      - -0.13410838962451707
-      - -0.26940766938235872
-      - 0.059387958491411916
-      - 0.13315110012784148
-      - -0.026643009453735222
-      - 0.11563494762719904
-      - 0.1977503090738098
+      - -0.57002738377039996
+      - 0.26768820668274806
+      - -0.16648000725306769
+      - -0.029265130309575962
+      - -0.56836950256345298
+      - -0.16714115605554794
+      - -0.10837811587794499
     accelerations:
       - 0
       - 0
@@ -1831,23 +1831,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 371931146
+      nanosec: 346904168
   - positions:
-      - 0.58153017058549861
-      - -0.24884739968524808
-      - -0.44826399075930851
-      - -2.1473758046517348
-      - 0.083013962220239546
-      - 0.18645186276254583
-      - -2.2757871152295834
+      - 1.3049969386096931
+      - -1.8199939112303407
+      - 1.0704379129595618
+      - -1.5847184799472798
+      - -0.26544040101208838
+      - -0.060235672387776468
+      - -0.77013512848508325
     velocities:
-      - -0.13098051739637603
-      - -0.26137320483780624
-      - 0.057226306063766802
-      - 0.12925965637697634
-      - -0.025810381912124564
-      - 0.11321275272271322
-      - 0.18994998465577279
+      - -0.56744338139680839
+      - 0.26833862105691741
+      - -0.16797323047428497
+      - -0.029096144920581886
+      - -0.56601971332207168
+      - -0.16630497837390351
+      - -0.10822971516095399
     accelerations:
       - 0
       - 0
@@ -1859,23 +1859,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 383527220
+      nanosec: 353296459
   - positions:
-      - 0.57998126283384122
-      - -0.25192566775475439
-      - -0.44759049033061005
-      - -2.145849348892872
-      - 0.082709436664329991
-      - 0.18779085971038811
-      - -2.2735576089669878
+      - 1.3015992897086452
+      - -1.818379941501532
+      - 1.0694228455314667
+      - -1.5848921937944969
+      - -0.26883476772787712
+      - -0.061230044633648291
+      - -0.77078465156858267
     velocities:
-      - -0.12754931528589003
-      - -0.25273383146702472
-      - 0.054944941656523867
-      - 0.12520022124704411
-      - -0.024961422903028871
-      - 0.11037485505620873
-      - 0.18210428963411129
+      - -0.56736587812791295
+      - 0.27022695573662636
+      - -0.17032890213965043
+      - -0.029025289356165218
+      - -0.56642260942830081
+      - -0.16577573156320938
+      - -0.10856184867441342
     accelerations:
       - 0
       - 0
@@ -1887,23 +1887,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 396896710
+      nanosec: 360065209
   - positions:
-      - 0.57846474381510826
-      - -0.25491695739827286
-      - -0.44693188399745687
-      - -2.144355211610447
-      - 0.082411923815600446
-      - 0.1890991015154469
-      - -2.2713983726074085
+      - 1.2982390081434203
+      - -1.8167693809790146
+      - 1.0684090716731831
+      - -1.5850640363287447
+      - -0.27218981788904689
+      - -0.062215595962181069
+      - -0.77142833716395831
     velocities:
-      - -0.12361933376616764
-      - -0.24374449402765533
-      - 0.053004428830888746
-      - 0.1213243503813715
-      - -0.024145493685713535
-      - 0.10720554325759964
-      - 0.17466759552191541
+      - -0.56324540705813075
+      - 0.26976325916283012
+      - -0.17143975295726663
+      - -0.02859825477147138
+      - -0.56485602061125118
+      - -0.16424859892226154
+      - -0.10815727803233052
     accelerations:
       - 0
       - 0
@@ -1915,23 +1915,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 408717782
+      nanosec: 366899126
   - positions:
-      - 0.57697720160477961
-      - -0.25782659952894499
-      - -0.44627803434420138
-      - -2.1428826781782533
-      - 0.082119347008528631
-      - 0.19037428405565424
-      - -2.2692930801146161
+      - 1.2948772799227384
+      - -1.8151473232791835
+      - 1.0673801532198237
+      - -1.5852355734106647
+      - -0.27554862711871431
+      - -0.063196944336044919
+      - -0.77207415935369417
     velocities:
-      - -0.12364960572751826
-      - -0.24050374895707927
-      - 0.054051120835503182
-      - 0.12188356132866772
-      - -0.024198892435886028
-      - 0.10557055795985867
-      - 0.1735129671306741
+      - -0.56283742632415923
+      - 0.27186239907291287
+      - -0.17343936392284867
+      - -0.028581124945963102
+      - -0.56394610001864209
+      - -0.16364452040716679
+      - -0.10831529485323592
     accelerations:
       - 0
       - 0
@@ -1943,23 +1943,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 421960971
+      nanosec: 373284584
   - positions:
-      - 0.575269649008743
-      - -0.26113385967894359
-      - -0.44553290041621713
-      - -2.1411994737498192
-      - 0.08178555270538948
-      - 0.19183137379885276
-      - -2.2669131687564104
+      - 1.2913428477644302
+      - -1.8134308232483469
+      - 1.0662714933427231
+      - -1.5854143699438921
+      - -0.27909244055628302
+      - -0.064207190170237738
+      - -0.77275661155377218
     velocities:
-      - -0.12086269755489502
-      - -0.23325288430727933
-      - 0.052778760232205457
-      - 0.1191405164983466
-      - -0.023600855395203651
-      - 0.10301815081232393
-      - 0.16759407680594243
+      - -0.59007302606679279
+      - 0.28882028599072995
+      - -0.18493935119046481
+      - -0.029985469478740416
+      - -0.58962847638400784
+      - -0.1693641939488954
+      - -0.11385700805370386
     accelerations:
       - 0
       - 0
@@ -1971,23 +1971,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 435453362
+      nanosec: 384891501
   - positions:
-      - 0.5736161972001671
-      - -0.26431584583643769
-      - -0.44481409497342145
-      - -2.1395704500766932
-      - 0.081463165816572855
-      - 0.19324372744454929
-      - -2.2646358144540999
+      - 1.2877333127597597
+      - -1.811651279195835
+      - 1.0651289247657765
+      - -1.585597083716296
+      - -0.2827052549603144
+      - -0.065241648831481244
+      - -0.77345438467340766
     velocities:
-      - -0.11615152785361303
-      - -0.22282712263967253
-      - 0.050094865694436955
-      - 0.11403884724756212
-      - -0.022552195168527338
-      - 0.099297485338660355
-      - 0.15870785488684988
+      - -0.61049056065955765
+      - 0.30139442951873757
+      - -0.1948176586730308
+      - -0.030702215107826281
+      - -0.61338622659893283
+      - -0.17428160792284833
+      - -0.11824100170178582
     accelerations:
       - 0
       - 0
@@ -1999,23 +1999,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 450496338
+      nanosec: 393273293
   - positions:
-      - 0.57199575947544701
-      - -0.26740331805355316
-      - -0.4441043350758595
-      - -2.1379706458851433
-      - 0.081147266708403831
-      - 0.19461791755726518
-      - -2.2624246818422118
+      - 1.284091893127995
+      - -1.8098325238476469
+      - 1.0639670960378762
+      - -1.5857821837854169
+      - -0.28633752782253757
+      - -0.066294230910625335
+      - -0.7741585474016448
     velocities:
-      - -0.11422356464865281
-      - -0.21681159814297221
-      - 0.049289577175452751
-      - 0.11195420066330992
-      - -0.02209222479208927
-      - 0.096957267474404696
-      - 0.15406718134852537
+      - -0.60794587358046759
+      - 0.30438407666650974
+      - -0.19480891458442515
+      - -0.030815273423878735
+      - -0.60750969115941755
+      - -0.17608195576733615
+      - -0.11765717456760338
     accelerations:
       - 0
       - 0
@@ -2027,23 +2027,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 465515087
+      nanosec: 399968334
   - positions:
-      - 0.57042833796398906
-      - -0.2703714695379692
-      - -0.44342037853723587
-      - -2.1364240302596604
-      - 0.08084244228847437
-      - 0.19594756810034789
-      - -2.2603096115446943
+      - 1.2805082357364563
+      - -1.8080263756264345
+      - 1.0628159564877668
+      - -1.5859646591773642
+      - -0.28990895791860516
+      - -0.067344336793742035
+      - -0.77485172370408373
     velocities:
-      - -0.11086279345876364
-      - -0.20906334050834649
-      - 0.048637778720588401
-      - 0.10951977141817484
-      - -0.021560883518183405
-      - 0.093721133876028026
-      - 0.14908564165021324
+      - -0.59750822069107601
+      - 0.30102853963037474
+      - -0.19328303290869664
+      - -0.030265064411023033
+      - -0.5975624075963899
+      - -0.17492788834126877
+      - -0.11574160429000573
     accelerations:
       - 0
       - 0
@@ -2055,23 +2055,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 478778194
+      nanosec: 406647168
   - positions:
-      - 0.56890996720612597
-      - -0.27322439024117096
-      - -0.44275942552669478
-      - -2.1349282263411262
-      - 0.080548190704177278
-      - 0.19723176496503439
-      - -2.2582847038572216
+      - 1.2769768161817654
+      - -1.806238599165866
+      - 1.0616679926630797
+      - -1.5861443304537299
+      - -0.29343080564289004
+      - -0.068384635711205538
+      - -0.77553560784939912
     velocities:
-      - -0.10600274288763926
-      - -0.19893462129839301
-      - 0.045721441063983743
-      - 0.10414042016617871
-      - -0.020475007626685093
-      - 0.089945461277845029
-      - 0.14041350039439091
+      - -0.58819909730088771
+      - 0.2977763604846656
+      - -0.19244859017425259
+      - -0.029802977447987135
+      - -0.58818260979797599
+      - -0.17296761214126877
+      - -0.11404229643986463
     accelerations:
       - 0
       - 0
@@ -2083,23 +2083,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 493843267
+      nanosec: 413471543
   - positions:
-      - 0.56722761459892568
-      - -0.27635842762616686
-      - -0.44202276783028371
-      - -2.1332675215430719
-      - 0.080222190084243028
-      - 0.19864630396555297
-      - -2.256060720311948
+      - 1.2734905854983274
+      - -1.8044654913938842
+      - 1.0605207663577061
+      - -1.5863214778458843
+      - -0.29690994775535773
+      - -0.069412434450134952
+      - -0.77621149393544087
     velocities:
-      - -0.10282589514549345
-      - -0.19106150256861587
-      - 0.044381735294507479
-      - 0.10090800081990144
-      - -0.019796473712963048
-      - 0.086679926391221476
-      - 0.13452224834102169
+      - -0.58096542504537718
+      - 0.29555272717782488
+      - -0.19239981459329356
+      - -0.029393213539266039
+      - -0.58136109707789085
+      - -0.17078905314403475
+      - -0.112758506333158
     accelerations:
       - 0
       - 0
@@ -2111,23 +2111,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 510522834
+      nanosec: 420079126
   - positions:
-      - 0.56562062955972559
-      - -0.27933304783058488
-      - -0.4413199229630172
-      - -2.1316807216641114
-      - 0.079911322440802346
-      - 0.19999606444779569
-      - -2.2539584478990915
+      - 1.270053838578483
+      - -1.8027076878690349
+      - 1.0593774041942545
+      - -1.5864959762796882
+      - -0.30034019370165388
+      - -0.070424999911650341
+      - -0.77687825643745634
     velocities:
-      - -0.098855453832091003
-      - -0.18205949544959235
-      - 0.043025739279530816
-      - 0.097231692627333624
-      - -0.019031443342674485
-      - 0.082762532792108739
-      - 0.12824025284828192
+      - -0.57308807323693833
+      - 0.2931252070295382
+      - -0.1919467839506124
+      - -0.028943456026401814
+      - -0.5739033656483844
+      - -0.16817363751460621
+      - -0.11131957459579479
     accelerations:
       - 0
       - 0
@@ -2139,23 +2139,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 527413247
+      nanosec: 426620209
   - positions:
-      - 0.56406689983352765
-      - -0.28218079145018993
-      - -0.44063968339504606
-      - -2.130148320609488
-      - 0.07961171837278172
-      - 0.2012913090220094
-      - -2.2519495833364536
+      - 1.2666813310040905
+      - -1.8009708521366385
+      - 1.058245802145569
+      - -1.5866674592192813
+      - -0.3037016162638379
+      - -0.071418795951096584
+      - -0.77753254321157705
     velocities:
-      - -0.095188788960331297
-      - -0.17395041572617681
-      - 0.041204919041858162
-      - 0.093416148876583838
-      - -0.018252862283893233
-      - 0.079445057550499215
-      - 0.12193538211737948
+      - -0.55521824084958782
+      - 0.28676813698647324
+      - -0.18649208735253292
+      - -0.02825799027916559
+      - -0.55304061293849505
+      - -0.16401160961120867
+      - -0.10768065038262603
     accelerations:
       - 0
       - 0
@@ -2167,23 +2167,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 544126222
+      nanosec: 433490959
   - positions:
-      - 0.56240875078473307
-      - -0.28520349785083715
-      - -0.43991557448705937
-      - -2.1285126514828554
-      - 0.079292545554097552
-      - 0.2026745201713018
-      - -2.2498277656370274
+      - 1.2633490208640676
+      - -1.7992473109762002
+      - 1.0571135104916818
+      - -1.5868362223043582
+      - -0.30702968151837801
+      - -0.072394878556887116
+      - -0.77817968910239144
     velocities:
-      - -0.08978298879767084
-      - -0.16305028773828045
-      - 0.039131356442687913
-      - 0.088422606577019558
-      - -0.017237618682627995
-      - 0.074777669240588623
-      - 0.11416590569245755
+      - -0.54900873303997977
+      - 0.28508068250634866
+      - -0.18653977700201371
+      - -0.027906387041960412
+      - -0.54690565067492036
+      - -0.16122398379787992
+      - -0.10652892003097338
     accelerations:
       - 0
       - 0
@@ -2195,23 +2195,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 562522163
+      nanosec: 440068626
   - positions:
-      - 0.56082868779354234
-      - -0.28805777044415742
-      - -0.4392228277067512
-      - -2.1269528962050286
-      - 0.078988821435586803
-      - 0.20398359791269297
-      - -2.2478259731292711
+      - 1.2600490723780924
+      - -1.7975301793512559
+      - 1.055979938890333
+      - -1.5870030540270301
+      - -0.31032639416676289
+      - -0.073355709187604631
+      - -0.77882074330926876
     velocities:
-      - -0.085828283188922536
-      - -0.15465921270662039
-      - 0.038046395186154311
-      - 0.085264488091126772
-      - -0.016588249781531353
-      - 0.07091220775392075
-      - 0.10889390608294566
+      - -0.54459285525745105
+      - 0.28451860438594873
+      - -0.18709289850317973
+      - -0.027627054791168722
+      - -0.54271277343420865
+      - -0.15889184266067088
+      - -0.10570275765529706
     accelerations:
       - 0
       - 0
@@ -2223,23 +2223,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 580503259
+      nanosec: 445237584
   - positions:
-      - 0.55917576003335234
-      - -0.29102253730004085
-      - -0.43849920153448202
-      - -2.1253225398344084
-      - 0.07867197134291444
-      - 0.20534892584313366
-      - -2.2457548893731492
+      - 1.2567601002138569
+      - -1.7958104329742641
+      - 1.0548351583280255
+      - -1.587168559188981
+      - -0.31361894184571598
+      - -0.074304769847990129
+      - -0.77946011979005347
     velocities:
-      - -0.080197157115355591
-      - -0.14331655122197137
-      - 0.03493695296876901
-      - 0.078876836493333546
-      - -0.015316904034930851
-      - 0.066154117736176035
-      - 0.099756047008316792
+      - -0.55255468117512763
+      - 0.28945838355200476
+      - -0.19337396501120871
+      - -0.027837620807760755
+      - -0.55237067757253155
+      - -0.15851599968994379
+      - -0.10741774898496312
     accelerations:
       - 0
       - 0
@@ -2251,23 +2251,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 602007801
+      nanosec: 455051126
   - positions:
-      - 0.55749097814040083
-      - -0.29402611433672837
-      - -0.4377623268060617
-      - -2.1236607132867427
-      - 0.078349570096704929
-      - 0.20673701701479735
-      - -2.243663940689185
+      - 1.2534434333196203
+      - -1.7940595228923339
+      - 1.0536700195018232
+      - -1.5873357476168128
+      - -0.31693044194197917
+      - -0.075255476377960331
+      - -0.78010425837580955
     velocities:
-      - -0.073477543418494287
-      - -0.1308489441352404
-      - 0.032415391289636171
-      - 0.07289654406120287
-      - -0.014132654803512293
-      - 0.060462199316735458
-      - 0.09135172289995061
+      - -0.54748560927255163
+      - 0.29046974372649925
+      - -0.1921054834247406
+      - -0.027679342860829643
+      - -0.54545886544792099
+      - -0.15761766371962738
+      - -0.10620734037544913
     accelerations:
       - 0
       - 0
@@ -2279,23 +2279,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 624033478
+      nanosec: 461555043
   - positions:
-      - 0.55582184604499663
-      - -0.29698675988385276
-      - -0.43703091540105166
-      - -2.1220130835522464
-      - 0.078030383319099922
-      - 0.20810558841866256
-      - -2.2416060726916975
+      - 1.249084383804681
+      - -1.7917392289043792
+      - 1.0521242682994902
+      - -1.5875548007090527
+      - -0.32128865615336905
+      - -0.076506344379312849
+      - -0.78095019635324492
     velocities:
-      - -0.066230071612881872
-      - -0.11733580272565945
-      - 0.029277045121127925
-      - 0.065713619512344029
-      - -0.012724273520924994
-      - 0.054110100998377653
-      - 0.081847142910744242
+      - -0.55224594507196023
+      - 0.29293742654309252
+      - -0.19888154403017155
+      - -0.027297605607791861
+      - -0.5578917232634506
+      - -0.1565227723423557
+      - -0.10745787567574254
     accelerations:
       - 0
       - 0
@@ -2307,23 +2307,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 648697292
+      nanosec: 471584084
   - positions:
-      - 0.55421372800432422
-      - -0.29982784956235103
-      - -0.43632592217110971
-      - -2.1204257936784741
-      - 0.077723188034476809
-      - 0.20941492097722197
-      - -2.2396338536391975
+      - 1.2458065758431001
+      - -1.7899817014499257
+      - 1.0509475821667233
+      - -1.5877198576698979
+      - -0.3245574194399834
+      - -0.077441725065315867
+      - -0.78158551182539193
     velocities:
-      - -0.058291962655256692
-      - -0.10275667431810849
-      - 0.02560114501640267
-      - 0.0575382729963397
-      - -0.011129180680393189
-      - 0.047229958725408694
-      - 0.071330675426044349
+      - -0.53998567193597546
+      - 0.29091642231225706
+      - -0.19354692938692067
+      - -0.02725602729053176
+      - -0.53759818687150918
+      - -0.15491610671805664
+      - -0.1045302382186223
     accelerations:
       - 0
       - 0
@@ -2335,23 +2335,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 675359102
+      nanosec: 478268793
   - positions:
-      - 0.55262037694426436
-      - -0.30263246781787112
-      - -0.435628664358642
-      - -2.1188539554756076
-      - 0.077419309821366189
-      - 0.21070213654057676
-      - -2.2376922960856449
+      - 1.2425933244259411
+      - -1.7882481027990491
+      - 1.0497859767798736
+      - -1.5878812102086375
+      - -0.32776683973693843
+      - -0.078361796271841394
+      - -0.78220764064480353
     velocities:
-      - -0.048453108692486715
-      - -0.085131047653951381
-      - 0.021126180992611848
-      - 0.047743751286312565
-      - -0.0092238292465740435
-      - 0.039015223515526998
-      - 0.0587445532529823
+      - -0.53707064403120919
+      - 0.28977595153749941
+      - -0.1953142305533615
+      - -0.026919441500810659
+      - -0.53691952068334559
+      - -0.15314790020888155
+      - -0.1040018891481501
     accelerations:
       - 0
       - 0
@@ -2363,23 +2363,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 707327540
+      nanosec: 484297084
   - positions:
-      - 0.55100662280921153
-      - -0.3054510569251434
-      - -0.43492123499154478
-      - -2.1172611823995107
-      - 0.077112023413406047
-      - 0.21198567226440632
-      - -2.2357456590885634
+      - 1.2393558279013004
+      - -1.7864955396282114
+      - 1.0485990034447585
+      - -1.5880432605616264
+      - -0.33100400630030541
+      - -0.07928032812331294
+      - -0.78283413643167876
     velocities:
-      - -0.036789213471614381
-      - -0.064064163990045483
-      - 0.016250726215523852
-      - 0.036498132294147577
-      - -0.0070326428362493584
-      - 0.029002198138138365
-      - 0.04430970963950203
+      - -0.53995346717155634
+      - 0.29324721560301359
+      - -0.19829596349384451
+      - -0.027129210737488363
+      - -0.53830129053877718
+      - -0.15313799450656168
+      - -0.10437053465877009
     accelerations:
       - 0
       - 0
@@ -2391,23 +2391,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 746198229
+      nanosec: 493305834
   - positions:
-      - 0.54941688194144833
-      - -0.30820234274206182
-      - -0.43422390776066722
-      - -2.1156924439507114
-      - 0.076810119493378237
-      - 0.21320942374284185
-      - -2.2338523950199614
+      - 1.236149178025886
+      - -1.7847453571525616
+      - 1.0474168844297651
+      - -1.588204064719682
+      - -0.33420396755734655
+      - -0.080192004729959757
+      - -0.78345321078038577
     velocities:
-      - -0.018426842846628051
-      - -0.031813692466761981
-      - 0.0080908860809238391
-      - 0.018195246289310133
-      - -0.0034985698079192788
-      - 0.013596482915715816
-      - 0.0218649465171676
+      - -0.53278383834826459
+      - 0.29139350483090165
+      - -0.19688944352048507
+      - -0.026748260059934477
+      - -0.53113498086589239
+      - -0.15149901233366481
+      - -0.10278505202632811
     accelerations:
       - 0
       - 0
@@ -2419,23 +2419,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 805395027
+      nanosec: 499914584
   - positions:
-      - 0.54904565470676947
-      - -0.30907826324262172
-      - -0.43445269597965452
-      - -2.1146975603791693
-      - 0.076624768561842041
-      - 0.20677920690837812
-      - -2.2324171133257495
+      - 1.2329864017951353
+      - -1.783010389163906
+      - 1.046241431850701
+      - -1.5883624975935693
+      - -0.33736096454267184
+      - -0.081091674709989847
+      - -0.78406281480899365
     velocities:
-      - 0.0053359926088837771
-      - 0.012173879106072932
-      - -0.021132332251083456
-      - 0.0059084120562428039
-      - -0.001029437037990195
-      - -0.23997057263034013
-      - 0.015114149856321597
+      - -0.52631545823754455
+      - 0.28901684757528906
+      - -0.1963462807345048
+      - -0.026361520322828392
+      - -0.52526680053537922
+      - -0.14946739165822154
+      - -0.10139793862890951
     accelerations:
       - 0
       - 0
@@ -2447,23 +2447,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 950885675
+      nanosec: 505047584
   - positions:
-      - 0.54910164519791571
-      - -0.30867216713222106
-      - -0.43524383002012518
-      - -2.1145104286426797
-      - 0.076591159562466482
-      - 0.19785566936057356
-      - -2.2319029208731056
+      - 1.2287977513253618
+      - -1.7807044114059603
+      - 1.0446637079799579
+      - -1.5885713637257497
+      - -0.34155150205374907
+      - -0.082276024977943965
+      - -0.78486906159053826
     velocities:
-      - 0.00094116766685416035
-      - 0.017534432370810007
-      - -0.035235289876959133
-      - 0.0077304008580907496
-      - -0.0014008212494161008
-      - -0.39081340827286626
-      - 0.022007476992162885
+      - -0.53335575405299773
+      - 0.29291199833699944
+      - -0.20369908262396563
+      - -0.026286891770495643
+      - -0.5373550065154854
+      - -0.14880670305980059
+      - -0.10277436915163554
     accelerations:
       - 0
       - 0
@@ -2475,23 +2475,23 @@ points:
     effort: []
     time_from_start:
       sec: 1
-      nanosec: 980335449
+      nanosec: 516594584
   - positions:
-      - 0.54905737123450526
-      - -0.30829057546538019
-      - -0.43605986748641223
-      - -2.1143421928294059
-      - 0.076560098195837686
-      - 0.18881701550494998
-      - -2.2314095728723253
+      - 1.2245882622903339
+      - -1.7783685105428666
+      - 1.0430589314103371
+      - -1.5887812683138667
+      - -0.34575669697025146
+      - -0.083458515994525057
+      - -0.785676810331709
     velocities:
-      - -0.0049922445651087808
-      - 0.020206136248358351
-      - -0.045227692121067205
-      - 0.0089321656533444121
-      - -0.0016725377252380022
-      - -0.5019552162982337
-      - 0.02672015878753127
+      - -0.53115866537018053
+      - 0.29446582392776105
+      - -0.20470387508061272
+      - -0.026176063121750876
+      - -0.53454181273262613
+      - -0.1480487787187435
+      - -0.10198576183699082
     accelerations:
       - 0
       - 0
@@ -2502,24 +2502,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 347613
+      sec: 1
+      nanosec: 526587584
   - positions:
-      - 0.54894974404717345
-      - -0.30793936467251865
-      - -0.4368664203297975
-      - -2.114186251408996
-      - 0.076530713092289093
-      - 0.17999270548329849
-      - -2.2309376995471082
+      - 1.2214509343835656
+      - -1.7766140508577086
+      - 1.0418540663474034
+      - -1.5889380500309325
+      - -0.34888331428653535
+      - -0.084340138047617363
+      - -0.78627680267113964
     velocities:
-      - -0.0079800320657373684
-      - 0.023161051683444702
-      - -0.05412236665558276
-      - 0.010313221522974416
-      - -0.0019511872524226603
-      - -0.58776784528400916
-      - 0.031405317078045281
+      - -0.51686455330938819
+      - 0.29003101267370635
+      - -0.19836207430913866
+      - -0.025862249216903735
+      - -0.51464391037805501
+      - -0.14590044713327557
+      - -0.098715603809990268
     accelerations:
       - 0
       - 0
@@ -2530,24 +2530,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 16905091
+      sec: 1
+      nanosec: 534920751
   - positions:
-      - 0.54886940510562932
-      - -0.30756566031673876
-      - -0.43769716673308134
-      - -2.1140250668119558
-      - 0.07650061793568165
-      - 0.17101802278557915
-      - -2.23045129632595
+      - 1.218296886032509
+      - -1.7748455184470902
+      - 1.0406274266055877
+      - -1.5890946719325361
+      - -0.35203738323677075
+      - -0.085220770232262807
+      - -0.78687903586827956
     velocities:
-      - -0.0073466633046231425
-      - 0.027745300286954083
-      - -0.062477702679687365
-      - 0.011962946163914804
-      - -0.0022453430090002484
-      - -0.67832968724567633
-      - 0.036293190144761962
+      - -0.52104533174208401
+      - 0.29344637472553992
+      - -0.20223351746437296
+      - -0.026029908550076388
+      - -0.51884703645450336
+      - -0.14614198602435396
+      - -0.099287397270570343
     accelerations:
       - 0
       - 0
@@ -2558,24 +2558,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 31894511
+      sec: 1
+      nanosec: 541624043
   - positions:
-      - 0.54874604785283465
-      - -0.30722418856442385
-      - -0.43849453890028484
-      - -2.1138751611452591
-      - 0.076472205444412855
-      - 0.16241709871568005
-      - -2.2299922665513838
+      - 1.2152110211390694
+      - -1.7731030615650027
+      - 1.0394237888899316
+      - -1.5892483918861335
+      - -0.35511569782206626
+      - -0.086087627425641022
+      - -0.78746622294325541
     velocities:
-      - -0.011921897930731416
-      - 0.029035855746616351
-      - -0.068923848536486726
-      - 0.012816347124214761
-      - -0.0024411387746530273
-      - -0.74491434388832634
-      - 0.039411725440598039
+      - -0.51210668686007432
+      - 0.28949954882250778
+      - -0.2002601147245707
+      - -0.025540264417753533
+      - -0.51035429608093297
+      - -0.14381330464413961
+      - -0.097356116939025406
     accelerations:
       - 0
       - 0
@@ -2586,24 +2586,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 43815355
+      sec: 1
+      nanosec: 546900459
   - positions:
-      - 0.54861355436399506
-      - -0.30684346732683893
-      - -0.43938132829399418
-      - -2.1137083678091497
-      - 0.07644063892417037
-      - 0.15294456626694874
-      - -2.2294820854961501
+      - 1.2121749761961278
+      - -1.7713832184419236
+      - 1.0382300965462721
+      - -1.58939948152343
+      - -0.35814516675644542
+      - -0.086939680638636593
+      - -0.78804254396567541
     velocities:
-      - -0.011590094037309632
-      - 0.032958390572057655
-      - -0.076883607228132492
-      - 0.014435154203984168
-      - -0.0027327382910696642
-      - -0.81949158004276457
-      - 0.044152078733200678
+      - -0.50343149179400071
+      - 0.28551732449890188
+      - -0.19841711435269965
+      - -0.025075336631819836
+      - -0.50194585722673912
+      - -0.14118824233888261
+      - -0.095474933929674904
     accelerations:
       - 0
       - 0
@@ -2614,24 +2614,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 55463356
+      sec: 1
+      nanosec: 553279418
   - positions:
-      - 0.54845044752747418
-      - -0.30652058772582136
-      - -0.44017478419423572
-      - -2.1135632406158602
-      - 0.076412753152636556
-      - 0.14447291695611816
-      - -2.2290324967604533
+      - 1.209186744348576
+      - -1.769685595552321
+      - 1.0370452645423316
+      - -1.5895479271860222
+      - -0.36112901225264671
+      - -0.087775584300126872
+      - -0.78860844346421577
     velocities:
-      - -0.016635082796330535
-      - 0.033206890489095878
-      - -0.081804304652270038
-      - 0.014976498339559973
-      - -0.0028760893785679986
-      - -0.86702980264301466
-      - 0.046365180656878585
+      - -0.49726128225519345
+      - 0.28267051658418241
+      - -0.19786806242657881
+      - -0.024699351414795363
+      - -0.49642011183212581
+      - -0.13870019986843379
+      - -0.094095781161121661
     accelerations:
       - 0
       - 0
@@ -2642,24 +2642,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 67077716
+      sec: 1
+      nanosec: 559938459
   - positions:
-      - 0.54825193730629374
-      - -0.30619117085759096
-      - -0.44101331208775751
-      - -2.1134123761483852
-      - 0.076383508275257722
-      - 0.13555427614151339
-      - -2.2285618264877338
+      - 1.2062684257113674
+      - -1.7680203309795108
+      - 1.0358819818976481
+      - -1.5896929799603796
+      - -0.36404060662102911
+      - -0.088591394142839563
+      - -0.78915946816603133
     velocities:
-      - -0.023725808051261666
-      - 0.03185751978729405
-      - -0.084708074002243713
-      - 0.014905076183277943
-      - -0.0029256060689360625
-      - -0.90480227807053404
-      - 0.046996302728935867
+      - -0.48662987461196616
+      - 0.27727092226387479
+      - -0.1953435130065927
+      - -0.024013527701489183
+      - -0.48777453111225189
+      - -0.13522431645237271
+      - -0.091902504911014871
     accelerations:
       - 0
       - 0
@@ -2670,24 +2670,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 76903192
+      sec: 1
+      nanosec: 567609376
   - positions:
-      - 0.54799572711946321
-      - -0.30587039271501038
-      - -0.44188542404043318
-      - -2.1132597989987749
-      - 0.076353437858555903
-      - 0.12633668403972564
-      - -2.2280791050808828
+      - 1.2033907869023659
+      - -1.7663732887185604
+      - 1.0347257169301463
+      - -1.589835960977281
+      - -0.36691004501713576
+      - -0.089390869916913307
+      - -0.78970133687331745
     velocities:
-      - -0.027535602885803603
-      - 0.031795579450538125
-      - -0.088549227920303827
-      - 0.015376259783676302
-      - -0.0030459211524612039
-      - -0.92953797127294213
-      - 0.048861097291357215
+      - -0.47252574699031136
+      - 0.27124396233897485
+      - -0.18970721439232371
+      - -0.023508137343771798
+      - -0.47074935161583237
+      - -0.1316546893401043
+      - -0.088851477797319722
     accelerations:
       - 0
       - 0
@@ -2698,24 +2698,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 86732378
+      sec: 1
+      nanosec: 573425459
   - positions:
-      - 0.54767306091777945
-      - -0.30557257699876361
-      - -0.44277013439994989
-      - -2.1131106671152966
-      - 0.076323402480298064
-      - 0.11702525680917773
-      - -2.2275987483050246
+      - 1.1996096783044248
+      - -1.764203839880857
+      - 1.0331891375140485
+      - -1.590022788871299
+      - -0.37069117916381467
+      - -0.090431432951794857
+      - -0.7904114222656361
     velocities:
-      - -0.033916306033531525
-      - 0.029289712255909064
-      - -0.089317564344828959
-      - 0.014973328841952268
-      - -0.0030296231565801156
-      - -0.93002202505847209
-      - 0.048423049357160131
+      - -0.47114938425341807
+      - 0.27089574485047802
+      - -0.19212421808107732
+      - -0.023317594056253978
+      - -0.4703585720957687
+      - -0.12913981848309941
+      - -0.088324226847143639
     accelerations:
       - 0
       - 0
@@ -2726,24 +2726,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 97397415
+      sec: 1
+      nanosec: 581686084
   - positions:
-      - 0.54731555917337626
-      - -0.3052914589434228
-      - -0.44365592249490804
-      - -2.1129643877954707
-      - 0.076293573458310518
-      - 0.10779414500448883
-      - -2.2271230570242611
+      - 1.1958809168748357
+      - -1.7620516765681524
+      - 1.0316606244533852
+      - -1.5902069283030933
+      - -0.37441650596624154
+      - -0.091449618194071575
+      - -0.79110853817653959
     velocities:
-      - -0.036877170041837125
-      - 0.027499985561649926
-      - -0.088503069137961687
-      - 0.014543454525139792
-      - -0.0029777418897879611
-      - -0.91607522830559207
-      - 0.047451253568223217
+      - -0.46508205664977065
+      - 0.26885125173264457
+      - -0.19151505138310346
+      - -0.02291701935475312
+      - -0.46511516822354226
+      - -0.12647961320196602
+      - -0.086823182968741572
     accelerations:
       - 0
       - 0
@@ -2754,24 +2754,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 108973116
+      sec: 1
+      nanosec: 591582168
   - positions:
-      - 0.54695090432576976
-      - -0.30502015552767797
-      - -0.44453152042292265
-      - -2.112820650193751
-      - 0.076264143857379188
-      - 0.098766068508351276
-      - -2.2266547005238433
+      - 1.1922945815422121
+      - -1.7599659666917462
+      - 1.0301863195448999
+      - -1.5903847264185604
+      - -0.37798755658797462
+      - -0.0924312965432873
+      - -0.79177563389506112
     velocities:
-      - -0.036459362037053952
-      - 0.026693359403858051
-      - -0.086608591654542724
-      - 0.014176573721696386
-      - -0.0029067493051275987
-      - -0.89281776254611545
-      - 0.046213492821373034
+      - -0.43896442284236309
+      - 0.25601891281705536
+      - -0.1803873345590499
+      - -0.021877281260014921
+      - -0.435436761063074
+      - -0.1204428061409375
+      - -0.081478871178650852
     accelerations:
       - 0
       - 0
@@ -2782,24 +2782,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 118916122
+      sec: 1
+      nanosec: 599942293
   - positions:
-      - 0.54658129299437364
-      - -0.30476282929928805
-      - -0.44538302276133729
-      - -2.1126819816650011
-      - 0.076235597645128897
-      - 0.090055333501659493
-      - -2.2262014244980195
+      - 1.1887007776883174
+      - -1.7578776747274441
+      - 1.0286812684516859
+      - -1.5905608699437039
+      - -0.38158912541911139
+      - -0.09339598184188648
+      - -0.7924423765458809
     velocities:
-      - -0.036451729561369078
-      - 0.025327071467873417
-      - -0.083866560181371827
-      - 0.013647553589061201
-      - -0.002809926765233093
-      - -0.8566272170974204
-      - 0.044579153268501127
+      - -0.44988195552127608
+      - 0.26198573564491706
+      - -0.18912997306183776
+      - -0.022017002391013299
+      - -0.45102389221425931
+      - -0.12021962765215562
+      - -0.083308382289668623
     accelerations:
       - 0
       - 0
@@ -2810,24 +2810,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 128887647
+      sec: 1
+      nanosec: 609277918
   - positions:
-      - 0.5462372914717265
-      - -0.30450819284089053
-      - -0.44620722814542702
-      - -2.1125462896907261
-      - 0.076207826430295583
-      - 0.081704029246606358
-      - -2.2257608759838892
+      - 1.1852015817978072
+      - -1.7558271958988241
+      - 1.0272121107180752
+      - -1.5907331220169112
+      - -0.38508183192221945
+      - -0.094335900962468466
+      - -0.7930878074015798
     velocities:
-      - -0.03543857849900249
-      - 0.024233544729426904
-      - -0.080536754273034777
-      - 0.01310187219803173
-      - -0.0027008005549578442
-      - -0.8165866711962575
-      - 0.042781323293903904
+      - -0.43874302545773164
+      - 0.25674045035604603
+      - -0.18579461724930754
+      - -0.021432611437851294
+      - -0.44003574481883345
+      - -0.11691682152374452
+      - -0.08087377900517724
     accelerations:
       - 0
       - 0
@@ -2838,24 +2838,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 138986880
+      sec: 1
+      nanosec: 616665418
   - positions:
-      - 0.5458445766812271
-      - -0.30421805167252353
-      - -0.44714720903276495
-      - -2.1123911746539759
-      - 0.076176090204516156
-      - 0.072293366440801837
-      - -2.225258148413392
+      - 1.1817307867937827
+      - -1.7537870858797826
+      - 1.0257391947301968
+      - -1.5909034501248898
+      - -0.3885493879475993
+      - -0.095259762226684391
+      - -0.7937251327190179
     velocities:
-      - -0.030146553206307068
-      - 0.024351586310066016
-      - -0.076479862935589768
-      - 0.012782781374878288
-      - -0.0025953639570862009
-      - -0.76220986807865754
-      - 0.041116602713852653
+      - -0.43267256365284473
+      - 0.25473625818400875
+      - -0.18421678558026333
+      - -0.021229604696519622
+      - -0.43210964087515219
+      - -0.11486191740220818
+      - -0.079297086751221699
     accelerations:
       - 0
       - 0
@@ -2866,24 +2866,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 150762206
+      sec: 1
+      nanosec: 625048959
   - positions:
-      - 0.54549832301690993
-      - -0.30393650987247328
-      - -0.44802681823612805
-      - -2.112243893336363
-      - 0.076146202207492159
-      - 0.063562025006931644
-      - -2.224784929685288
+      - 1.1783343250497427
+      - -1.7517800316899177
+      - 1.0242892465188855
+      - -1.5910701043238515
+      - -0.39194002800577465
+      - -0.096162279596270286
+      - -0.79434552409203796
     velocities:
-      - -0.028016721591904784
-      - 0.022736894235438593
-      - -0.070942335958672234
-      - 0.011880316095957067
-      - -0.0024113314588016135
-      - -0.70207602746017039
-      - 0.038160242191660448
+      - -0.42290533508568484
+      - 0.25003122260557653
+      - -0.18130685221409959
+      - -0.020734609423046495
+      - -0.4222379717030228
+      - -0.11199419461949307
+      - -0.077120431579035045
     accelerations:
       - 0
       - 0
@@ -2894,24 +2894,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 163806971
+      sec: 1
+      nanosec: 635051293
   - positions:
-      - 0.54514333503868617
-      - -0.30362655760351781
-      - -0.44897044141082676
-      - -2.1120840744403506
-      - 0.076113962038189889
-      - 0.054306645886423623
-      - -2.224274632920539
+      - 1.1749600664230759
+      - -1.7497796866303321
+      - 1.0228351356529994
+      - -1.5912353197788049
+      - -0.39530950199760917
+      - -0.097052057971946548
+      - -0.79495872871167517
     velocities:
-      - -0.023855745317054072
-      - 0.021343648068804993
-      - -0.064374172418428258
-      - 0.010936489964017762
-      - -0.0022020658582389669
-      - -0.62910652694499025
-      - 0.03484255524413752
+      - -0.41512911110521555
+      - 0.24708316689890633
+      - -0.17861587516357841
+      - -0.020400294375784247
+      - -0.41340104526317356
+      - -0.10991926495129371
+      - -0.075235580074748787
     accelerations:
       - 0
       - 0
@@ -2922,24 +2922,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 178843009
+      sec: 1
+      nanosec: 643524959
   - positions:
-      - 0.54483656626593191
-      - -0.30334309826947464
-      - -0.44981594597621583
-      - -2.1119396031398976
-      - 0.076084950146235966
-      - 0.046116284729423453
-      - -2.2238156357399266
+      - 1.1716399234186095
+      - -1.7478045337815242
+      - 1.0213927565077583
+      - -1.5913973632664156
+      - -0.39862944503306824
+      - -0.097924234796764251
+      - -0.7955590031969797
     velocities:
-      - -0.020147858171727616
-      - 0.019585295896144338
-      - -0.057570648077626012
-      - 0.0099248975460183714
-      - -0.0019848085295923481
-      - -0.55176179424621807
-      - 0.031411238803529508
+      - -0.41458460823454601
+      - 0.24659716886729294
+      - -0.18114017588103282
+      - -0.020129546397388495
+      - -0.41589844733015668
+      - -0.10842365175746695
+      - -0.074845486767320321
     accelerations:
       - 0
       - 0
@@ -2950,24 +2950,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 192033418
+      sec: 1
+      nanosec: 651902501
   - positions:
-      - 0.54455126751590166
-      - -0.30305181069183001
-      - -0.45065749799193822
-      - -2.1117937791322161
-      - 0.07605588950159213
-      - 0.038090404548299317
-      - -2.2233559720173073
+      - 1.168393911765667
+      - -1.7458654325598599
+      - 1.0199747054195325
+      - -1.591555939092413
+      - -0.40187061177995781
+      - -0.098775491608131233
+      - -0.79614259936887843
     velocities:
-      - -0.016503426581748571
-      - 0.016943112069280255
-      - -0.048737694337596831
-      - 0.0084532317904434592
-      - -0.0016840021948236691
-      - -0.46213467935164987
-      - 0.026622875456172781
+      - -0.40615361697146507
+      - 0.24223242407542342
+      - -0.17878835552933856
+      - -0.019715117437203229
+      - -0.407185028436094
+      - -0.10578073684045367
+      - -0.072945879977010172
     accelerations:
       - 0
       - 0
@@ -2978,24 +2978,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 208824732
+      sec: 1
+      nanosec: 659911584
   - positions:
-      - 0.54428847072708475
-      - -0.30273918883874756
-      - -0.45152105023302125
-      - -2.1116410720253938
-      - 0.076025785143026339
-      - 0.030041233741720907
-      - -2.2228797691564206
+      - 1.1651830481383323
+      - -1.7439432649109567
+      - 1.0185592194949571
+      - -1.5917124420722353
+      - -0.40507849182085265
+      - -0.099610962544059226
+      - -0.79671687306086059
     velocities:
-      - -0.011108727190305779
-      - 0.014209150257413877
-      - -0.038442802649309403
-      - 0.0068579051124580404
-      - -0.0013455513934464219
-      - -0.35376238137104887
-      - 0.021283693695885942
+      - -0.40024291494889314
+      - 0.23992023342202362
+      - -0.17690905622036973
+      - -0.019520223185352563
+      - -0.39951494356152145
+      - -0.10387438371493055
+      - -0.071427180697160544
     accelerations:
       - 0
       - 0
@@ -3006,24 +3006,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 230315971
+      sec: 1
+      nanosec: 668381584
   - positions:
-      - 0.54403188138551839
-      - -0.30240639287093168
-      - -0.45241662367591612
-      - -2.1114809486252497
-      - 0.075994396914137091
-      - 0.022058942567246734
-      - -2.2223834167003185
+      - 1.1620307498527029
+      - -1.7420483227933381
+      - 1.0171614986552531
+      - -1.5918659596044618
+      - -0.40822687263717927
+      - -0.10042866682866666
+      - -0.79727741081767356
     velocities:
-      - -0.0058580033791463962
-      - 0.0085746712608180779
-      - -0.02245730177109357
-      - 0.0040691402139068598
-      - -0.0007917864026630193
-      - -0.19006240994427404
-      - 0.012532074512685308
+      - -0.39330843962909984
+      - 0.23646161969392596
+      - -0.17512089478303094
+      - -0.019135173088642102
+      - -0.39292314493992286
+      - -0.1016133618968776
+      - -0.069808654736626619
     accelerations:
       - 0
       - 0
@@ -3034,24 +3034,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 260341235
+      sec: 1
+      nanosec: 676746209
   - positions:
-      - 0.54514349618754554
-      - -0.30082551315654926
-      - -0.45348057906666134
-      - -2.111832764809082
-      - 0.076068751054490194
-      - 0.016294864505183096
-      - -2.2225123081281302
+      - 1.1589361691036928
+      - -1.7401825473720309
+      - 1.0157800285119651
+      - -1.5920164825585621
+      - -0.41131758887474335
+      - -0.10122777836172757
+      - -0.79782463099985412
     velocities:
-      - 0.022352898462104042
-      - 0.03117423637358183
-      - -0.0093367019478977388
-      - -0.01521978278745603
-      - 0.0029823155407073527
-      - -0.017343323521667866
-      - -0.017450267382393317
+      - -0.38612260311153018
+      - 0.23281563218653867
+      - -0.17308451606476238
+      - -0.018763683363007853
+      - -0.38572385123708691
+      - -0.099281308604956847
+      - -0.068151545954940301
     accelerations:
       - 0
       - 0
@@ -3062,24 +3062,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 363746071
+      sec: 1
+      nanosec: 685202418
   - positions:
-      - 0.54693397896088947
-      - -0.29800560183853575
-      - -0.45425815663049735
-      - -2.1133394991415564
-      - 0.07635995477750486
-      - 0.014879035059525661
-      - -2.2242876581230595
+      - 1.1558998478750366
+      - -1.7383463858054748
+      - 1.0144158511103916
+      - -1.5921640202966449
+      - -0.41434963466289715
+      - -0.1020081407977
+      - -0.79835851684404024
     velocities:
-      - 0.036173381412014656
-      - 0.060302329989777005
-      - -0.016371641856982068
-      - -0.033551287053892397
-      - 0.0064490984353765251
-      - -0.029805379326003533
-      - -0.039874673541714693
+      - -0.37811518346323164
+      - 0.22872610714858674
+      - -0.17048707185679701
+      - -0.018362928677474868
+      - -0.37757085023443809
+      - -0.096825614423946105
+      - -0.066356549910455434
     accelerations:
       - 0
       - 0
@@ -3090,24 +3090,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 429098490
+      sec: 1
+      nanosec: 695024418
   - positions:
-      - 0.54861128860564556
-      - -0.29515471636155827
-      - -0.45502335912920572
-      - -2.1149377254535127
-      - 0.076666897557126384
-      - 0.013467101303695508
-      - -2.2261934728741362
+      - 1.1528980352233877
+      - -1.7365270063320355
+      - 1.0130566797774661
+      - -1.5923095812547932
+      - -0.41734841940448214
+      - -0.10277375954925445
+      - -0.79888335195856586
     velocities:
-      - 0.047919773420113568
-      - 0.082243487924776773
-      - -0.022020679436626733
-      - -0.046309401792512299
-      - 0.0088889719262937542
-      - -0.040626491003098335
-      - -0.055411880764436802
+      - -0.37198732226006581
+      - 0.22605188533349313
+      - -0.16837729128008824
+      - -0.018099057191833336
+      - -0.37059045295075577
+      - -0.094919319513125433
+      - -0.064859147548977369
     accelerations:
       - 0
       - 0
@@ -3118,24 +3118,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 468607855
+      sec: 1
+      nanosec: 703481918
   - positions:
-      - 0.55028598568236664
-      - -0.2922501983860375
-      - -0.45579309523851141
-      - -2.1165698818931284
-      - 0.076980381347053234
-      - 0.012031933838010269
-      - -2.2281549111490881
+      - 1.1499506231753178
+      - -1.7347343424820107
+      - 1.0117148596286389
+      - -1.5924523549336951
+      - -0.42029234848778252
+      - -0.10352263808706522
+      - -0.79939559001235749
     velocities:
-      - 0.057115973622191764
-      - 0.099630823473047345
-      - -0.026772660407554123
-      - -0.056465157597620559
-      - 0.010851252258430555
-      - -0.04885588433438353
-      - -0.068161961148117151
+      - -0.36541095596024042
+      - 0.22265196740550985
+      - -0.16647350213718778
+      - -0.017739077101148418
+      - -0.36430275710707061
+      - -0.09279884100516031
+      - -0.06334619048693034
     accelerations:
       - 0
       - 0
@@ -3146,24 +3146,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 502018083
+      sec: 1
+      nanosec: 711662001
   - positions:
-      - 0.55186247334929506
-      - -0.28948634371499304
-      - -0.45651988525089326
-      - -2.118117461449871
-      - 0.077278004005726861
-      - 0.010676213145856782
-      - -2.2300321987805596
+      - 1.147033578323873
+      - -1.7329554444010189
+      - 1.0103780386584036
+      - -1.5925934736460794
+      - -0.42320559753495313
+      - -0.10425912759689272
+      - -0.7998995061464419
     velocities:
-      - 0.064585104363768298
-      - 0.11362262413004359
-      - -0.030159124558442982
-      - -0.063929529905693297
-      - 0.012305788235816901
-      - -0.055316596021357453
-      - -0.077913724478008511
+      - -0.35861636752743459
+      - 0.21947137366438998
+      - -0.16397452476614457
+      - -0.017422657288422235
+      - -0.35700278104862393
+      - -0.090895868128681376
+      - -0.061768179011689489
     accelerations:
       - 0
       - 0
@@ -3174,24 +3174,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 528664555
+      sec: 1
+      nanosec: 719926710
   - positions:
-      - 0.55349353661063261
-      - -0.28660245394209521
-      - -0.4572706018583117
-      - -2.1197210992108433
-      - 0.077586959942171962
-      - 0.0092726807115661447
-      - -2.2319974053460796
+      - 1.1428345789550296
+      - -1.7303795363219077
+      - 1.0084501688206897
+      - -1.5927973371740927
+      - -0.42738540999272978
+      - -0.10532174487793756
+      - -0.80061917039222064
     velocities:
-      - 0.070586771897532791
-      - 0.12544352811563578
-      - -0.032289516294608765
-      - -0.069332989018802305
-      - 0.01337187194859955
-      - -0.060981049850696904
-      - -0.085361315862259646
+      - -0.32805914671230513
+      - 0.20293899473766569
+      - -0.14904932775114446
+      - -0.016122394970646995
+      - -0.32383852618652897
+      - -0.084471916651844961
+      - -0.055941509700488279
     accelerations:
       - 0
       - 0
@@ -3202,24 +3202,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 553778848
+      sec: 1
+      nanosec: 733312501
   - positions:
-      - 0.55513754122053915
-      - -0.2836734929788518
-      - -0.45803065643496826
-      - -2.121342069474037
-      - 0.077899867213612389
-      - 0.0078604144540563269
-      - -2.2340060245661517
+      - 1.1399579017566186
+      - -1.7286286906271973
+      - 1.0070940422772587
+      - -1.5929341874216381
+      - -0.43028504244601135
+      - -0.10602589230539983
+      - -0.80110988588123144
     velocities:
-      - 0.077906117382388049
-      - 0.1391078924206533
-      - -0.035517363311102348
-      - -0.076278208013955781
-      - 0.014742410466431944
-      - -0.066948055527062414
-      - -0.094992403429939018
+      - -0.35890889356365091
+      - 0.22016160704516469
+      - -0.16773424191525585
+      - -0.017335811825861847
+      - -0.35765632351076415
+      - -0.088319269376487625
+      - -0.060914925746602414
     accelerations:
       - 0
       - 0
@@ -3230,24 +3230,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 577049291
+      sec: 1
+      nanosec: 743288085
   - positions:
-      - 0.55675387408962185
-      - -0.28076828657569591
-      - -0.45877517686077862
-      - -2.1229355526381664
-      - 0.078208123764774054
-      - 0.0064680724338473557
-      - -2.2360018071445666
+      - 1.1370825849531139
+      - -1.72685875635806
+      - 1.0057455412561698
+      - -1.5930723135793585
+      - -0.4331591916527453
+      - -0.10673354305558891
+      - -0.80159570293901861
     velocities:
-      - 0.083735039328653907
-      - 0.15110647307450545
-      - -0.039260550725771975
-      - -0.083376801293236311
-      - 0.016143584416958411
-      - -0.071948200318367808
-      - -0.10493072125928775
+      - -0.35873485455499704
+      - 0.22110746953778587
+      - -0.16863214167988683
+      - -0.01719812493082342
+      - -0.35899634024052396
+      - -0.088309319507940104
+      - -0.060459321101762359
     accelerations:
       - 0
       - 0
@@ -3258,24 +3258,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 597144016
+      sec: 1
+      nanosec: 751565585
   - positions:
-      - 0.558299447875657
-      - -0.27796904504907344
-      - -0.45948712445052864
-      - -2.1244603321230988
-      - 0.078503607972880113
-      - 0.0051341958031179916
-      - -2.2379300891259017
+      - 1.1342267148228475
+      - -1.7250956372645081
+      - 1.0043992988086248
+      - -1.5932093752836938
+      - -0.43601374217175443
+      - -0.10743663149351194
+      - -0.80207495854055955
     velocities:
-      - 0.088573617460318249
-      - 0.16083028146345177
-      - -0.041451667376799145
-      - -0.088079317444021241
-      - 0.017081696767870705
-      - -0.076128003880775355
-      - -0.11188131338195836
+      - -0.35827358225948858
+      - 0.22111771875628758
+      - -0.16964892029804823
+      - -0.017129821133598764
+      - -0.35895635062037184
+      - -0.087921440986552774
+      - -0.059986219262608977
     accelerations:
       - 0
       - 0
@@ -3286,24 +3286,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 615213478
+      sec: 1
+      nanosec: 761828335
   - positions:
-      - 0.55993886412908456
-      - -0.27498236636423029
-      - -0.46024232583846192
-      - -2.1260770914646363
-      - 0.078817475440755749
-      - 0.0037212042171318829
-      - -2.2399946141549427
+      - 1.1313989221845551
+      - -1.7233445673403607
+      - 1.0030611061013011
+      - -1.5933452589722019
+      - -0.43883575639866867
+      - -0.10813302736113044
+      - -0.80254619173275055
     velocities:
-      - 0.093449120791515711
-      - 0.1707492936708046
-      - -0.043097561516546611
-      - -0.092159695677994938
-      - 0.017902823671996571
-      - -0.080523534294966254
-      - -0.11821398236956852
+      - -0.3512159286770094
+      - 0.21770225281061961
+      - -0.16641684672555915
+      - -0.016895416214058469
+      - -0.35014127652940708
+      - -0.086519564085194317
+      - -0.058380709867844535
     accelerations:
       - 0
       - 0
@@ -3314,24 +3314,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 634156402
+      sec: 1
+      nanosec: 770004835
   - positions:
-      - 0.56146489223313167
-      - -0.27218015775609233
-      - -0.46094379052662821
-      - -2.1275821277841387
-      - 0.07911020604089708
-      - 0.0024020617808527297
-      - -2.241935238235687
+      - 1.1279807903816321
+      - -1.7212225744607774
+      - 1.0014367866669958
+      - -1.5935094873225306
+      - -0.44224602896755655
+      - -0.1089761258880608
+      - -0.80311179862672033
     velocities:
-      - 0.097296508570121051
-      - 0.1794488658193012
-      - -0.045524519072290062
-      - -0.096926862816599804
-      - 0.018867439476934623
-      - -0.084004342699114595
-      - -0.12554317451503202
+      - -0.33996231880797673
+      - 0.21086780822734269
+      - -0.16230295130499151
+      - -0.016317195712775113
+      - -0.33931416862682973
+      - -0.083407118636183913
+      - -0.056099924470025868
     accelerations:
       - 0
       - 0
@@ -3342,24 +3342,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 650376276
+      sec: 1
+      nanosec: 781657501
   - positions:
-      - 0.563045209549208
-      - -0.2692548495451686
-      - -0.46167136550020343
-      - -2.1291428979125602
-      - 0.079414324177242937
-      - 0.0010327135609595355
-      - -2.243967953371683
+      - 1.1246142892582414
+      - -1.7191311545929309
+      - 0.99982281989387944
+      - -1.5936707121527687
+      - -0.44560902573822592
+      - -0.10979790966762744
+      - -0.80366460661786165
     velocities:
-      - 0.1013052507473874
-      - 0.18797135038985907
-      - -0.047749564573371948
-      - -0.10112840994653702
-      - 0.019719998728785743
-      - -0.087229549823661379
-      - -0.1324021495640165
+      - -0.33416707974065984
+      - 0.20759442657281235
+      - -0.16101468095473218
+      - -0.01591375000315005
+      - -0.33503132463426633
+      - -0.08119623880124037
+      - -0.054715255674958253
     accelerations:
       - 0
       - 0
@@ -3370,24 +3370,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 666911889
+      sec: 1
+      nanosec: 791851335
   - positions:
-      - 0.56470413114353679
-      - -0.26616864273134611
-      - -0.46243607632365674
-      - -2.130779301115107
-      - 0.079733848500346496
-      - -0.00039857113279173959
-      - -2.2461234286059408
+      - 1.1213387355156923
+      - -1.7170909441162652
+      - 0.9982440958824339
+      - -1.5938275575149432
+      - -0.44887824499224799
+      - -0.1105921408754931
+      - -0.80419822193020774
     velocities:
-      - 0.10601912125188116
-      - 0.1980546426870769
-      - -0.049461216086742005
-      - -0.10530618845944316
-      - 0.020583947239524735
-      - -0.091347142865341527
-      - -0.13943285108694783
+      - -0.32331147680192762
+      - 0.20172209533079524
+      - -0.15593897794722059
+      - -0.015509462347043573
+      - -0.32213426170088716
+      - -0.078253056782134595
+      - -0.05249117958855342
     accelerations:
       - 0
       - 0
@@ -3398,24 +3398,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 683635786
+      sec: 1
+      nanosec: 803533293
   - positions:
-      - 0.56620969837035262
-      - -0.26334439316460945
-      - -0.46312648650531091
-      - -2.1322617914686206
-      - 0.080023912612371809
-      - -0.0017021668832473988
-      - -2.2480969174222567
+      - 1.1175344526440985
+      - -1.7147152132714281
+      - 0.99639943273803833
+      - -1.5940094597879426
+      - -0.45267479684732392
+      - -0.11150644793905658
+      - -0.80481268327112043
     velocities:
-      - 0.10829673475156699
-      - 0.20397716935913468
-      - -0.049010721961740049
-      - -0.10609953790214874
-      - 0.020779584605722119
-      - -0.094255317317862075
-      - -0.14178758001533051
+      - -0.30938692976782334
+      - 0.19393633134271368
+      - -0.14955639680639488
+      - -0.014919833654925753
+      - -0.30669065378683802
+      - -0.074433922683860745
+      - -0.049745662131301681
     accelerations:
       - 0
       - 0
@@ -3426,24 +3426,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 698780068
+      sec: 1
+      nanosec: 816440501
   - positions:
-      - 0.5677572950439943
-      - -0.26041572335302871
-      - -0.46383571733139484
-      - -2.1337871301546842
-      - 0.080322927545340012
-      - -0.0030480252756970717
-      - -2.2501479892099598
+      - 1.1144606013600484
+      - -1.7127939483691499
+      - 0.99489606143980336
+      - -1.5941557647651958
+      - -0.45574894887276607
+      - -0.11223565888600379
+      - -0.80530519092300945
     velocities:
-      - 0.11043374323306608
-      - 0.20984971562064644
-      - -0.049444295819318029
-      - -0.10763754687743395
-      - 0.021111454002253619
-      - -0.096775027559718688
-      - -0.14525616082018511
+      - -0.30645165879300545
+      - 0.19188344684136044
+      - -0.14995880989684446
+      - -0.014629162339112524
+      - -0.30562537103738163
+      - -0.07235528344987123
+      - -0.048920306171020632
     accelerations:
       - 0
       - 0
@@ -3454,24 +3454,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 714129416
+      sec: 1
+      nanosec: 826617251
   - positions:
-      - 0.56934361485593499
-      - -0.25739089958557543
-      - -0.46456500638670589
-      - -2.1353531682370601
-      - 0.080630504398370398
-      - -0.0044290989925722408
-      - -2.2522760438418379
+      - 1.1114243082912869
+      - -1.71088989637789
+      - 0.99340387501482785
+      - -1.5943001472629519
+      - -0.45878306364593247
+      - -0.11294853017269113
+      - -0.80578754529454444
     velocities:
-      - 0.11371673773689875
-      - 0.21771841712127571
-      - -0.051385440565540105
-      - -0.11149428025454497
-      - 0.021920570569364115
-      - -0.099518217798021683
-      - -0.15211435736035109
+      - -0.30122379363167318
+      - 0.18890526037833183
+      - -0.14872307776803251
+      - -0.014241622103824033
+      - -0.30213565172382634
+      - -0.070411187448015125
+      - -0.047711270225648278
     accelerations:
       - 0
       - 0
@@ -3482,24 +3482,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 727263329
+      sec: 1
+      nanosec: 836661960
   - positions:
-      - 0.5709856006150148
-      - -0.25424006785222947
-      - -0.4653217887362347
-      - -2.1369742106416458
-      - 0.080949522942316984
-      - -0.0058556140542316316
-      - -2.2545039086216092
+      - 1.1077570215140418
+      - -1.7085864823510597
+      - 0.99158658972075564
+      - -1.5944740806169539
+      - -0.46244828388397474
+      - -0.11379789700793788
+      - -0.80636463437912664
     velocities:
-      - 0.11807895521175359
-      - 0.22737204633736743
-      - -0.053302483671909831
-      - -0.11548539516994112
-      - 0.022750509059247439
-      - -0.10308581785846879
-      - -0.15939524950766629
+      - -0.29681393616086205
+      - 0.18774724431911016
+      - -0.14588334079268095
+      - -0.014246334430727736
+      - -0.29394798099519026
+      - -0.069449554047224818
+      - -0.046445944535309543
     accelerations:
       - 0
       - 0
@@ -3510,24 +3510,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 742457787
+      sec: 1
+      nanosec: 853407626
   - positions:
-      - 0.57265091720440719
-      - -0.25100959111041488
-      - -0.46608642290623559
-      - -2.1386193956655943
-      - 0.081273982997990227
-      - -0.0073125967304835887
-      - -2.2567896592884309
+      - 1.1047973192808391
+      - -1.706722801479728
+      - 0.99011210077177025
+      - -1.5946140319709818
+      - -0.46541042514353681
+      - -0.11448177479154265
+      - -0.80682617049741512
     velocities:
-      - 0.12139237513749812
-      - 0.23615920619583258
-      - -0.057399310140899933
-      - -0.1215536404256731
-      - 0.023994846750981322
-      - -0.10549499859832831
-      - -0.16988092424336076
+      - -0.2985097004621165
+      - 0.18789180677961878
+      - -0.14912449295811314
+      - -0.014153291182010882
+      - -0.29797226377975128
+      - -0.068479893266718328
+      - -0.04639245796334205
     accelerations:
       - 0
       - 0
@@ -3538,24 +3538,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 757331276
+      sec: 1
+      nanosec: 863620126
   - positions:
-      - 0.57412565420104567
-      - -0.24813570150041564
-      - -0.46676348731843664
-      - -2.1400720633858694
-      - 0.081561026599747927
-      - -0.0085975434430898921
-      - -2.2588310975589696
+      - 1.1018942643827441
+      - -1.7048875555983369
+      - 0.98866372088961141
+      - -1.5947517264878401
+      - -0.46830630490124731
+      - -0.11515062588310979
+      - -0.80727480601220425
     velocities:
-      - 0.12331251083412334
-      - 0.24127071205561773
-      - -0.055778468062531839
-      - -0.12081070044682726
-      - 0.023896375143123271
-      - -0.10796638631465838
-      - -0.17042274419315984
+      - -0.28766732528976441
+      - 0.18171385089932418
+      - -0.14411392718972321
+      - -0.013611301544363109
+      - -0.28736862812367364
+      - -0.066017883181913761
+      - -0.044335201859702349
     accelerations:
       - 0
       - 0
@@ -3566,24 +3566,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 768984473
+      sec: 1
+      nanosec: 875207418
   - positions:
-      - 0.57563723185425753
-      - -0.24516552673664321
-      - -0.46745963938904495
-      - -2.141564356855326
-      - 0.081856490655267358
-      - -0.0099169839446294709
-      - -2.2609508046017446
+      - 1.0990778670426935
+      - -1.7031051957148831
+      - 0.9872515400232913
+      - -1.5948851835358584
+      - -0.47111555016865886
+      - -0.11579579759869081
+      - -0.80770667969664955
     velocities:
-      - 0.12731744573657069
-      - 0.25086932899004727
-      - -0.05859062570360557
-      - -0.12551604056221879
-      - 0.024865774994990587
-      - -0.11117829367691272
-      - -0.17909096863184248
+      - -0.27577890532079619
+      - 0.17495054638237525
+      - -0.13807723366134261
+      - -0.013106323540886401
+      - -0.27441599337619771
+      - -0.063264410258819917
+      - -0.042138816142796146
     accelerations:
       - 0
       - 0
@@ -3594,24 +3594,220 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 2
-      nanosec: 780358871
+      sec: 1
+      nanosec: 885203418
+  - positions:
+      - 1.0957974402262507
+      - -1.7010280798121142
+      - 0.98559498431466108
+      - -1.5950400479107436
+      - -0.47439324493162999
+      - -0.1165394169598117
+      - -0.80820552191677719
+    velocities:
+      - -0.26849940392200627
+      - 0.17036786069497828
+      - -0.13550929197705128
+      - -0.012720418917189598
+      - -0.26744947994411444
+      - -0.060751770189174395
+      - -0.040655284246323184
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 898431460
+  - positions:
+      - 1.0925911366065266
+      - -1.6989963497870424
+      - 0.98396291465361396
+      - -1.5951907150058844
+      - -0.47760335972627121
+      - -0.11725632741892816
+      - -0.80868851375086015
+    velocities:
+      - -0.27003074301248592
+      - 0.17057275895079549
+      - -0.13890533484529921
+      - -0.01254294535144832
+      - -0.27242644465985072
+      - -0.059545202054599101
+      - -0.040534254959801411
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 913319585
+  - positions:
+      - 1.0894039869581595
+      - -1.6969710190777887
+      - 0.98233343269447926
+      - -1.5953405843098791
+      - -0.48078746286238871
+      - -0.11796134554582802
+      - -0.80916358664696386
+    velocities:
+      - -0.26362516604327652
+      - 0.1676519037440995
+      - -0.13532464687557855
+      - -0.012324742318794239
+      - -0.26436658278592295
+      - -0.058144505487818889
+      - -0.039124396288172499
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 926678710
+  - positions:
+      - 1.0863315423469824
+      - -1.6950112999739122
+      - 0.9807598803632267
+      - -1.5954852435621503
+      - -0.48385121646769413
+      - -0.11864030367982452
+      - -0.80961696185360454
+    velocities:
+      - -0.24965131241762289
+      - 0.1595750064214414
+      - -0.12774669154839738
+      - -0.011780051316461326
+      - -0.24848032838206283
+      - -0.05524789316250886
+      - -0.036685694465912293
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 940188835
+  - positions:
+      - 1.0833370650509382
+      - -1.6931048766929413
+      - 0.97921002031713356
+      - -1.59562510297466
+      - -0.48685198837311194
+      - -0.11929293684140944
+      - -0.81005502531897911
+    velocities:
+      - -0.25438694954063329
+      - 0.16130986604696526
+      - -0.13309760133688237
+      - -0.011755536614143287
+      - -0.25670479093192811
+      - -0.05455937667970135
+      - -0.037082341032876269
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 958395960
+  - positions:
+      - 1.0803293147557025
+      - -1.6911840029824092
+      - 0.97764901775954871
+      - -1.595765946785445
+      - -0.48985512033646855
+      - -0.11994178279329072
+      - -0.81049025458226376
+    velocities:
+      - -0.24659014572669818
+      - 0.15792544183969806
+      - -0.1278478424416066
+      - -0.011563773394392022
+      - -0.24584311449386367
+      - -0.053339329575255758
+      - -0.035518097848914382
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 970154460
+  - positions:
+      - 1.0774782052619518
+      - -1.6893557763692388
+      - 0.9761689560874085
+      - -1.5958995840134482
+      - -0.49269820391305513
+      - -0.12055896835809346
+      - -0.81089876425922558
+    velocities:
+      - -0.23380381993475455
+      - 0.14966195630379137
+      - -0.12208224453020336
+      - -0.010893751729738487
+      - -0.23412202651041517
+      - -0.050295713761481002
+      - -0.033388514895879462
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 1
+      nanosec: 983631126
   - positions:
-      - 0.57717338533802443
-      - -0.24212054209488063
-      - -0.46816182068598489
-      - -2.1430768505931628
-      - 0.082156543601946491
-      - -0.011264867322967378
-      - -2.2631218450853594
+      - 1.0743198368211109
+      - -1.6873322220578324
+      - 0.97451830896259528
+      - -1.5960472362107627
+      - -0.4958512264081868
+      - -0.12123455223134895
+      - -0.81134734854201251
     velocities:
-      - 0.12830901040499781
-      - 0.25536605428889786
-      - -0.058049228612880491
-      - -0.12585262200128125
-      - 0.024989432836426899
-      - -0.11305591992448358
-      - -0.18139007301913065
+      - -0.2214933433463348
+      - 0.14187916062704414
+      - -0.11599128132709496
+      - -0.010375772539471952
+      - -0.22064669963952632
+      - -0.047015115836426029
+      - -0.031335308396399919
     accelerations:
       - 0
       - 0
@@ -3623,23 +3819,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 793537729
+      nanosec: 8168
   - positions:
-      - 0.578731959411935
-      - -0.23900268149855022
-      - -0.46887253789661776
-      - -2.1446113989955142
-      - 0.082461576517955146
-      - -0.012638461777622212
-      - -2.2653483435940416
+      - 1.0712978516155143
+      - -1.6853968523027176
+      - 0.97292645737016326
+      - -1.5961878103714575
+      - -0.49887529531369557
+      - -0.12186948210840168
+      - -0.8117725014899726
     velocities:
-      - 0.12851512912494956
-      - 0.25886480904785253
-      - -0.057850696882999625
-      - -0.12620779149650635
-      - 0.025109231023668521
-      - -0.11431946467366084
-      - -0.18377278384379089
+      - -0.21792701829261707
+      - 0.13917294256626614
+      - -0.11546155029762432
+      - -0.010136721266830087
+      - -0.21788658415863238
+      - -0.045086256402629886
+      - -0.030557189896147963
     accelerations:
       - 0
       - 0
@@ -3651,23 +3847,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 805030757
+      nanosec: 18285668
   - positions:
-      - 0.58031952052648461
-      - -0.23580294744003427
-      - -0.46960736303274836
-      - -2.1461834661084582
-      - 0.082774634525339952
-      - -0.014034627324551546
-      - -2.2676561063893286
+      - 1.0683392827169116
+      - -1.6834947537120619
+      - 0.97136612025915992
+      - -1.5963257090916962
+      - -0.5018265591484381
+      - -0.12248473217927407
+      - -0.81218435292277413
     velocities:
-      - 0.13379956685551833
-      - 0.27001771923606793
-      - -0.061182139629290762
-      - -0.13156949809077675
-      - 0.026226361355758478
-      - -0.11757559847852656
-      - -0.19406015326804973
+      - -0.20436017572198384
+      - 0.13172706533758469
+      - -0.10758032494972954
+      - -0.0095504126355709061
+      - -0.20340028283547334
+      - -0.042621166754196992
+      - -0.028331204080301556
     accelerations:
       - 0
       - 0
@@ -3679,23 +3875,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 818659498
+      nanosec: 33453543
   - positions:
-      - 0.58192377680461327
-      - -0.23254064017089812
-      - -0.47033909956696923
-      - -2.1477630519766198
-      - 0.083089886275592689
-      - -0.01545251311931089
-      - -2.270000525364658
+      - 1.0651825182904577
+      - -1.6814648125971221
+      - 0.96969198462376471
+      - -1.5964722074715469
+      - -0.50498378385189946
+      - -0.12313665783822557
+      - -0.81262015607083482
     velocities:
-      - 0.13379717063491381
-      - 0.27318915426897972
-      - -0.060526875238523251
-      - -0.13133450676086131
-      - 0.026235260977268724
-      - -0.11863866948627826
-      - -0.19579193567895661
+      - -0.19817972413539406
+      - 0.12675140929841705
+      - -0.10639542349893223
+      - -0.0090846515969953429
+      - -0.19989594228348806
+      - -0.040181692532940044
+      - -0.027284129130929587
     accelerations:
       - 0
       - 0
@@ -3707,23 +3903,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 830250739
+      nanosec: 53263085
   - positions:
-      - 0.58356918797836221
-      - -0.22917164744394902
-      - -0.47109961578719173
-      - -2.149389879467237
-      - 0.083415231343644536
-      - -0.016900967523246779
-      - -2.2724457458261096
+      - 1.0621470570548943
+      - -1.6795092768651569
+      - 0.96807825502823341
+      - -1.5966132774160782
+      - -0.50801241033496047
+      - -0.12375686685637037
+      - -0.81303541761751619
     velocities:
-      - 0.13882389343872178
-      - 0.28478845549728204
-      - -0.063664282249447257
-      - -0.13657741108391819
-      - 0.027340072140406468
-      - -0.12211192876541675
-      - -0.20634410994036204
+      - -0.18333798471836812
+      - 0.11824351102565665
+      - -0.0975329087176003
+      - -0.0085194550877871252
+      - -0.18288539550036989
+      - -0.037446921588281751
+      - -0.024985071054286469
     accelerations:
       - 0
       - 0
@@ -3735,23 +3931,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 843886425
+      nanosec: 70084668
   - positions:
-      - 0.58523392563421583
-      - -0.22572883345208691
-      - -0.47185841717496074
-      - -2.1510275221277055
-      - 0.083743495263197271
-      - -0.01837558315758259
-      - -2.2749362669319098
+      - 1.0590172180886788
+      - -1.6774907223976392
+      - 0.96640876243948559
+      - -1.5967584977106641
+      - -0.51113701400947642
+      - -0.12439251562097788
+      - -0.81345982913167636
     velocities:
-      - 0.1387174757604471
-      - 0.28819956098362098
-      - -0.062525942427904144
-      - -0.13589152447790048
-      - 0.027264626663636743
-      - -0.12343564732803185
-      - -0.20761568430945612
+      - -0.16800714102696282
+      - 0.10830854335867782
+      - -0.089860718992082672
+      - -0.0077899916209226453
+      - -0.16772424286822693
+      - -0.033867141106513732
+      - -0.02269226212575615
     accelerations:
       - 0
       - 0
@@ -3763,23 +3959,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 855632227
+      nanosec: 90064043
   - positions:
-      - 0.5866425255869685
-      - -0.22279003812224724
-      - -0.4725063775130271
-      - -2.1524189986435753
-      - 0.084022974761851985
-      - -0.019624506881656976
-      - -2.2770774741613882
+      - 1.0561487780672372
+      - -1.67563947637661
+      - 0.96487201291567237
+      - -1.5968913601974557
+      - -0.51400125930743945
+      - -0.12496712673705707
+      - -0.81384508639671949
     velocities:
-      - 0.14181991381403453
-      - 0.2969087021922775
-      - -0.065800990126860942
-      - -0.1407287379727897
-      - 0.028291167746289245
-      - -0.12568181448817445
-      - -0.21754403299362246
+      - -0.15300552633990486
+      - 0.098707007305688335
+      - -0.082313332864565242
+      - -0.0070553794336147731
+      - -0.1532201691233159
+      - -0.030407990033524609
+      - -0.020458730219977819
     accelerations:
       - 0
       - 0
@@ -3791,23 +3987,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 865422741
+      nanosec: 110661751
   - positions:
-      - 0.58806549747044135
-      - -0.21979732107605637
-      - -0.47315845803670642
-      - -2.1538222270928151
-      - 0.084305327865813656
-      - -0.020889817918132984
-      - -2.2792596083134975
+      - 1.0531312322037427
+      - -1.6736939518955931
+      - 0.96324137447795188
+      - -1.5970304144963983
+      - -0.51702082994168175
+      - -0.12555884469279355
+      - -0.81424581865841983
     velocities:
-      - 0.14319880594126935
-      - 0.30204515168334534
-      - -0.065929387411763754
-      - -0.14138554022645689
-      - 0.02846473669639658
-      - -0.12726792896573819
-      - -0.22084906134393117
+      - -0.14534083694944719
+      - 0.094196392863211714
+      - -0.078147302839268351
+      - -0.0067396440572881702
+      - -0.1446389070340571
+      - -0.028591849999395218
+      - -0.019163128804827387
     accelerations:
       - 0
       - 0
@@ -3819,23 +4015,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 875206332
+      nanosec: 138527126
   - positions:
-      - 0.58950173825823116
-      - -0.21675192770836008
-      - -0.4738162127652033
-      - -2.1552382385268811
-      - 0.08459078705539208
-      - -0.02216972708885875
-      - -2.2814855001643104
+      - 1.0501198793604394
+      - -1.671744177346812
+      - 0.96161298283531416
+      - -1.5971693689193334
+      - -0.52002732732206358
+      - -0.12614735022293283
+      - -0.81464023011865283
     velocities:
-      - 0.14427607736461659
-      - 0.30670408004774929
-      - -0.065698200204233018
-      - -0.14179321090181235
-      - 0.02860543455956541
-      - -0.12866054191083998
-      - -0.22384093470619557
+      - -0.12885458585705167
+      - 0.083471544103633111
+      - -0.069790230032598058
+      - -0.0059471081104727043
+      - -0.12856756080196399
+      - -0.025065107098780716
+      - -0.016783694963902449
     accelerations:
       - 0
       - 0
@@ -3847,23 +4043,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 886807802
+      nanosec: 163494626
   - positions:
-      - 0.59095373509009441
-      - -0.21364834001493099
-      - -0.47448083241301747
-      - -2.1566693378264601
-      - 0.084879843775941849
-      - -0.023465545780227307
-      - -2.2837601198629454
+      - 1.0470161280130295
+      - -1.6697338578831495
+      - 0.95992587377748773
+      - -1.5973123333845547
+      - -0.52312654765282907
+      - -0.12674374841446645
+      - -0.81504192309082302
     velocities:
-      - 0.14528254349431891
-      - 0.31165728736238785
-      - -0.065863469285003404
-      - -0.14267434338442714
-      - 0.028842014759519408
-      - -0.13003630197951274
-      - -0.22769076410610084
+      - -0.10916992349124936
+      - 0.070937102245842557
+      - -0.059208900337039766
+      - -0.0050426228236833447
+      - -0.1087213421044209
+      - -0.020916610864647502
+      - -0.014047088075014156
     accelerations:
       - 0
       - 0
@@ -3875,23 +4071,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 897037059
+      nanosec: 196811918
   - positions:
-      - 0.59240938639709628
-      - -0.21050663825737356
-      - -0.47514528761617042
-      - -2.1581039865950395
-      - 0.085170157814460912
-      - -0.02477059265438836
-      - -2.286064999911523
+      - 1.0441407356258039
+      - -1.6678689603276322
+      - 0.95835843844639113
+      - -1.5974446220001881
+      - -0.52599771449310273
+      - -0.12728521201823617
+      - -0.81541051576256285
     velocities:
-      - 0.1453260241127769
-      - 0.31477382827059014
-      - -0.065654492967555614
-      - -0.14265404105633805
-      - 0.028890522372030228
-      - -0.13068567293221539
-      - -0.23010136288731711
+      - -0.085061929136335188
+      - 0.054987619083475665
+      - -0.046800254982104772
+      - -0.0038684904113432559
+      - -0.085642033287221545
+      - -0.015657521488932395
+      - -0.0108725122189223
     accelerations:
       - 0
       - 0
@@ -3903,23 +4099,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 906859362
+      nanosec: 230003085
   - positions:
-      - 0.59388096466754225
-      - -0.20730642020188003
-      - -0.47581885707257027
-      - -2.1595548606250938
-      - 0.085464315085578418
-      - -0.02608950005036928
-      - -2.2884229148798809
+      - 1.0412424321780869
+      - -1.6659896303075539
+      - 0.9567716370511844
+      - -1.5975776307731573
+      - -0.5288938487195638
+      - -0.12781011667166448
+      - -0.81577964275270043
     velocities:
-      - 0.14611954414395797
-      - 0.3193395217094584
-      - -0.065780121334122879
-      - -0.14330857879013167
-      - 0.029080658589257714
-      - -0.13176789467087513
-      - -0.23375397196530326
+      - -0.054376425435373493
+      - 0.035290292170515228
+      - -0.029795876987181703
+      - -0.0024919061885138186
+      - -0.054375537098022099
+      - -0.0096379715213938018
+      - -0.0069037461828551604
     accelerations:
       - 0
       - 0
@@ -3931,23 +4127,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 916967949
+      nanosec: 278348460
   - positions:
-      - 0.59536308164627139
-      - -0.20405419985429471
-      - -0.47650162567397858
-      - -2.1610205048116091
-      - 0.08576204552259295
-      - -0.027419197447628066
-      - -2.2908328141018082
+      - 1.0386481587640559
+      - -1.6645653557580338
+      - 0.95589317867710344
+      - -1.5976858474229543
+      - -0.53144565914437714
+      - -0.13000023447560147
+      - -0.81604405462495644
     velocities:
-      - 0.14926938673980425
-      - 0.32872063455410461
-      - -0.069791784826626277
-      - -0.14860890346854363
-      - 0.030216661263445743
-      - -0.13363597705494948
-      - -0.24564756646917238
+      - -0.022380788370069449
+      - -0.0021870957646336984
+      - 0.0095286771321352933
+      - -0.00081391619803566776
+      - -0.021400383777683524
+      - -0.093601983637760358
+      - 0.00014536461576979614
     accelerations:
       - 0
       - 0
@@ -3959,23 +4155,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 927182034
+      nanosec: 381665876
   - positions:
-      - 0.5968700024733925
-      - -0.20072587177278628
-      - -0.47719491176516915
-      - -2.1625063306284371
-      - 0.086064463772107358
-      - -0.028768484418138177
-      - -2.2933059591550826
+      - 1.0375915876853699
+      - -1.6651541588240171
+      - 0.95678447876540373
+      - -1.5977222965155613
+      - -0.53245197780345532
+      - -0.13653182373147305
+      - -0.81596865408463992
     velocities:
-      - 0.15121271406213335
-      - 0.33504070081586845
-      - -0.068744019980465254
-      - -0.1483237991028126
-      - 0.030216547059255479
-      - -0.13567443512440969
-      - -0.24799873039989731
+      - -0.031818349382947919
+      - -0.028281726870771559
+      - 0.035088484463585527
+      - -0.0012255420257415142
+      - -0.030227434528129853
+      - -0.24367698838974702
+      - 0.0037435802241955036
     accelerations:
       - 0
       - 0
@@ -3987,23 +4183,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 936992041
+      nanosec: 430102502
   - positions:
-      - 0.5983803491798807
-      - -0.19735304643792057
-      - -0.4778850336458717
-      - -2.1639940593162943
-      - 0.086367916170772441
-      - -0.030129306688531123
-      - -2.2958111714859286
+      - 1.0367579917422161
+      - -1.6661411570172906
+      - 0.95777622339764434
+      - -1.5977569595060941
+      - -0.53326723443861546
+      - -0.14366323236470041
+      - -0.81585113192939196
     velocities:
-      - 0.15154445860538138
-      - 0.33942641164072457
-      - -0.068900811262170999
-      - -0.14881248522694518
-      - 0.030374408490207092
-      - -0.13665383744140935
-      - -0.25177276672831866
+      - -0.036320722616651535
+      - -0.051369329911191926
+      - 0.045214259047682177
+      - -0.0016840136269488486
+      - -0.036230362584717582
+      - -0.33640333649169402
+      - 0.0058069894865738318
     accelerations:
       - 0
       - 0
@@ -4015,23 +4211,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 948799277
+      nanosec: 456673543
   - positions:
-      - 0.59990299237388889
-      - -0.19392158371811938
-      - -0.47858175289417676
-      - -2.1654942630222416
-      - 0.086674533935424319
-      - -0.031503606870589056
-      - -2.2983688172335097
+      - 1.036047554558726
+      - -1.6671829443323822
+      - 0.95868321739301743
+      - -1.5977888975307575
+      - -0.53397905723395833
+      - -0.15031906314932109
+      - -0.81573338232840875
     velocities:
-      - 0.15209136156584188
-      - 0.34411612194796359
-      - -0.068734052598921361
-      - -0.149127903728413
-      - 0.030505338365164846
-      - -0.13774205606291623
-      - -0.25537112910638998
+      - -0.04197995631982538
+      - -0.062569313999908088
+      - 0.054449294033037374
+      - -0.0018985360513404793
+      - -0.041972565587209236
+      - -0.39602071456704185
+      - 0.0071640804351308495
     accelerations:
       - 0
       - 0
@@ -4043,23 +4239,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 959336197
+      nanosec: 475072877
   - positions:
-      - 0.60142926971864774
-      - -0.19044502243256042
-      - -0.47928335868722977
-      - -2.1670023330825274
-      - 0.086983394844825687
-      - -0.032885749141772017
-      - -2.3009720721551985
+      - 1.0352600884244303
+      - -1.668237982911597
+      - 0.95971565591175745
+      - -1.5978204305183463
+      - -0.53474991515723636
+      - -0.15740983158549096
+      - -0.81560273122065285
     velocities:
-      - 0.15433225339457549
-      - 0.35251135984904336
-      - -0.072270322530265232
-      - -0.15353312236954189
-      - 0.031474264229892551
-      - -0.13914892192730852
-      - -0.26664477270374326
+      - -0.052644756169337198
+      - -0.071794031272698483
+      - 0.069696940565521517
+      - -0.0021487835685271598
+      - -0.051621845054983524
+      - -0.47989015013374792
+      - 0.0090040674175962884
     accelerations:
       - 0
       - 0
@@ -4071,23 +4267,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 968432759
+      nanosec: 496883752
   - positions:
-      - 0.60297724420861942
-      - -0.18689163743811035
-      - -0.47999499287478414
-      - -2.1685287176221024
-      - 0.087296651973044634
-      - -0.034285655349701669
-      - -2.3036419956372756
+      - 1.0345114847443477
+      - -1.6693695076402204
+      - 0.96071296588888189
+      - -1.5978544430557273
+      - -0.53549513348612499
+      - -0.16455086352848353
+      - -0.81546561756207336
     velocities:
-      - 0.15592929824034396
-      - 0.35917367842151116
-      - -0.072159687171132816
-      - -0.15401116776354315
-      - 0.031628811806387346
-      - -0.1408457363133902
-      - -0.27089399708630324
+      - -0.055771650140587201
+      - -0.085980951165863309
+      - 0.074193015548467023
+      - -0.0024445816517729663
+      - -0.056003677836715497
+      - -0.5322907228748095
+      - 0.010292865639357111
     accelerations:
       - 0
       - 0
@@ -4099,23 +4295,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 978737129
+      nanosec: 511729210
   - positions:
-      - 0.60453590108025501
-      - -0.18327658605233055
-      - -0.48070829803178217
-      - -2.1700631437708746
-      - 0.087612243369389969
-      - -0.035699977129919125
-      - -2.3063610836375914
+      - 1.0338458573849587
+      - -1.6704771687390969
+      - 0.96160353808199006
+      - -1.5978868721122224
+      - -0.53617029097428415
+      - -0.17114542057629209
+      - -0.81533572318776504
     velocities:
-      - 0.15616493500455136
-      - 0.36362686295697993
-      - -0.071004411499664133
-      - -0.15329966815182305
-      - 0.031555827499880021
-      - -0.14197350659091643
-      - -0.27303061571099474
+      - -0.057276969612135784
+      - -0.095120357623037416
+      - 0.076306619758970753
+      - -0.0026774894117042765
+      - -0.058230356436398852
+      - -0.56144048378582667
+      - 0.011111175858022656
     accelerations:
       - 0
       - 0
@@ -4127,23 +4323,23 @@ points:
     effort: []
     time_from_start:
       sec: 2
-      nanosec: 988590142
+      nanosec: 525094002
   - positions:
-      - 0.60609292030205242
-      - -0.17962151459546466
-      - -0.48141992032385478
-      - -2.1715975400739476
-      - 0.087928490943296717
-      - -0.037120886664129341
-      - -2.3091152653084834
+      - 1.0330977912821186
+      - -1.6716313636035265
+      - 0.96260065077517276
+      - -1.5979201604227877
+      - -0.53691764433373834
+      - -0.17823984667944176
+      - -0.81519389974666334
     velocities:
-      - 0.15607254899858142
-      - 0.36757089578714852
-      - -0.071079252437548368
-      - -0.15340182231908492
-      - 0.031639044769688263
-      - -0.14248251299727402
-      - -0.27678465828121052
+      - -0.063095761070065079
+      - -0.099686228470243193
+      - 0.084632637347506035
+      - -0.0030027816865733545
+      - -0.063037541159203417
+      - -0.6092013011607994
+      - 0.012319571930614049
     accelerations:
       - 0
       - 0
@@ -4154,24 +4350,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 210857
+      sec: 2
+      nanosec: 538419585
   - positions:
-      - 0.60765885161765731
-      - -0.17590974540451912
-      - -0.48213738757697344
-      - -2.1731410676251124
-      - 0.088247288604465318
-      - -0.03855106238402764
-      - -2.3119236003390986
+      - 1.0323057662041417
+      - -1.6728803091143805
+      - 0.96365906677071267
+      - -1.5979563879327428
+      - -0.53771172299513992
+      - -0.18582283459568813
+      - -0.81503963820292913
     velocities:
-      - 0.15640608453885785
-      - 0.37229025572594038
-      - -0.070871042615191754
-      - -0.15354437944550586
-      - 0.031743108736115952
-      - -0.14323107775118568
-      - -0.2807470888645206
+      - -0.067981466486670875
+      - -0.10664273185486624
+      - 0.090885039714147908
+      - -0.003162168175454391
+      - -0.067891043670368148
+      - -0.65007323105254045
+      - 0.013338560247331288
     accelerations:
       - 0
       - 0
@@ -4182,24 +4378,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 11049591
+      sec: 2
+      nanosec: 550123793
   - positions:
-      - 0.6092231685374293
-      - -0.17215958055508113
-      - -0.48285220588321726
-      - -2.1746825332800324
-      - 0.088566335331357035
-      - -0.039985309359379294
-      - -2.3147653437625091
+      - 1.0315893354045207
+      - -1.6739777211433815
+      - 0.96461266620573172
+      - -1.5979878352421928
+      - -0.53842690658035475
+      - -0.19255888968672555
+      - -0.81490077128778793
     velocities:
-      - 0.15585142834907736
-      - 0.37533884994816391
-      - -0.070493244226491197
-      - -0.15305778218566607
-      - 0.031708683622172183
-      - -0.14334245141589233
-      - -0.2835454292989254
+      - -0.075734332265180157
+      - -0.11038718809276288
+      - 0.1002174017707874
+      - -0.0031475572988108004
+      - -0.074839982809125252
+      - -0.69290852915769252
+      - 0.014357410945820444
     accelerations:
       - 0
       - 0
@@ -4210,24 +4406,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 21085832
+      sec: 2
+      nanosec: 563296377
   - positions:
-      - 0.61078772826920735
-      - -0.16836794086771611
-      - -0.48357336495015496
-      - -2.1762296353456874
-      - 0.088887214577548279
-      - -0.041421260664821553
-      - -2.3176569440395971
+      - 1.0308107111686404
+      - -1.6751255742966671
+      - 0.9656462846850179
+      - -1.5980217540623056
+      - -0.53919621075246293
+      - -0.19976454383372991
+      - -0.81474984400511008
     velocities:
-      - 0.15793768446339529
-      - 0.38386915653753323
-      - -0.074038248436276474
-      - -0.1570424007162162
-      - 0.032603572166007268
-      - -0.14430935839438427
-      - -0.29543643071197917
+      - -0.07935354573240061
+      - -0.11793334665901005
+      - 0.10548019209804962
+      - -0.003594821412088901
+      - -0.078263407646291874
+      - -0.73989914001834889
+      - 0.015624549162601514
     accelerations:
       - 0
       - 0
@@ -4238,24 +4434,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 30522646
+      sec: 2
+      nanosec: 575031252
   - positions:
-      - 0.61236542041960773
-      - -0.16451089506519065
-      - -0.48429728765056995
-      - -2.1777838823330136
-      - 0.089210268706928914
-      - -0.042868042003712942
-      - -2.3206029397360566
+      - 1.0300326934824755
+      - -1.6763868405563396
+      - 0.96668245490659932
+      - -1.5980596146420487
+      - -0.53997840915245543
+      - -0.20730866092914024
+      - -0.81458919483145742
     velocities:
-      - 0.1579827548019353
-      - 0.38794731267438604
-      - -0.072597084684514021
-      - -0.15564536003723856
-      - 0.032372560372790932
-      - -0.14497553302731359
-      - -0.29666179762593403
+      - -0.076786144578695592
+      - -0.13096618760714407
+      - 0.10216421640089673
+      - -0.0038823362643565459
+      - -0.078089577251181694
+      - -0.76036387514888648
+      - 0.016285366535262923
     accelerations:
       - 0
       - 0
@@ -4266,24 +4462,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 40550642
+      sec: 2
+      nanosec: 586618918
   - positions:
-      - 0.61393712361526043
-      - -0.16062115118941325
-      - -0.48501526142261225
-      - -2.1793313647707224
-      - 0.089532616349042485
-      - -0.044316335221295723
-      - -2.3235762258613732
+      - 1.0292403755999537
+      - -1.677717915827375
+      - 0.96772808340560845
+      - -1.598097758817296
+      - -0.54078480038897703
+      - -0.21502458372265004
+      - -0.81442276805169456
     velocities:
-      - 0.1568247124832895
-      - 0.38988941341881983
-      - -0.071320199024029363
-      - -0.15414739169407479
-      - 0.032135942700315896
-      - -0.14478525009260074
-      - -0.2977628917924604
+      - -0.080941077705890707
+      - -0.1336474058419162
+      - 0.1064418276045901
+      - -0.003886645744076541
+      - -0.081885602954429593
+      - -0.78012109035999033
+      - 0.016960365516871218
     accelerations:
       - 0
       - 0
@@ -4294,24 +4490,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 52400348
+      sec: 2
+      nanosec: 598343085
   - positions:
-      - 0.61548278149978786
-      - -0.15674292874495557
-      - -0.48571724742411554
-      - -2.1808536367478153
-      - 0.089850356499566936
-      - -0.045751602837877929
-      - -2.3265391855155659
+      - 1.0284398666719803
+      - -1.6790874493972741
+      - 0.96878108798372164
+      - -1.5981373724120174
+      - -0.54160239439353608
+      - -0.22286275842317643
+      - -0.8142509130484934
     velocities:
-      - 0.15377831383250798
-      - 0.38744226354822953
-      - -0.069609740657495214
-      - -0.15117858769478071
-      - 0.031576968199174403
-      - -0.142950581872373
-      - -0.29580213422040413
+      - -0.082585544109104306
+      - -0.13711138161416372
+      - 0.10777308543662871
+      - -0.0037443017202916729
+      - -0.084426893541154652
+      - -0.78899674344861304
+      - 0.017335083871281735
     accelerations:
       - 0
       - 0
@@ -4322,24 +4518,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 62145274
+      sec: 2
+      nanosec: 611628585
   - positions:
-      - 0.61701347909127147
-      - -0.15286038788483788
-      - -0.48641967539607706
-      - -2.1823666290665895
-      - 0.090166795728184396
-      - -0.047172706721994827
-      - -2.329525945069272
+      - 1.0276371246484823
+      - -1.6804810835933226
+      - 0.96983364144079676
+      - -1.5981783274668884
+      - -0.54242351459038385
+      - -0.23075284836199952
+      - -0.81407516537780267
     velocities:
-      - 0.15336322468166724
-      - 0.39046609187408954
-      - -0.071510129678429679
-      - -0.1524536184502564
-      - 0.031917397059796369
-      - -0.14190410050431027
-      - -0.30291850701357786
+      - -0.078437687470505058
+      - -0.14133606660234002
+      - 0.10301955244766381
+      - -0.0041944073694222721
+      - -0.080812519592694559
+      - -0.78552572656580211
+      - 0.017603463020365733
     accelerations:
       - 0
       - 0
@@ -4350,24 +4546,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 71841611
+      sec: 2
+      nanosec: 621657960
   - positions:
-      - 0.61852597216911964
-      - -0.14898334927794124
-      - -0.48710817804962442
-      - -2.1838565461842592
-      - 0.090479054439477194
-      - -0.048578790843029573
-      - -2.33250739836362
+      - 1.026877708797739
+      - -1.6819077908451123
+      - 0.9708281696214458
+      - -1.5982195402614761
+      - -0.54321612488867455
+      - -0.23845951833188719
+      - -0.8139013278487689
     velocities:
-      - 0.15017158539748052
-      - 0.38738195443383472
-      - -0.069584987143462271
-      - -0.14923665404371886
-      - 0.031309074562061986
-      - -0.13965635055761935
-      - -0.30045501995380797
+      - -0.075084249296265823
+      - -0.1421981052225289
+      - 0.097977114725297049
+      - -0.0040169775659948514
+      - -0.078745451482165454
+      - -0.76012809486037469
+      - 0.017218740654540314
     accelerations:
       - 0
       - 0
@@ -4378,24 +4574,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 81723535
+      sec: 2
+      nanosec: 633548127
   - positions:
-      - 0.62000977949338365
-      - -0.14513453072171623
-      - -0.48778701333458607
-      - -2.1853216442242771
-      - 0.09078668882487019
-      - -0.049960244136030403
-      - -2.3354792851011994
+      - 1.0261476879461258
+      - -1.6833059407957911
+      - 0.97177916260375763
+      - -1.5982595445352208
+      - -0.54398230220433774
+      - -0.24586263566050973
+      - -0.81373193930662502
     velocities:
-      - 0.14781563727355757
-      - 0.38477853646761728
-      - -0.068024794985584017
-      - -0.14608102908397669
-      - 0.030691955743920266
-      - -0.13735241988083891
-      - -0.29802242396355066
+      - -0.071293187164915214
+      - -0.13804754487195744
+      - 0.09285279965087273
+      - -0.0039884874584402309
+      - -0.074933477776905746
+      - -0.72560995457872957
+      - 0.016714253583866592
     accelerations:
       - 0
       - 0
@@ -4406,24 +4602,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 92017130
+      sec: 2
+      nanosec: 643477085
   - positions:
-      - 0.62146844260871892
-      - -0.14131274016358233
-      - -0.48845349139054056
-      - -2.1867602136190518
-      - 0.091089359822865562
-      - -0.051317243989247911
-      - -2.3384373669702487
+      - 1.025436222461342
+      - -1.6846371956031676
+      - 0.97269941739688814
+      - -1.5982970589737266
+      - -0.54472692800886691
+      - -0.25290973148691592
+      - -0.81356829819830312
     velocities:
-      - 0.14479271699079574
-      - 0.3807758156021519
-      - -0.065722918990567453
-      - -0.14236238541230267
-      - 0.029975496729278992
-      - -0.13471584395931885
-      - -0.29426780896715582
+      - -0.071740878316400317
+      - -0.1289997340219747
+      - 0.092416527647518135
+      - -0.0036275905447817293
+      - -0.074459028146220449
+      - -0.69355843845783649
+      - 0.016208725927403845
     accelerations:
       - 0
       - 0
@@ -4434,24 +4630,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 104196994
+      sec: 2
+      nanosec: 657010835
   - positions:
-      - 0.62290170288797864
-      - -0.13751870274415165
-      - -0.48910914667375555
-      - -2.1881736191467458
-      - 0.091387327753845379
-      - -0.052649358825924052
-      - -2.3413837469123004
+      - 1.0247422366802281
+      - -1.6858906583380728
+      - 0.97359457321178078
+      - -1.5983334071901891
+      - -0.54544583986396522
+      - -0.25963878313380673
+      - -0.81340936487169524
     velocities:
-      - 0.14193168471854145
-      - 0.37719771939332492
-      - -0.064075665283749444
-      - -0.13926735122349815
-      - 0.029383577656248411
-      - -0.13211126713709842
-      - -0.29174659162708022
+      - -0.067529366550904094
+      - -0.12324576998891998
+      - 0.08713271524943661
+      - -0.0036160628173203834
+      - -0.070050630846023282
+      - -0.65766045320147914
+      - 0.015635046855329873
     accelerations:
       - 0
       - 0
@@ -4462,24 +4658,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 113666738
+      sec: 2
+      nanosec: 666658127
   - positions:
-      - 0.62430629090590617
-      - -0.13376042248206632
-      - -0.48974910018794066
-      - -2.1895567558288396
-      - 0.091679495858059859
-      - -0.053955749872253551
-      - -2.3443058250775985
+      - 1.0239603922594533
+      - -1.6873122860571748
+      - 0.97459854598216322
+      - -1.5983744514959364
+      - -0.54625833844534932
+      - -0.26717591149363651
+      - -0.81322840796447693
     velocities:
-      - 0.13804250212738958
-      - 0.37155775195240404
-      - -0.062109147081769622
-      - -0.13557726294479114
-      - 0.028662855715885206
-      - -0.12898180837175224
-      - -0.28772225003827295
+      - -0.0636390902726615
+      - -0.11432836767775148
+      - 0.081513487433249418
+      - -0.0033470137975523569
+      - -0.065834318602917219
+      - -0.60772285664034864
+      - 0.014720404443724621
     accelerations:
       - 0
       - 0
@@ -4490,24 +4686,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 123833270
+      sec: 2
+      nanosec: 682385418
   - positions:
-      - 0.62568136609872294
-      - -0.13004116663719872
-      - -0.49038404903098115
-      - -2.1909168774918046
-      - 0.091967319242474616
-      - -0.055232905903373958
-      - -2.347219244690804
+      - 1.0232266286578442
+      - -1.6886181456295979
+      - 0.97553512016373944
+      - -1.5984120897367824
+      - -0.5470182338094931
+      - -0.27409972754849071
+      - -0.81305914002003199
     velocities:
-      - 0.13711152611318628
-      - 0.37180878873161677
-      - -0.063561974601040266
-      - -0.13552441703639662
-      - 0.02869639853441527
-      - -0.12695190381435092
-      - -0.29197799185746415
+      - -0.061228389703552828
+      - -0.10356660003904153
+      - 0.077514707498456653
+      - -0.0028886758369140227
+      - -0.062951877737743092
+      - -0.55766667212589571
+      - 0.013727071757120939
     accelerations:
       - 0
       - 0
@@ -4518,24 +4714,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 133341809
+      sec: 2
+      nanosec: 697083002
   - positions:
-      - 0.62701966334503645
-      - -0.12638375875112046
-      - -0.49098996827463393
-      - -2.1922311876643672
-      - 0.092245976452476369
-      - -0.05647935950503722
-      - -2.3500698149892139
+      - 1.0225020559360056
+      - -1.6897806797412678
+      - 0.97645292273382267
+      - -1.5984462895639762
+      - -0.54775130846071185
+      - -0.2805577003540955
+      - -0.81289845607561495
     velocities:
-      - 0.13106438632050954
-      - 0.36011350782562029
-      - -0.057930345054768212
-      - -0.12770536760895157
-      - 0.027086088373723432
-      - -0.12276205841424902
-      - -0.27800912290712004
+      - -0.05848220509772982
+      - -0.093759188207064387
+      - 0.074282535701839875
+      - -0.0028539222087897854
+      - -0.058986093071062486
+      - -0.52299845907970421
+      - 0.013096835451641387
     accelerations:
       - 0
       - 0
@@ -4546,24 +4742,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 145474432
+      sec: 2
+      nanosec: 711749293
   - positions:
-      - 0.62832629095018233
-      - -0.12277459907195526
-      - -0.49159570135924219
-      - -2.1935248058271011
-      - 0.092520727496387359
-      - -0.057692830601121695
-      - -2.3529148876150159
+      - 1.0217407361823017
+      - -1.6910461569696182
+      - 0.97741897047839077
+      - -1.5984842433860897
+      - -0.54852678161578416
+      - -0.28744526451731817
+      - -0.81272478339245813
     velocities:
-      - 0.1304700987789448
-      - 0.36090261721085504
-      - -0.060469555011011707
-      - -0.12878856999591506
-      - 0.027372427700110585
-      - -0.12065690345265463
-      - -0.28488820508740931
+      - -0.050998047855295513
+      - -0.087454776619189351
+      - 0.064648170017975876
+      - -0.0026107047097674516
+      - -0.052316993264274483
+      - -0.46592478391546871
+      - 0.011824046471119487
     accelerations:
       - 0
       - 0
@@ -4574,24 +4770,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 155455045
+      sec: 2
+      nanosec: 726667668
   - positions:
-      - 0.62960695112132903
-      - -0.11920729453570357
-      - -0.49218076793604104
-      - -2.1947836718676692
-      - 0.092788602102931952
-      - -0.058880851194118289
-      - -2.3557199465451131
+      - 1.0209862868168618
+      - -1.6923370648825382
+      - 0.97837150015624608
+      - -1.5985222459334896
+      - -0.54930176295716127
+      - -0.29425464466951712
+      - -0.81255008080963942
     velocities:
-      - 0.1261183161081591
-      - 0.35307643385964133
-      - -0.056742296778763142
-      - -0.12343121270171764
-      - 0.026285451775092169
-      - -0.11741449251069812
-      - -0.27628702176558684
+      - -0.044335835023285357
+      - -0.074937497481888826
+      - 0.055779361032332254
+      - -0.0021773087073358776
+      - -0.045507434048443478
+      - -0.393906160179887
+      - 0.010203716473595406
     accelerations:
       - 0
       - 0
@@ -4602,24 +4798,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 165229028
+      sec: 2
+      nanosec: 743472168
   - positions:
-      - 0.6308573809127167
-      - -0.11568465565918845
-      - -0.49275708421522785
-      - -2.1960171555356602
-      - 0.093051557064270665
-      - -0.060041260084553218
-      - -2.3585058680714361
+      - 1.020251919622764
+      - -1.693529489197978
+      - 0.97929313412850638
+      - -1.5985575180793643
+      - -0.55004804902667881
+      - -0.30059979223853917
+      - -0.8123831622026072
     velocities:
-      - 0.1239905580729164
-      - 0.35099329094382503
-      - -0.057938360409025481
-      - -0.12299540393240849
-      - 0.026243743724080313
-      - -0.11496355439456719
-      - -0.27943514022433258
+      - -0.037773856684264327
+      - -0.059865551569039915
+      - 0.04735839471988889
+      - -0.0017999704505141331
+      - -0.038153795071219465
+      - -0.32055732180606611
+      - 0.0085426542321090874
     accelerations:
       - 0
       - 0
@@ -4630,24 +4826,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 175579511
+      sec: 2
+      nanosec: 765005835
   - positions:
-      - 0.6320779428554324
-      - -0.11221129613970809
-      - -0.49331928683884735
-      - -2.1972197737828165
-      - 0.093308354027053966
-      - -0.061174379999184625
-      - -2.3612578592297604
+      - 1.0193900036368126
+      - -1.6949129047820759
+      - 0.98037215417287749
+      - -1.5985991058131379
+      - -0.55092157344647297
+      - -0.30780538793189077
+      - -0.81218627161944046
     velocities:
-      - 0.12112036490387025
-      - 0.34597447516288909
-      - -0.05605734356658236
-      - -0.11942108238624564
-      - 0.025513466724255602
-      - -0.11233731725233152
-      - -0.27476000569407738
+      - -0.024627143014560836
+      - -0.040151882855006357
+      - 0.030829084536478337
+      - -0.0012163522280423968
+      - -0.025028444638679707
+      - -0.20190790776013876
+      - 0.0056899475428525779
     accelerations:
       - 0
       - 0
@@ -4658,24 +4854,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 185383122
+      sec: 2
+      nanosec: 796315043
   - positions:
-      - 0.63326772891016248
-      - -0.1087903072298973
-      - -0.49386653426940647
-      - -2.1983911148235626
-      - 0.093558908525763457
-      - -0.062280047322943594
-      - -2.3639732024907678
+      - 1.018625066518432
+      - -1.6961341359179791
+      - 0.98162407945087293
+      - -1.5986264264594736
+      - -0.5516429651679523
+      - -0.31370248152894614
+      - -0.81196969302850786
     velocities:
-      - 0.11785938409340276
-      - 0.3400714622513441
-      - -0.053899951009745747
-      - -0.11570393024923356
-      - 0.024765007321241081
-      - -0.10955420912880041
-      - -0.26956930185909711
+      - 0.0005203378535778313
+      - -0.0090958438074896716
+      - 0.016345672509117996
+      - 0.00028510237302049933
+      - 0.0020787189772824015
+      - -0.026993834454730376
+      - 0.0029083631478030577
     accelerations:
       - 0
       - 0
@@ -4686,24 +4882,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 195661102
+      sec: 2
+      nanosec: 863656835
   - positions:
-      - 0.63442213782219214
-      - -0.10543337453924062
-      - -0.49439706637596637
-      - -2.1995281769412283
-      - 0.09380254583569142
-      - -0.06335564489306629
-      - -2.3666423774197733
+      - 1.0211552359587222
+      - -1.6981429916854676
+      - 0.98383049473880624
+      - -1.5984950709374484
+      - -0.54900195419295006
+      - -0.31360661177067334
+      - -0.81141239416697941
     velocities:
-      - 0.1145272395380659
-      - 0.33419097010339199
-      - -0.053040255312301221
-      - -0.11298648444696965
-      - 0.024223302414573159
-      - -0.1065380989654149
-      - -0.26666785487249695
+      - 0.056172534398787992
+      - -0.041018941707357037
+      - 0.036030756676153783
+      - 0.0026461964283239618
+      - 0.056646279177353105
+      - 0.014026997191099427
+      - 0.010322514651628976
     accelerations:
       - 0
       - 0
@@ -4714,24 +4910,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 3
-      nanosec: 207445820
+      sec: 2
+      nanosec: 964957960
   - positions:
-      - 0.63576583757103655
-      - -0.10148398259237418
-      - -0.4950128053159048
-      - -2.2008493760114578
-      - 0.094086134779585856
-      - -0.064608527866778787
-      - -2.3697865056589249
+      - 1.0237960403346953
+      - -1.7000749822344234
+      - 0.98545959586638188
+      - -1.5983729010880121
+      - -0.54634529223877915
+      - -0.31294481893456966
+      - -0.81093691042892868
     velocities:
-      - 0.10948481215597855
-      - 0.32393526380720716
-      - -0.049465913333428424
-      - -0.10734152169650257
-      - 0.023061786756400915
-      - -0.10256592223776587
-      - -0.25684092404891568
+      - 0.082747785633857054
+      - -0.060635489180536711
+      - 0.049835054671013644
+      - 0.003816819605759714
+      - 0.082675335906693334
+      - 0.020592565579912916
+      - 0.014725433368475489
     accelerations:
       - 0
       - 0
@@ -4743,23 +4939,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 218735526
+      nanosec: 9044543
   - positions:
-      - 0.63706598015254712
-      - -0.09761579412887654
-      - -0.49561693274770868
-      - -2.2021332448344904
-      - 0.094362219217160259
-      - -0.065819963787197747
-      - -2.3728888306496221
+      - 1.0265530970685299
+      - -1.702098011677551
+      - 0.98710389695014844
+      - -1.5982467092901027
+      - -0.54358503350185994
+      - -0.3122584624708229
+      - -0.81044788621007569
     velocities:
-      - 0.10730715005691818
-      - 0.32043374526990931
-      - -0.050061574172715999
-      - -0.10589732453858801
-      - 0.022785024530901619
-      - -0.099774023174844559
-      - -0.25753747565839569
+      - 0.10121400666473446
+      - -0.074082299184048628
+      - 0.060179847281361977
+      - 0.0046087776794515537
+      - 0.10146647804529967
+      - 0.024983057440467492
+      - 0.017974299672794451
     accelerations:
       - 0
       - 0
@@ -4771,23 +4967,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 232081646
+      nanosec: 40077627
   - positions:
-      - 0.63833087488618467
-      - -0.093814176868543903
-      - -0.49620029577507335
-      - -2.2033766383219517
-      - 0.094630114267586551
-      - -0.066997586459470446
-      - -2.3759384426702406
+      - 1.0293766942761304
+      - -1.704167966695638
+      - 0.98877036213031411
+      - -1.5981181040539165
+      - -0.5407577326615014
+      - -0.31156056206168287
+      - -0.80994560120682402
     velocities:
-      - 0.10322178145721848
-      - 0.31224085316877243
-      - -0.046683942465247312
-      - -0.10098703857920054
-      - 0.021777990326352695
-      - -0.096611228532715754
-      - -0.24900114938168941
+      - 0.12080573154613997
+      - -0.088178761027804101
+      - 0.07152321362089234
+      - 0.0054511751392139719
+      - 0.12164243748915744
+      - 0.029385664323084822
+      - 0.021620348407031781
     accelerations:
       - 0
       - 0
@@ -4799,23 +4995,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 243615992
+      nanosec: 70082252
   - positions:
-      - 0.63954900269043036
-      - -0.090101989916004974
-      - -0.49676311433252313
-      - -2.2045767088110635
-      - 0.094889160016343288
-      - -0.068136455349871963
-      - -2.3789245675300847
+      - 1.0321984648118265
+      - -1.7062399046969587
+      - 0.99041965053994152
+      - -1.5979894863094162
+      - -0.53793754304270036
+      - -0.31086222845109979
+      - -0.80943977092524011
     velocities:
-      - 0.099885488403689202
-      - 0.3063697103900212
-      - -0.046973423058855907
-      - -0.099131136883232301
-      - 0.021416954820651639
-      - -0.093433088071238624
-      - -0.24827708795665812
+      - 0.13551082258365474
+      - -0.099316229154222682
+      - 0.079331304008431289
+      - 0.0061306393891903642
+      - 0.13621260855339382
+      - 0.033644563621507473
+      - 0.024425676606379131
     accelerations:
       - 0
       - 0
@@ -4827,23 +5023,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 255618689
+      nanosec: 97032627
   - positions:
-      - 0.64073038025900519
-      - -0.086464047286263501
-      - -0.49731192743846603
-      - -2.205740247697737
-      - 0.095140716842006381
-      - -0.069239718885513085
-      - -2.3818624851520314
+      - 1.0351143366883888
+      - -1.7083793777642566
+      - 0.99211462318786781
+      - -1.5978564987116652
+      - -0.53502308169418111
+      - -0.31013389067028202
+      - -0.80891163320116988
     velocities:
-      - 0.09675831252975807
-      - 0.2993799390286685
-      - -0.043960993006179637
-      - -0.094632856408284505
-      - 0.020478203540809627
-      - -0.090650063801650749
-      - -0.24022665061448747
+      - 0.1505160873288971
+      - -0.11054187824562833
+      - 0.087078805572317508
+      - 0.0068948821452622031
+      - 0.15004923974544232
+      - 0.037954287237168809
+      - 0.027362348896645537
     accelerations:
       - 0
       - 0
@@ -4855,23 +5051,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 268597628
+      nanosec: 118383502
   - positions:
-      - 0.64187572774793111
-      - -0.08289477676186198
-      - -0.49784516841280185
-      - -2.2068690938299214
-      - 0.095385225909675062
-      - -0.070310730630718757
-      - -2.3847546306259644
+      - 1.0383652141356858
+      - -1.710761478515969
+      - 0.99399654629484324
+      - -1.5977081986604436
+      - -0.53177128278823793
+      - -0.30931605150741703
+      - -0.80831566727013771
     velocities:
-      - 0.094332185278075853
-      - 0.29529306476162021
-      - -0.044069871918860179
-      - -0.092966487332622513
-      - 0.020145345462437788
-      - -0.088189780675978893
-      - -0.2396139736053442
+      - 0.16960963514646776
+      - -0.12475329768767107
+      - 0.097271028756208286
+      - 0.0078090695010035356
+      - 0.16859261269871623
+      - 0.043156102646127577
+      - 0.031183454761248747
     accelerations:
       - 0
       - 0
@@ -4883,23 +5079,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 280229614
+      nanosec: 143297002
   - positions:
-      - 0.64298155203831053
-      - -0.079407579067963266
-      - -0.49835655126140155
-      - -2.2079564148821498
-      - 0.095621128636395783
-      - -0.071347938353161625
-      - -2.3875790893445581
+      - 1.0411482693208356
+      - -1.712804065999322
+      - 0.99559533401201172
+      - -1.5975807303464595
+      - -0.52899189649195233
+      - -0.30860461044423176
+      - -0.80780055572264209
     velocities:
-      - 0.090491871595340362
-      - 0.28687953139497069
-      - -0.041322280823050318
-      - -0.088670384136498248
-      - 0.019252384028216569
-      - -0.085138736125831241
-      - -0.23156358017011219
+      - 0.17960631267698848
+      - -0.13168989492356722
+      - 0.10298111046811025
+      - 0.0082537469089542775
+      - 0.17900731662363589
+      - 0.045953478340266855
+      - 0.033396606856409106
     accelerations:
       - 0
       - 0
@@ -4911,23 +5107,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 293918646
+      nanosec: 161629419
   - positions:
-      - 0.64421547152831726
-      - -0.07546149547101548
-      - -0.49892784285318353
-      - -2.2091719178770273
-      - 0.095885308360763186
-      - -0.072510154573696112
-      - -2.39078272402956
+      - 1.0444379347906163
+      - -1.7152193466869292
+      - 0.99747346679812798
+      - -1.5974294574924259
+      - -0.525711819346803
+      - -0.3077549399560684
+      - -0.80718553034303286
     velocities:
-      - 0.086466428150049485
-      - 0.27844786348476441
-      - -0.040996654785160229
-      - -0.085943248079032378
-      - 0.018696465715435536
-      - -0.081396609960730906
-      - -0.22820095803776105
+      - 0.18307794258970514
+      - -0.13463565783160408
+      - 0.10402000203968306
+      - 0.0084447557252569343
+      - 0.18228102420882883
+      - 0.047741402136090315
+      - 0.034349654836832166
     accelerations:
       - 0
       - 0
@@ -4939,23 +5135,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 307939820
+      nanosec: 181641919
   - positions:
-      - 0.64540177402773324
-      - -0.071624350366710512
-      - -0.49948082504906571
-      - -2.2103405004417076
-      - 0.096139706136048386
-      - -0.073625498852147656
-      - -2.3939117726851693
+      - 1.04713660534197
+      - -1.7171941950855059
+      - 0.99901403186442217
+      - -1.5973057265792696
+      - -0.52301249281762474
+      - -0.30705839647320876
+      - -0.80667448327646374
     velocities:
-      - 0.082954691652596779
-      - 0.26983592156812308
-      - -0.038080985223993509
-      - -0.081363774536281311
-      - 0.017730052534459161
-      - -0.078187371316803062
-      - -0.21922758289260885
+      - 0.19534635977247813
+      - -0.14338065526983923
+      - 0.11071055774971809
+      - 0.0090237568647334385
+      - 0.19440306437154933
+      - 0.050799858095378528
+      - 0.037071909541511947
     accelerations:
       - 0
       - 0
@@ -4967,23 +5163,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 322091992
+      nanosec: 198322544
   - positions:
-      - 0.64654467505163449
-      - -0.067882535940816321
-      - -0.50001492963247185
-      - -2.2114662361731554
-      - 0.096385227471997015
-      - -0.074699911370104011
-      - -2.3969737080406177
+      - 1.0499388876756743
+      - -1.7192484540056512
+      - 1.0006020946979381
+      - -1.5971767086630384
+      - -0.5202152449500701
+      - -0.30632770797029529
+      - -0.80613903489090988
     velocities:
-      - 0.08000553693681485
-      - 0.26337645153975742
-      - -0.036847929560747179
-      - -0.078438476194413909
-      - 0.017120180307319089
-      - -0.075410518003884541
-      - -0.21465120432107723
+      - 0.20082611899528174
+      - -0.14746120066386278
+      - 0.11325570031436064
+      - 0.0092840639697451578
+      - 0.19999562973132981
+      - 0.052795945136028345
+      - 0.038480361903613133
     accelerations:
       - 0
       - 0
@@ -4995,23 +5191,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 336789082
+      nanosec: 213357752
   - positions:
-      - 0.6476448138633597
-      - -0.064234458588500576
-      - -0.50052908495792037
-      - -2.212549589317236
-      - 0.096621920805940273
-      - -0.075736168842195889
-      - -2.3999657710711761
+      - 1.0528460764382324
+      - -1.7213760659172672
+      - 1.0022454672371097
+      - -1.5970429555983932
+      - -0.51730847347553277
+      - -0.30556566845438582
+      - -0.80557731283718192
     velocities:
-      - 0.077136611941416119
-      - 0.25737148183954589
-      - -0.036304134067113566
-      - -0.07612618793308315
-      - 0.016639985401353398
-      - -0.072750385726577008
-      - -0.21160439085211394
+      - 0.21189790265462918
+      - -0.15491326305011682
+      - 0.11983255562828969
+      - 0.0097072049505653596
+      - 0.21260569763129236
+      - 0.055583726930425771
+      - 0.04117025756386853
     accelerations:
       - 0
       - 0
@@ -5023,23 +5219,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 350383153
+      nanosec: 228426252
   - positions:
-      - 0.64869541473541337
-      - -0.060703495163774844
-      - -0.50101911429820656
-      - -2.2135840856640194
-      - 0.096848301249422578
-      - -0.076729595077789453
-      - -2.4028647193666535
+      - 1.0558426932733846
+      - -1.7235692831287059
+      - 1.0039301015661874
+      - -1.5969046922051109
+      - -0.5143149929665628
+      - -0.30477315949657119
+      - -0.80499248007949176
     velocities:
-      - 0.073817743390874463
-      - 0.24952323476816238
-      - -0.035016472271533178
-      - -0.073104528060806251
-      - 0.016010963766671108
-      - -0.069742629541498083
-      - -0.20627093620603945
+      - 0.21912237176885865
+      - -0.15993763024135285
+      - 0.12354267613840882
+      - 0.010039964546910082
+      - 0.2200793664738874
+      - 0.05780891798634584
+      - 0.043050539902935746
     accelerations:
       - 0
       - 0
@@ -5051,23 +5247,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 365452908
+      nanosec: 243555169
   - positions:
-      - 0.6498407695198879
-      - -0.056803582279837873
-      - -0.50155435746440613
-      - -2.2147121298161849
-      - 0.097095561373158071
-      - -0.077814213948055044
-      - -2.4060749827682639
+      - 1.058498233837436
+      - -1.7255124827158062
+      - 1.0054154942894131
+      - -1.5967818055369485
+      - -0.51166515539997892
+      - -0.30406601519953502
+      - -0.80446919423102892
     velocities:
-      - 0.070166319408432487
-      - 0.24050011750213071
-      - -0.033444660852239766
-      - -0.06958605271173797
-      - 0.015268750176740769
-      - -0.06637098609563083
-      - -0.19959543854323109
+      - 0.22131557169241009
+      - -0.16229535768235473
+      - 0.12317955835416364
+      - 0.010276754884004421
+      - 0.22041372449826008
+      - 0.059438127528588652
+      - 0.043702677190085026
     accelerations:
       - 0
       - 0
@@ -5079,23 +5275,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 381875317
+      nanosec: 256544127
   - positions:
-      - 0.65092998658741841
-      - -0.0530453481308409
-      - -0.50206455721893606
-      - -2.2157849601620243
-      - 0.097331120293000908
-      - -0.078846399175296539
-      - -2.4091775992041007
+      - 1.0612147522538176
+      - -1.7274982013160873
+      - 1.0069307973771846
+      - -1.5966562170488701
+      - -0.50894960712296178
+      - -0.30333745577304821
+      - -0.80392875214315407
     velocities:
-      - 0.066745980234563143
-      - 0.23177760738379258
-      - -0.031903337974616967
-      - -0.066202562351408881
-      - 0.014550569668764222
-      - -0.063167774165726859
-      - -0.19292799572029537
+      - 0.22954903562843401
+      - -0.1675914747496787
+      - 0.12794016634433639
+      - 0.010624252313165447
+      - 0.22937228323421718
+      - 0.061605833463498655
+      - 0.045865405349404101
     accelerations:
       - 0
       - 0
@@ -5107,23 +5303,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 398644656
+      nanosec: 268562544
   - positions:
-      - 0.65195859699271996
-      - -0.049446389317087412
-      - -0.50254439033171394
-      - -2.2167973690616507
-      - 0.097553764523587633
-      - -0.079824153450758981
-      - -2.4121499062304403
+      - 1.0641551348597762
+      - -1.7296342692883775
+      - 1.0085767410661901
+      - -1.5965212081506366
+      - -0.50599468512392554
+      - -0.30255702824322606
+      - -0.80333498928307279
     velocities:
-      - 0.062629259208125801
-      - 0.22027517020745305
-      - -0.029199483084908346
-      - -0.061580735567427804
-      - 0.013549169157499553
-      - -0.059538182793340257
-      - -0.18195009346993707
+      - 0.25976996014700959
+      - -0.18904823420709405
+      - 0.14432499051775999
+      - 0.01206277270979419
+      - 0.2589347388313209
+      - 0.068915125994069598
+      - 0.052641308443373135
     accelerations:
       - 0
       - 0
@@ -5135,23 +5331,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 415378412
+      nanosec: 288307460
   - positions:
-      - 0.65305593614601354
-      - -0.045561097614133461
-      - -0.50306186137363684
-      - -2.2178791157065945
-      - 0.097792041759234533
-      - -0.080864511232866129
-      - -2.4153774716291001
+      - 1.0673034470523048
+      - -1.7319386644717927
+      - 1.0103099309261054
+      - -1.5963749262607045
+      - -0.50285475665449952
+      - -0.30170117107526706
+      - -0.80269472416504073
     velocities:
-      - 0.059615687426878852
-      - 0.21237102758250873
-      - -0.028700291355055366
-      - -0.059126576654915532
-      - 0.013035103527569801
-      - -0.056387509046554478
-      - -0.17783712346991851
+      - 0.2650527385269168
+      - -0.19397780515161453
+      - 0.14550382283150368
+      - 0.01234443992153272
+      - 0.26414769165909191
+      - 0.072836370005101181
+      - 0.054110798842895817
     accelerations:
       - 0
       - 0
@@ -5163,23 +5359,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 433859639
+      nanosec: 300077085
   - positions:
-      - 0.65408855016412781
-      - -0.041854603980779351
-      - -0.50354653449330466
-      - -2.2188952522121581
-      - 0.098016232715682772
-      - -0.08184514853811671
-      - -2.4184582690500194
+      - 1.0700126756596329
+      - -1.7339151282476672
+      - 1.0117990474982566
+      - -1.5962492403574018
+      - -0.50014458343595647
+      - -0.30095399057823208
+      - -0.80213769770120469
     velocities:
-      - 0.055844204938651811
-      - 0.20184392128366949
-      - -0.026757342723293259
-      - -0.055341550536554866
-      - 0.012220457978803482
-      - -0.053001533970635978
-      - -0.16906852587217111
+      - 0.27887340175364361
+      - -0.20290764279433529
+      - 0.15328139891287534
+      - 0.0129701139696142
+      - 0.27856931899581172
+      - 0.076764831084834617
+      - 0.057615760332127924
     accelerations:
       - 0
       - 0
@@ -5191,23 +5387,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 452538307
+      nanosec: 313394252
   - positions:
-      - 0.65506088814463548
-      - -0.038319740757932923
-      - -0.50400557326124951
-      - -2.2198527042607257
-      - 0.098227786042018095
-      - -0.082768437464092751
-      - -2.4214071161720283
+      - 1.0727940291496227
+      - -1.7359473926715097
+      - 1.0133142685608021
+      - -1.5961192137542812
+      - -0.49737364307941229
+      - -0.3001751492567038
+      - -0.80156178166768666
     velocities:
-      - 0.052650693121589695
-      - 0.19231753221396961
-      - -0.024579276579812001
-      - -0.051609553457620229
-      - 0.011410791042351699
-      - -0.050010079633719277
-      - -0.16002064399303945
+      - 0.27752962876441595
+      - -0.20300660258899528
+      - 0.15057460324556288
+      - 0.01302144809767194
+      - 0.2759765396064881
+      - 0.078408347814782786
+      - 0.057587201375837602
     accelerations:
       - 0
       - 0
@@ -5219,23 +5415,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 470442031
+      nanosec: 325078252
   - positions:
-      - 0.65606821091661871
-      - -0.03460510788964994
-      - -0.50447810390971926
-      - -2.2208433123741749
-      - 0.098447021718355052
-      - -0.083727971711394233
-      - -2.4245058880438872
+      - 1.075587701900873
+      - -1.7379849518208825
+      - 1.0148319900054581
+      - -1.595988712581393
+      - -0.49458434080383235
+      - -0.29938399752468353
+      - -0.80097821343022269
     velocities:
-      - 0.048368779065256523
-      - 0.17970357622688546
-      - -0.022240884525108324
-      - -0.047361332065253024
-      - 0.0104908156469182
-      - -0.046280823065277941
-      - -0.14915299860815096
+      - 0.28268983838938133
+      - -0.20561332914805858
+      - 0.1539401841882424
+      - 0.013129642941759381
+      - 0.28352130352314964
+      - 0.079897182717128951
+      - 0.059371266283923571
     accelerations:
       - 0
       - 0
@@ -5247,23 +5443,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 491901156
+      nanosec: 335053669
   - positions:
-      - 0.65708929970534025
-      - -0.030784137677311783
-      - -0.50496087397454925
-      - -2.221849893709805
-      - 0.098670123871403184
-      - -0.084701461903885875
-      - -2.4277063630973994
+      - 1.0784100067052023
+      - -1.7400403893657403
+      - 1.0163591817603772
+      - -1.5958565551154937
+      - -0.49176801991583752
+      - -0.29858051006491038
+      - -0.80038362406507468
     velocities:
-      - 0.044704076899190578
-      - 0.16821949328310826
-      - -0.021258143058044215
-      - -0.044070628331811634
-      - 0.009771843779567177
-      - -0.042542970463768703
-      - -0.14118613635696714
+      - 0.28398404285221485
+      - -0.20653977581837088
+      - 0.15360072515794942
+      - 0.013297447218283078
+      - 0.28349486159956599
+      - 0.08084910261556362
+      - 0.060066486229854603
     accelerations:
       - 0
       - 0
@@ -5275,23 +5471,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 515573342
+      nanosec: 344964835
   - positions:
-      - 0.65810650743309862
-      - -0.026925241986478214
-      - -0.5054418664524426
-      - -2.2228515911322035
-      - 0.098892493351848423
-      - -0.085670199525901256
-      - -2.4309454938806008
+      - 1.0813099479658517
+      - -1.7421430748155191
+      - 1.0179306336360419
+      - -1.5957217759511602
+      - -0.48885673951159092
+      - -0.29775848849070058
+      - -0.79976534489186868
     velocities:
-      - 0.040564224598443034
-      - 0.15488952021034313
-      - -0.019434971730604496
-      - -0.040055663021028365
-      - 0.0088962101008160481
-      - -0.038558643207279315
-      - -0.13057554260507354
+      - 0.30968546667806945
+      - -0.22182953770952607
+      - 0.17084971873891242
+      - 0.013939013780072403
+      - 0.31762141717777465
+      - 0.085449984330493015
+      - 0.067015288130117284
     accelerations:
       - 0
       - 0
@@ -5303,23 +5499,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 540946040
+      nanosec: 361608169
   - positions:
-      - 0.65909610926680251
-      - -0.023115758256883785
-      - -0.50591050015391448
-      - -2.2238259148290891
-      - 0.099109123095179316
-      - -0.086611859876571246
-      - -2.4341506652595268
+      - 1.0850883185153593
+      - -1.7448869609723308
+      - 1.0199568319401426
+      - -1.5955446005740779
+      - -0.48508699213007112
+      - -0.29668693328433315
+      - -0.79895116249145148
     velocities:
-      - 0.03615329152828603
-      - 0.13992043377378827
-      - -0.016991102056375447
-      - -0.035464701405137078
-      - 0.0078905623426603205
-      - -0.034345544736167744
-      - -0.11757308134871676
+      - 0.31382390831407375
+      - -0.22908742932258769
+      - 0.16660233101220642
+      - 0.014817273467134243
+      - 0.31185592053930822
+      - 0.09067618353078287
+      - 0.06770350065468736
     accelerations:
       - 0
       - 0
@@ -5331,23 +5527,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 566854779
+      nanosec: 374944585
   - positions:
-      - 0.66004140864258487
-      - -0.019429368452371656
-      - -0.50635751997040956
-      - -2.2247558088767807
-      - 0.099316178307971612
-      - -0.08750977804181706
-      - -2.4372568897335287
+      - 1.0882313326387913
+      - -1.7471747866463727
+      - 1.0216260567267597
+      - -1.5953968036591779
+      - -0.48195075497009715
+      - -0.29577203934583668
+      - -0.79826860363519436
     velocities:
-      - 0.031449751454856074
-      - 0.12349325722613591
-      - -0.014533187948266923
-      - -0.030799317622258716
-      - 0.0068628183597654255
-      - -0.029999241562420797
-      - -0.1034309826707388
+      - 0.3178380162401333
+      - -0.23049932985004234
+      - 0.16929693812910662
+      - 0.014847842011539426
+      - 0.31881994344713854
+      - 0.092431717035633007
+      - 0.06944290595707521
     accelerations:
       - 0
       - 0
@@ -5359,23 +5555,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 595218312
+      nanosec: 385063419
   - positions:
-      - 0.66097015097426282
-      - -0.015770927615867455
-      - -0.50679918441167615
-      - -2.2256709937100894
-      - 0.099520155691250167
-      - -0.088389724122354144
-      - -2.4403477366803603
+      - 1.0914056660016667
+      - -1.7494790764971684
+      - 1.0233051711105345
+      - -1.5952471233853496
+      - -0.47878451891671353
+      - -0.29484077375709289
+      - -0.79757280799546926
     velocities:
-      - 0.026860560061487217
-      - 0.10602792797853414
-      - -0.012645567344512554
-      - -0.026337032521817943
-      - 0.0058718428326634143
-      - -0.025365745465879823
-      - -0.089360597646742146
+      - 0.31740233318083011
+      - -0.23048540695857736
+      - 0.16730583120660428
+      - 0.015007337320232543
+      - 0.31616815762244949
+      - 0.093565624647358639
+      - 0.069770492015376398
     accelerations:
       - 0
       - 0
@@ -5387,23 +5583,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 628441241
+      nanosec: 395236960
   - positions:
-      - 0.66192117173021559
-      - -0.011989308695556835
-      - -0.50725124745512484
-      - -2.2266069119502703
-      - 0.099728982321912601
-      - -0.0892862031848599
-      - -2.4435472297874554
+      - 1.0946140006995759
+      - -1.751802208093586
+      - 1.0249964153792888
+      - -1.5950959909206037
+      - -0.47557828300694166
+      - -0.29389549398607229
+      - -0.79686236452237835
     velocities:
-      - 0.020950419600900567
-      - 0.083921982633483147
-      - -0.0098839475349684538
-      - -0.020570138094262318
-      - 0.0045926759333803765
-      - -0.019707499596273478
-      - -0.070894703972858006
+      - 0.32450940200032519
+      - -0.23475729706621654
+      - 0.17066470236188205
+      - 0.015331679104118319
+      - 0.32372245699390118
+      - 0.095590964167879489
+      - 0.072134825893577031
     accelerations:
       - 0
       - 0
@@ -5415,23 +5611,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 670024510
+      nanosec: 406663710
   - positions:
-      - 0.66283092541102007
-      - -0.0083077409710780037
-      - -0.50768412264047669
-      - -2.2275027884499981
-      - 0.099929192013598775
-      - -0.090132900032940791
-      - -2.4466685489063531
+      - 1.0979265743690856
+      - -1.7541920431462164
+      - 1.0267388708330754
+      - -1.5949405581316525
+      - -0.4722564227248624
+      - -0.29292162454330417
+      - -0.79611978713490383
     velocities:
-      - 0.013099872388532928
-      - 0.0534254275604642
-      - -0.0061381618164819121
-      - -0.01284468464669989
-      - 0.0028731332812329822
-      - -0.011992527922425431
-      - -0.045159322127162192
+      - 0.34873435130794722
+      - -0.24967702620495258
+      - 0.18471175837803852
+      - 0.016211695580112519
+      - 0.35186427584106084
+      - 0.10084269197270376
+      - 0.078953636482131995
     accelerations:
       - 0
       - 0
@@ -5443,23 +5639,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 725395337
+      nanosec: 421686585
   - positions:
-      - 0.66320766765369088
-      - -0.0055814189776309054
-      - -0.50773612443424521
-      - -2.2283176190680951
-      - 0.1001064564372378
-      - -0.087051879975942367
-      - -2.4494938262770281
+      - 1.1007285753364682
+      - -1.7562162814481685
+      - 1.0281979825117298
+      - -1.5948080966887972
+      - -0.46946031180396619
+      - -0.29209541505485731
+      - -0.79548612004529884
     velocities:
-      - -0.0085048463700864805
-      - -0.0071268977910681581
-      - 0.013963094971411724
-      - -0.0037001945793934256
-      - 0.00069067908707422829
-      - 0.16347220138440172
-      - -0.011805969830181515
+      - 0.35191920927112186
+      - -0.25470863626546963
+      - 0.18242092979287616
+      - 0.016670734536970433
+      - 0.35081034363968511
+      - 0.10457336411799113
+      - 0.079745173878830192
     accelerations:
       - 0
       - 0
@@ -5471,23 +5667,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 888540958
+      nanosec: 430067877
   - positions:
-      - 0.66300904141959005
-      - -0.0059531290701180855
-      - -0.50699018538448071
-      - -2.2284675154752671
-      - 0.10013626416088152
-      - -0.078346936372613141
-      - -2.4499998940047387
+      - 1.1041979567839617
+      - -1.7587321127723445
+      - 1.0299830737395383
+      - -1.59464289808283
+      - -0.46600923525794724
+      - -0.29104598853268293
+      - -0.79469757074101188
     velocities:
-      - -0.0022047167796037546
-      - -0.014032558746430274
-      - 0.029408400958049582
-      - -0.0050524072422090021
-      - 0.0010576218578399652
-      - 0.34187562756810452
-      - -0.017860495460989206
+      - 0.33931745065581637
+      - -0.24693046320922976
+      - 0.17298438209209122
+      - 0.016357029967133289
+      - 0.33499041583965261
+      - 0.10434028910357744
+      - 0.07705585374645528
     accelerations:
       - 0
       - 0
@@ -5499,23 +5695,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 924003488
+      nanosec: 441456169
   - positions:
-      - 0.66297915619595837
-      - -0.0063309352084733499
-      - -0.50619709048633399
-      - -2.2285985812443285
-      - 0.10016388985587797
-      - -0.069251330348903489
-      - -2.4504674782050597
+      - 1.107711472992031
+      - -1.7612633660560175
+      - 1.0317920507683298
+      - -1.5944765920435096
+      - -0.46249397776522416
+      - -0.28997996454686892
+      - -0.79388917339348486
     velocities:
-      - 0.0016818693801088
-      - -0.018895264347175783
-      - 0.040923667397547381
-      - -0.0063495885669013164
-      - 0.0013768403447206319
-      - 0.46945593686526782
-      - -0.023204446351155327
+      - 0.35339232096459877
+      - -0.25458408041875252
+      - 0.18121243532506767
+      - 0.016812758745108677
+      - 0.35246901028168814
+      - 0.10752697209442401
+      - 0.081537671126868591
     accelerations:
       - 0
       - 0
@@ -5527,23 +5723,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 946963712
+      nanosec: 451585877
   - positions:
-      - 0.66302520350544725
-      - -0.006667160504519814
-      - -0.5054635845488693
-      - -2.2287107380217388
-      - 0.10018830935464136
-      - -0.060906789714707449
-      - -2.4508797799761486
+      - 1.1104680800567091
+      - -1.7632542172432919
+      - 1.0331958838664981
+      - -1.5943448987811131
+      - -0.4597495185311874
+      - -0.28912978137659351
+      - -0.79325210940329971
     velocities:
-      - 0.0041035614519047787
-      - -0.022453713563301756
-      - 0.049420903289648634
-      - -0.0074461839392200684
-      - 0.001630210868741645
-      - 0.55950701593945973
-      - -0.027538707181021239
+      - 0.34318722580785122
+      - -0.24750968729260453
+      - 0.17466939676122029
+      - 0.016392899738031493
+      - 0.34188120685850171
+      - 0.10599424398227891
+      - 0.079536414942797631
     accelerations:
       - 0
       - 0
@@ -5555,23 +5751,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 963620362
+      nanosec: 463277169
   - positions:
-      - 0.66309426033904473
-      - -0.0070069530911569568
-      - -0.5047147392808814
-      - -2.2288220111394677
-      - 0.10021278854159374
-      - -0.052435647220223335
-      - -2.4512933785079229
+      - 1.1133854631535312
+      - -1.7653408536669339
+      - 1.034690080428281
+      - -1.5942071116063137
+      - -0.45682195558229616
+      - -0.28824723513410611
+      - -0.79256819281404023
     velocities:
-      - 0.0052003492283674603
-      - -0.025877395885626864
-      - 0.057020437798615604
-      - -0.0085586133193975619
-      - 0.0018708729749286658
-      - 0.63919146616591449
-      - -0.031748296018150733
+      - 0.36658787816251914
+      - -0.26342059903172532
+      - 0.18597170853748754
+      - 0.017494818270688293
+      - 0.3652840222414287
+      - 0.11174542302375563
+      - 0.085894718218276128
     accelerations:
       - 0
       - 0
@@ -5583,23 +5779,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 978772403
+      nanosec: 470083835
   - positions:
-      - 0.66316091276459588
-      - -0.0074005248139424971
-      - -0.50385784125174682
-      - -2.2289492565039715
-      - 0.10024063745985116
-      - -0.042782094064746028
-      - -2.4517655733932813
+      - 1.1163150641082731
+      - -1.7674489884908695
+      - 1.0361681343684264
+      - -1.5940672905588502
+      - -0.45390081290280265
+      - -0.2873452332228425
+      - -0.79187909751074048
     velocities:
-      - 0.0043614472415032688
-      - -0.030053516350236273
-      - 0.065041184403923313
-      - -0.0097730017902022265
-      - 0.0021225586608839644
-      - 0.72856786266132501
-      - -0.036136290062235676
+      - 0.36460926194153842
+      - -0.26268399278605747
+      - 0.18314162248784788
+      - 0.01746715621208559
+      - 0.36282311942655154
+      - 0.11307872090799728
+      - 0.085875609493895694
     accelerations:
       - 0
       - 0
@@ -5611,23 +5807,23 @@ points:
     effort: []
     time_from_start:
       sec: 3
-      nanosec: 991915789
+      nanosec: 480048252
   - positions:
-      - 0.66322993797174479
-      - -0.0077786113740523294
-      - -0.50303481790132887
-      - -2.2290698069443722
-      - 0.10026712843156227
-      - -0.033501996819507295
-      - -2.4522155000787884
+      - 1.1192365873749561
+      - -1.7695478370068658
+      - 1.0376347263430012
+      - -1.5939277253405084
+      - -0.4509854851702772
+      - -0.28643650976983703
+      - -0.79118709306160095
     velocities:
-      - 0.0068622360919231469
-      - -0.032541150216384221
-      - 0.071215585028330947
-      - -0.010282084075183932
-      - 0.0022753796777969402
-      - 0.80515620931675957
-      - -0.038614651274154241
+      - 0.36443852746230382
+      - -0.2618594077301134
+      - 0.18231689206518426
+      - 0.017478881385213457
+      - 0.36285446886368472
+      - 0.11383309295753076
+      - 0.086463278923504541
     accelerations:
       - 0
       - 0
@@ -5638,24 +5834,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 5225418
+      sec: 3
+      nanosec: 488482919
   - positions:
-      - 0.66332752119965677
-      - -0.0081109365005600266
-      - -0.50229272275623771
-      - -2.2291749883634271
-      - 0.10029067135991988
-      - -0.025126408338838696
-      - -2.452615186824032
+      - 1.1221774164369707
+      - -1.771652763350184
+      - 1.0391071415090234
+      - -1.5937875183543257
+      - -0.44804378002216277
+      - -0.28551957998914318
+      - -0.79048457455319387
     velocities:
-      - 0.010453190611789889
-      - -0.034023591897555591
-      - 0.076195969307924105
-      - -0.010719536477915065
-      - 0.0024074833563676821
-      - 0.86060700895959286
-      - -0.040867083774654048
+      - 0.37406675560585645
+      - -0.26673694131273051
+      - 0.18768553620849127
+      - 0.017732329219091958
+      - 0.37567554550197074
+      - 0.11594019797674167
+      - 0.089840898849978909
     accelerations:
       - 0
       - 0
@@ -5666,24 +5862,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 15585500
+      sec: 3
+      nanosec: 496614335
   - positions:
-      - 0.6634518499696932
-      - -0.0084596747870890384
-      - -0.5015025103164078
-      - -2.2292847921580177
-      - 0.10031551110918928
-      - -0.016210480454539777
-      - -2.4530369974011315
+      - 1.125175619158215
+      - -1.7737957250433312
+      - 1.0405987479968777
+      - -1.5936441529170708
+      - -0.44504891732809987
+      - -0.28458309894587464
+      - -0.78976297152597541
     velocities:
-      - 0.015363872607126509
-      - -0.034757567876275139
-      - 0.08019653562877796
-      - -0.010799827541562735
-      - 0.0024883248772297364
-      - 0.90954307383371757
-      - -0.042106172023110282
+      - 0.377605323030618
+      - -0.2697116316063442
+      - 0.18745135709825139
+      - 0.018067392446074127
+      - 0.37711356497946447
+      - 0.1180566470551099
+      - 0.091151419089641111
     accelerations:
       - 0
       - 0
@@ -5694,24 +5890,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 25636818
+      sec: 3
+      nanosec: 505152752
   - positions:
-      - 0.66363759578681958
-      - -0.0088092476274675615
-      - -0.5006774757088307
-      - -2.2293947456273853
-      - 0.10034105022178434
-      - -0.0069139641833458575
-      - -2.4534698976318059
+      - 1.1283031662453733
+      - -1.7760206556106377
+      - 1.0421508271759419
+      - -1.5934952918353713
+      - -0.44191390094825489
+      - -0.28361111545521539
+      - -0.7890020049187868
     velocities:
-      - 0.019427336571228016
-      - -0.035450790775398096
-      - 0.084134807602793557
-      - -0.011257981232837527
-      - 0.002610447691875047
-      - 0.94304540168002693
-      - -0.044346765531596666
+      - 0.39958198488129804
+      - -0.28445295127666365
+      - 0.19727400842876366
+      - 0.019146232718174918
+      - 0.39872382404172763
+      - 0.12423607348082459
+      - 0.097414092447458578
     accelerations:
       - 0
       - 0
@@ -5722,24 +5918,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 35440050
+      sec: 3
+      nanosec: 516587669
   - positions:
-      - 0.66388203890733599
-      - -0.0091530931353050685
-      - -0.49983250371265653
-      - -2.2295024307044233
-      - 0.10036677924135211
-      - 0.002597956837586052
-      - -2.4539047758300878
+      - 1.1314806272513585
+      - -1.7782911344144552
+      - 1.0437048531471913
+      - -1.593342643648981
+      - -0.43874587949387883
+      - -0.28260656073607127
+      - -0.7882254475961239
     velocities:
-      - 0.026487582157321912
-      - -0.034173687548594817
-      - 0.085344838708909937
-      - -0.010850007548326726
-      - 0.0026008699982244619
-      - 0.95435230718179886
-      - -0.044049476182905438
+      - 0.39840661577502817
+      - -0.2844620120490583
+      - 0.19438238811187147
+      - 0.019165943401260998
+      - 0.39708218890302266
+      - 0.12656339051595314
+      - 0.097627845796323984
     accelerations:
       - 0
       - 0
@@ -5750,24 +5946,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 45343736
+      sec: 3
+      nanosec: 525031919
   - positions:
-      - 0.66416251346754052
-      - -0.0094909364118931031
-      - -0.4989789181111825
-      - -2.2296087085426586
-      - 0.10039256061529898
-      - 0.012149594301969931
-      - -2.4543405285394493
+      - 1.1346505765389765
+      - -1.7805519580197873
+      - 1.0452454098101454
+      - -1.5931901113001676
+      - -0.43558336695107003
+      - -0.2815903303629797
+      - -0.78744541216622954
     velocities:
-      - 0.029263577723742933
-      - -0.033406421668108477
-      - 0.08519912737943712
-      - -0.010447888310747173
-      - 0.0025578451335531074
-      - 0.95381796768967186
-      - -0.043172669507754426
+      - 0.39827785264941745
+      - -0.28337322210899246
+      - 0.19344320965505776
+      - 0.019169995941952924
+      - 0.39746204704920696
+      - 0.12776990860132906
+      - 0.098330242558757872
     accelerations:
       - 0
       - 0
@@ -5778,24 +5974,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 57499813
+      sec: 3
+      nanosec: 533554794
   - positions:
-      - 0.66448049976837587
-      - -0.0098147225697351875
-      - -0.49813386025101974
-      - -2.2297103476456344
-      - 0.10041777777375013
-      - 0.021571204424864772
-      - -2.4547660287027004
+      - 1.1378258064740923
+      - -1.7828093879431763
+      - 1.0467806967725295
+      - -1.5930371046543672
+      - -0.43241431692538718
+      - -0.2805652194165501
+      - -0.78665837556794838
     velocities:
-      - 0.032666875115688311
-      - -0.031804409710049476
-      - 0.083774859587175093
-      - -0.0099369983537916772
-      - 0.0024863220066423651
-      - 0.93350933648710932
-      - -0.041905349128402956
+      - 0.39818287220391863
+      - -0.28270168711342486
+      - 0.19216407503400801
+      - 0.019221040404199856
+      - 0.39709177796991263
+      - 0.12868463395092794
+      - 0.098958777168297082
     accelerations:
       - 0
       - 0
@@ -5806,24 +6002,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 66947474
+      sec: 3
+      nanosec: 543558544
   - positions:
-      - 0.664814800222575
-      - -0.010124240313430856
-      - -0.49730873006091569
-      - -2.2298076077951126
-      - 0.10044222651384303
-      - 0.030714107689262325
-      - -2.4551785519770837
+      - 1.1425344283436838
+      - -1.7861576843491485
+      - 1.0490317953048685
+      - -1.5928085706732877
+      - -0.42773027243134698
+      - -0.27902457005928283
+      - -0.7854839195236526
     velocities:
-      - 0.034049027629362995
-      - -0.030208582552716672
-      - 0.081290701421558809
-      - -0.0094585609161702468
-      - 0.0023971805582791348
-      - 0.89971598901602756
-      - -0.040412170357754504
+      - 0.37139625187043579
+      - -0.26627804018462092
+      - 0.17487392153004552
+      - 0.018299024836075064
+      - 0.36628496419550327
+      - 0.12496183633805806
+      - 0.092333750865369199
     accelerations:
       - 0
       - 0
@@ -5834,24 +6030,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 76797051
+      sec: 3
+      nanosec: 556585585
   - positions:
-      - 0.66513627491712746
-      - -0.010423987173938068
-      - -0.49650992706072655
-      - -2.2299020750253975
-      - 0.10046590640933203
-      - 0.039494348517675471
-      - -2.4555790430673574
+      - 1.1458150064474886
+      - -1.7884568663371889
+      - 1.0506105187083246
+      - -1.5926522771204761
+      - -0.42442444479062241
+      - -0.27797346286671915
+      - -0.78465128119880223
     velocities:
-      - 0.031284322567896233
-      - -0.029645346306438794
-      - 0.07880054074677692
-      - -0.0094026437770091224
-      - 0.0023439078241112533
-      - 0.86163772939000904
-      - -0.039731324042862434
+      - 0.41944289357437448
+      - -0.29415894522758157
+      - 0.20084846394758976
+      - 0.020027916044618923
+      - 0.42191939579536275
+      - 0.13402152646900534
+      - 0.10674133727525349
     accelerations:
       - 0
       - 0
@@ -5862,24 +6058,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 87002344
+      sec: 3
+      nanosec: 564957919
   - positions:
-      - 0.66544513526063109
-      - -0.010709472235108916
-      - -0.49574782424993596
-      - -2.2299916414806447
-      - 0.10048842459898562
-      - 0.047824775333794971
-      - -2.4559601439573262
+      - 1.1491468151466619
+      - -1.7908090036640298
+      - 1.0521832837123417
+      - -1.5924910335278848
+      - -0.42110072454574288
+      - -0.27689027132204352
+      - -0.78380378017300978
     velocities:
-      - 0.029899069334297999
-      - -0.028132494450856565
-      - 0.074907294547633507
-      - -0.0089150106037293227
-      - 0.0022245010342194374
-      - 0.81327819281610869
-      - -0.037759508849936591
+      - 0.41827410034083529
+      - -0.29518766200029045
+      - 0.19679930460875777
+      - 0.020260017757254768
+      - 0.41719060800836694
+      - 0.13651651529734718
+      - 0.10666672508580481
     accelerations:
       - 0
       - 0
@@ -5890,24 +6086,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 96802458
+      sec: 3
+      nanosec: 573285169
   - positions:
-      - 0.66577562013724667
-      - -0.01103705522358618
-      - -0.4948860598673479
-      - -2.2300943089281255
-      - 0.10051397003510247
-      - 0.057164788272212955
-      - -2.4563937303527386
+      - 1.152476131386367
+      - -1.7931570120898497
+      - 1.0537416447090746
+      - -1.5923296487581644
+      - -0.4177782621160786
+      - -0.27579388412206274
+      - -0.78295149379025197
     velocities:
-      - 0.026424845785186859
-      - -0.026518728733458105
-      - 0.069514793030412664
-      - -0.0082539760202438323
-      - 0.0020562503379001826
-      - 0.75243888178234397
-      - -0.034891870374334805
+      - 0.4184660247275313
+      - -0.29445777276797036
+      - 0.19562869403442737
+      - 0.020294439230087176
+      - 0.41765442434615557
+      - 0.1378890599468986
+      - 0.1074560172056806
     accelerations:
       - 0
       - 0
@@ -5918,24 +6114,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 110947233
+      sec: 3
+      nanosec: 581789252
   - positions:
-      - 0.6660719062600563
-      - -0.011342474874992221
-      - -0.49408951472822915
-      - -2.2301897030138731
-      - 0.10053759794218398
-      - 0.065726181983313758
-      - -2.4567955240286743
+      - 1.1558446472784396
+      - -1.7955236318868304
+      - 1.055309164834856
+      - -1.5921662411580444
+      - -0.41441463914642451
+      - -0.27468002390717433
+      - -0.78208258675836906
     velocities:
-      - 0.022912549112713013
-      - -0.024861302141510156
-      - 0.064237361287795586
-      - -0.0078138068758349527
-      - 0.0019160262748474253
-      - 0.68576975541402951
-      - -0.032672515122392755
+      - 0.42098679933541627
+      - -0.29598687403771212
+      - 0.19494511056095756
+      - 0.020506848572490258
+      - 0.41939096714979401
+      - 0.13988604906464655
+      - 0.10873876106238625
     accelerations:
       - 0
       - 0
@@ -5946,24 +6142,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 122323128
+      sec: 3
+      nanosec: 591664544
   - positions:
-      - 0.66636691630818579
-      - -0.011669756884578239
-      - -0.49324860658785868
-      - -2.2302914899263664
-      - 0.10056259625525575
-      - 0.074675158477201603
-      - -2.4572214701260813
+      - 1.1601938889100656
+      - -1.7985663253535911
+      - 1.0573178453027734
+      - -1.5919553046695245
+      - -0.41006800216593053
+      - -0.27324196712532628
+      - -0.78095005392088712
     velocities:
-      - 0.02004104885642654
-      - -0.022139116429659021
-      - 0.056881914337757411
-      - -0.0068485569035587916
-      - 0.0016867297689342196
-      - 0.60318266825857991
-      - -0.028727546936979383
+      - 0.43166579107628983
+      - -0.30416414316041607
+      - 0.19645785226883308
+      - 0.021298139820216194
+      - 0.42670757146754584
+      - 0.1450868213863338
+      - 0.11212586927428639
     accelerations:
       - 0
       - 0
@@ -5974,24 +6170,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 137229081
+      sec: 3
+      nanosec: 603369002
   - positions:
-      - 0.66664026494452833
-      - -0.012001638212251396
-      - -0.4924100782313291
-      - -2.2303945950962443
-      - 0.10058763205373475
-      - 0.083469577145169782
-      - -2.4576492076471741
+      - 1.1629518774253795
+      - -1.8004859225997398
+      - 1.0585834886713517
+      - -1.5918221944359456
+      - -0.40730206296918336
+      - -0.27233390936086305
+      - -0.78022429637017687
     velocities:
-      - 0.015348438019802674
-      - -0.01943056761619686
-      - 0.048679964391093518
-      - -0.0059915276381485409
-      - 0.0014521904816255631
-      - 0.5080525103864153
-      - -0.024807302730176502
+      - 0.45745201016012332
+      - -0.32006683129098368
+      - 0.20790308729902102
+      - 0.022358143048560739
+      - 0.45512366959768252
+      - 0.15235599714798403
+      - 0.12007588927217153
     accelerations:
       - 0
       - 0
@@ -6002,24 +6198,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 153977813
+      sec: 3
+      nanosec: 613394127
   - positions:
-      - 0.66689969830191453
-      - -0.012347026030454629
-      - -0.49155188296237806
-      - -2.2305013853592377
-      - 0.10061332678070298
-      - 0.092294565363103959
-      - -2.458089048941603
+      - 1.165724622586265
+      - -1.8024235867012985
+      - 1.0598379712336059
+      - -1.591687019818407
+      - -0.40453677633136631
+      - -0.27140424131115981
+      - -0.77949336262349334
     velocities:
-      - 0.011512177798617663
-      - -0.015296386940265786
-      - 0.037995859558015085
-      - -0.0047134203870121795
-      - 0.0011358442334881947
-      - 0.38583994336242122
-      - -0.019441882913823115
+      - 0.45881000306455577
+      - -0.32130908934365104
+      - 0.20643457233145837
+      - 0.022488377450332962
+      - 0.45620890441794598
+      - 0.15527965573290464
+      - 0.12090560703204431
     accelerations:
       - 0
       - 0
@@ -6030,24 +6226,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 173806375
+      sec: 3
+      nanosec: 620038710
   - positions:
-      - 0.66713288456340436
-      - -0.012690379897620873
-      - -0.49071237017419606
-      - -2.2306074171786485
-      - 0.10063856979947693
-      - 0.10054954991187551
-      - -2.4585221051777433
+      - 1.1685010256066979
+      - -1.8043600022706698
+      - 1.0610854788411319
+      - -1.5915514054143565
+      - -0.40176731554917938
+      - -0.27046193877574964
+      - -0.77875807613756365
     velocities:
-      - 0.0058726132051154147
-      - -0.0095451805539011839
-      - 0.023014983320201042
-      - -0.0029550285415573382
-      - 0.00069592579839964448
-      - 0.21622099523451083
-      - -0.011961132089192542
+      - 0.45955413675622275
+      - -0.32086754570836934
+      - 0.20559186678081104
+      - 0.022551703647486265
+      - 0.45726193294965406
+      - 0.15711538486891516
+      - 0.12170652699839295
     accelerations:
       - 0
       - 0
@@ -6058,24 +6254,24 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 201996288
+      sec: 3
+      nanosec: 626626544
   - positions:
-      - 0.66715601290112969
-      - -0.01288238798013994
-      - -0.49030110449771414
-      - -2.2306670994811979
-      - 0.10065154173063036
-      - 0.10415934790230358
-      - -2.4587476083110333
+      - 1.1730837067090869
+      - -1.8075499693764789
+      - 1.0631246147502722
+      - -1.5913262792728036
+      - -0.39720461257767004
+      - -0.26888265942942974
+      - -0.77753864184176091
     velocities:
-      - -0.0034148770746069423
-      - -0.0070826480879670872
-      - 0.012801378988162488
-      - -0.0021911364352082681
-      - 0.00042932633504281961
-      - 0.10328165905913109
-      - -0.0075743614258560578
+      - 0.44190471609782939
+      - -0.30917373573802753
+      - 0.19441849997214225
+      - 0.021989461390084598
+      - 0.43686310759215746
+      - 0.15555140489849953
+      - 0.11733116955334459
     accelerations:
       - 0
       - 0
@@ -6086,5 +6282,3841 @@ points:
       - 0
     effort: []
     time_from_start:
-      sec: 4
-      nanosec: 228643541
+      sec: 3
+      nanosec: 636588419
+  - positions:
+      - 1.1758696163903457
+      - -1.8094664171255368
+      - 1.0643661561103386
+      - -1.5911908939140358
+      - -0.39440755351945045
+      - -0.26792929423040279
+      - -0.77678901165705605
+    velocities:
+      - 0.46807682058260669
+      - -0.32216076373833874
+      - 0.20768774924815839
+      - 0.022935544409440692
+      - 0.4673992352768257
+      - 0.16008667104343585
+      - 0.12592367556947098
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 644921960
+  - positions:
+      - 1.1787256466268374
+      - -1.8114311138654204
+      - 1.0656257274595877
+      - -1.5910512467606266
+      - -0.39155164950912069
+      - -0.26695307535873797
+      - -0.77601766024963903
+    velocities:
+      - 0.47786903255025343
+      - -0.32884808068731652
+      - 0.20998193430083673
+      - 0.023374603462805579
+      - 0.47779577543565471
+      - 0.16367142971556861
+      - 0.12923979168816518
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 651646835
+  - positions:
+      - 1.1815616818224552
+      - -1.8133861190682683
+      - 1.0668619464940934
+      - -1.5909117626523883
+      - -0.38872385940309034
+      - -0.26597129139400405
+      - -0.77524994274675407
+    velocities:
+      - 0.47344982377872274
+      - -0.32597636489459553
+      - 0.205989457931954
+      - 0.023308123828582144
+      - 0.47191684597823119
+      - 0.16407855147190445
+      - 0.12835576280045632
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 656607002
+  - positions:
+      - 1.1844024892236029
+      - -1.815337035345101
+      - 1.068093680412646
+      - -1.5907721391553424
+      - -0.38588725100908394
+      - -0.264984552319385
+      - -0.7744769950243815
+    velocities:
+      - 0.47497692800956981
+      - -0.32578604225989766
+      - 0.2055320845730893
+      - 0.023388811808077638
+      - 0.47379039628419001
+      - 0.16498314108303552
+      - 0.12940405181786735
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 663547835
+  - positions:
+      - 1.187260693371109
+      - -1.8172933020171436
+      - 1.069325314144921
+      - -1.5906315812386635
+      - -0.3830322936358096
+      - -0.26399050402494206
+      - -0.77369559933600196
+    velocities:
+      - 0.47781426277579053
+      - -0.32674011977581247
+      - 0.2053983616617156
+      - 0.023533478901836453
+      - 0.4768799425158799
+      - 0.16621945344902789
+      - 0.13079412009015287
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 670083002
+  - positions:
+      - 1.1901065585697983
+      - -1.8192381669070035
+      - 1.0705418162597045
+      - -1.5904913701849424
+      - -0.38019059112107861
+      - -0.26299503988206668
+      - -0.77291469093262843
+    velocities:
+      - 0.47878070634646802
+      - -0.32566603842944475
+      - 0.20502972603828928
+      - 0.023409620412753348
+      - 0.48051942821978516
+      - 0.16656990751697964
+      - 0.13192539409703066
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 676697710
+  - positions:
+      - 1.1929460841594561
+      - -1.8211746449451864
+      - 1.0717463864793979
+      - -1.5903509449569548
+      - -0.37735996434286301
+      - -0.26199824491660245
+      - -0.7721328951773343
+    velocities:
+      - 0.46990455952531635
+      - -0.32057492552976485
+      - 0.1986469229206648
+      - 0.023263059665861344
+      - 0.46829240436992342
+      - 0.16562116359250736
+      - 0.12947465234693661
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 683477544
+  - positions:
+      - 1.195820134076623
+      - -1.823119874995055
+      - 1.0729629516769641
+      - -1.5902100404992296
+      - -0.37447725910001456
+      - -0.26099581435336888
+      - -0.77133565720731367
+    velocities:
+      - 0.49309051793314473
+      - -0.33066338460032974
+      - 0.20992278588525093
+      - 0.023775573853862871
+      - 0.49969110546461476
+      - 0.1692834539123883
+      - 0.13778495310404473
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 691597335
+  - positions:
+      - 1.1987910534671578
+      - -1.8251273306200286
+      - 1.0742074271658575
+      - -1.590063327513271
+      - -0.37151224175668324
+      - -0.25996517091819427
+      - -0.77050889769580799
+    velocities:
+      - 0.49196546929115048
+      - -0.33368876098811884
+      - 0.20448936275853871
+      - 0.024413258789701447
+      - 0.48956968213138524
+      - 0.17214140305638753
+      - 0.13679663946939924
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 698394919
+  - positions:
+      - 1.2027763330377972
+      - -1.8278201156786027
+      - 1.0758544791866955
+      - -1.5898661626315183
+      - -0.36753599174143031
+      - -0.25856831884781784
+      - -0.7693955070052847
+    velocities:
+      - 0.49121695643570856
+      - -0.3333331408954423
+      - 0.20098777804071441
+      - 0.02459639107291765
+      - 0.48661855459589715
+      - 0.17449538493514088
+      - 0.13694909806535038
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 707311960
+  - positions:
+      - 1.2058495815848134
+      - -1.8298788317252115
+      - 1.0771175810664317
+      - -1.5897151118776736
+      - -0.3644544677211104
+      - -0.2574952665031045
+      - -0.76853018596358336
+    velocities:
+      - 0.51924336985474961
+      - -0.34706649673635848
+      - 0.21292099772660278
+      - 0.025514223008354885
+      - 0.52074641144156009
+      - 0.18084445326561005
+      - 0.1464812476420474
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 715075210
+  - positions:
+      - 1.2089326764213375
+      - -1.8319498435869115
+      - 1.0783653827149449
+      - -1.589561787499074
+      - -0.3613838071351001
+      - -0.25640437857767989
+      - -0.76766180021007691
+    velocities:
+      - 0.50983184985635921
+      - -0.34283567498811013
+      - 0.2053337626166746
+      - 0.02540370314291333
+      - 0.50742291974143783
+      - 0.18174970845839611
+      - 0.1436446735145733
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 722514460
+  - positions:
+      - 1.2120970309689894
+      - -1.8340585027281733
+      - 1.0796396713366634
+      - -1.5894052785408954
+      - -0.35821884313796504
+      - -0.25528818954366844
+      - -0.76676461190532985
+    velocities:
+      - 0.52475106216204304
+      - -0.35076678045920623
+      - 0.20976692647621384
+      - 0.026167395537935664
+      - 0.52229144940462757
+      - 0.18663772411010376
+      - 0.14856888960195455
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 730297877
+  - positions:
+      - 1.2152475816026216
+      - -1.8361573251336296
+      - 1.0808930677001971
+      - -1.5892486458934807
+      - -0.3550742112410849
+      - -0.25416247743920961
+      - -0.76586954559835685
+    velocities:
+      - 0.5271275086807099
+      - -0.35043481852432351
+      - 0.20926130172291157
+      - 0.026228082534978832
+      - 0.52603461834400345
+      - 0.18844466886541963
+      - 0.14995414751802841
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 736943210
+  - positions:
+      - 1.2184054181561594
+      - -1.838251535448306
+      - 1.0821394351573748
+      - -1.5890914040898929
+      - -0.35192191967645736
+      - -0.25302838441426245
+      - -0.76496921227488968
+    velocities:
+      - 0.52835619224378538
+      - -0.34975368549194719
+      - 0.20803171697029343
+      - 0.026345679994922162
+      - 0.52709024600469789
+      - 0.18975722360290251
+      - 0.15080268963964924
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 741601502
+  - positions:
+      - 1.221554419779536
+      - -1.8403283965425496
+      - 1.0833737537135688
+      - -1.5889350672591651
+      - -0.34876879575545872
+      - -0.25189306501715242
+      - -0.76406737418264248
+    velocities:
+      - 0.54732160890957171
+      - -0.35504254424657078
+      - 0.2168274432770545
+      - 0.026443818350196623
+      - 0.5572342956082228
+      - 0.19238546227296421
+      - 0.15839311920747096
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 751585794
+  - positions:
+      - 1.2247734606801788
+      - -1.8424493835866593
+      - 1.0846193689866814
+      - -1.5887732739246947
+      - -0.34557075804619791
+      - -0.25073320337590027
+      - -0.76314505710625413
+    velocities:
+      - 0.51805497440001524
+      - -0.34459548067450102
+      - 0.19789706706116608
+      - 0.026309012309520766
+      - 0.51170257784598094
+      - 0.19092691017649333
+      - 0.14788034078346188
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 757355669
+  - positions:
+      - 1.2279834473035398
+      - -1.8445536099231115
+      - 1.0858515384577421
+      - -1.5886128868863945
+      - -0.34236576207720715
+      - -0.24957047808657196
+      - -0.76222098168749708
+    velocities:
+      - 0.51932159104074593
+      - -0.34322358749276249
+      - 0.19709827746441688
+      - 0.026367069777903075
+      - 0.51360575335604097
+      - 0.19168718186707948
+      - 0.14880362022764204
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 764313210
+  - positions:
+      - 1.2324083057630699
+      - -1.8474236687657308
+      - 1.0875379756605097
+      - -1.5883927875938768
+      - -0.33793397383439655
+      - -0.24798536098270502
+      - -0.76093962599405451
+    velocities:
+      - 0.54899957298875823
+      - -0.3586355957703406
+      - 0.20635376774076966
+      - 0.027765373457788952
+      - 0.54417664935049881
+      - 0.19919523663183042
+      - 0.15835343690339415
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 774307419
+  - positions:
+      - 1.2357897363913761
+      - -1.8496126269674844
+      - 1.0888058163214562
+      - -1.5882239703656682
+      - -0.33455341411111017
+      - -0.24676851155919521
+      - -0.75995831627056998
+    velocities:
+      - 0.56316215193425867
+      - -0.36490015280581373
+      - 0.20992826148788815
+      - 0.028255312401343321
+      - 0.56142339588198031
+      - 0.20363771550046436
+      - 0.16333344564183144
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 780350752
+  - positions:
+      - 1.2391319162541741
+      - -1.8517751668716826
+      - 1.0900418528749762
+      - -1.5880561002288665
+      - -0.33122026591092774
+      - -0.24554842259115625
+      - -0.75898789209410744
+    velocities:
+      - 0.55956234691535489
+      - -0.36020978798184583
+      - 0.20691188463638652
+      - 0.027940379200071832
+      - 0.56037615061891111
+      - 0.20376077370828349
+      - 0.16292800170167704
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 786641960
+  - positions:
+      - 1.2425022299969741
+      - -1.8539374924987937
+      - 1.0912798938848303
+      - -1.5878869593441307
+      - -0.32785406773034342
+      - -0.24432009912124683
+      - -0.75800602692998387
+    velocities:
+      - 0.56272177654864852
+      - -0.3607306634386574
+      - 0.20584050883189117
+      - 0.028326861387608225
+      - 0.56105848427455363
+      - 0.20531229384364647
+      - 0.16393368834425284
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 793326210
+  - positions:
+      - 1.2469640152233568
+      - -1.8567884008518847
+      - 1.0928974411069607
+      - -1.5876623398829905
+      - -0.3234013019931542
+      - -0.24268303440166897
+      - -0.75670405482690439
+    velocities:
+      - 0.55840618705814149
+      - -0.35560741004944002
+      - 0.20159045871615283
+      - 0.028149856527161829
+      - 0.55701737865439083
+      - 0.20477061362580046
+      - 0.16309654579702992
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 801629044
+  - positions:
+      - 1.2502831814553499
+      - -1.8588962809015344
+      - 1.0940873846949415
+      - -1.5874950887925867
+      - -0.32008701864167161
+      - -0.24146156083646741
+      - -0.75573334464233177
+    velocities:
+      - 0.55702349051893107
+      - -0.35177859614293916
+      - 0.19964958828765916
+      - 0.027907070790822205
+      - 0.55831912344925705
+      - 0.20382889436724558
+      - 0.16332403339435014
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 808213085
+  - positions:
+      - 1.2536396225764199
+      - -1.8610105787316211
+      - 1.095280806107545
+      - -1.5873261154198841
+      - -0.31673246127929222
+      - -0.24023539348889963
+      - -0.75474919003254026
+    velocities:
+      - 0.56103422313803852
+      - -0.35312407134144547
+      - 0.19856471060180428
+      - 0.028315676318606077
+      - 0.55983948627032187
+      - 0.20485206396538702
+      - 0.16448516253654799
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 815086877
+  - positions:
+      - 1.2581118643783351
+      - -1.8638116792737449
+      - 1.0968497178309562
+      - -1.587101185446071
+      - -0.31225692610511185
+      - -0.23860141440828739
+      - -0.7534350561703419
+    velocities:
+      - 0.58019037006934782
+      - -0.35765712650969017
+      - 0.20432918604385877
+      - 0.028639061032921326
+      - 0.58722617375889752
+      - 0.20714892366088947
+      - 0.17166412656676461
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 826614836
+  - positions:
+      - 1.2615824078198572
+      - -1.8659717798572704
+      - 1.0980484145490434
+      - -1.5869257544840267
+      - -0.30879617095860801
+      - -0.23734610860680705
+      - -0.75241462055373398
+    velocities:
+      - 0.5728022717530219
+      - -0.35784953920190726
+      - 0.19608009223971135
+      - 0.029074980110675247
+      - 0.56987717959748496
+      - 0.2091124688098209
+      - 0.16820591888165093
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 833485336
+  - positions:
+      - 1.2650439277218455
+      - -1.868120930820746
+      - 1.0992258706039137
+      - -1.5867507170827546
+      - -0.30534195767695221
+      - -0.23608351766865071
+      - -0.75139597290040117
+    velocities:
+      - 0.57182670565700133
+      - -0.35540895441053472
+      - 0.19334699216857978
+      - 0.029048753677512733
+      - 0.5692445466626419
+      - 0.20998326781068158
+      - 0.16810519407272678
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 838363419
+  - positions:
+      - 1.2695739576154128
+      - -1.8709176316054799
+      - 1.1007470587651027
+      - -1.5865209281680874
+      - -0.30082297094709021
+      - -0.23441467941510546
+      - -0.75006212936829275
+    velocities:
+      - 0.56185147538748081
+      - -0.34611787435591296
+      - 0.1876225983022313
+      - 0.028578509751182358
+      - 0.55984569407410234
+      - 0.20764677283152819
+      - 0.16540482097856268
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 846724627
+  - positions:
+      - 1.2729820201596249
+      - -1.8729940851397826
+      - 1.1018827656076462
+      - -1.5863490475830979
+      - -0.29740769964041774
+      - -0.23316830957815424
+      - -0.74905543096475924
+    velocities:
+      - 0.5849069353543197
+      - -0.3525109946390424
+      - 0.19530049882580314
+      - 0.029213704955993933
+      - 0.58944815257956285
+      - 0.21019520940962941
+      - 0.17336503108902787
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 856582169
+  - positions:
+      - 1.2764295948081039
+      - -1.8750937485204751
+      - 1.1030116917144281
+      - -1.5861732768656562
+      - -0.2939768853000978
+      - -0.23190768271895024
+      - -0.74803973086701048
+    velocities:
+      - 0.55995771179682774
+      - -0.34336932989974872
+      - 0.18143765929918368
+      - 0.028790319637218731
+      - 0.55462831780065858
+      - 0.20808123069924228
+      - 0.16452392262223217
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 864834377
+  - positions:
+      - 1.2799644164118305
+      - -1.8772187462774483
+      - 1.1041588937580198
+      - -1.5859948623075677
+      - -0.29043588631692252
+      - -0.23062997197638582
+      - -0.74699476686806576
+    velocities:
+      - 0.58629470486531554
+      - -0.35365173366294295
+      - 0.18866243802975485
+      - 0.029851077633979067
+      - 0.58420842582387178
+      - 0.21331903000258046
+      - 0.17288253075346907
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 870104586
+  - positions:
+      - 1.2834832801016005
+      - -1.8793340094671274
+      - 1.1052802233018924
+      - -1.5858158033774787
+      - -0.28692627565078987
+      - -0.22934559125913592
+      - -0.74595696685354806
+    velocities:
+      - 0.58054733459315011
+      - -0.34934824770331641
+      - 0.18382716238679542
+      - 0.029659573300774331
+      - 0.57785654181231028
+      - 0.21339327656450338
+      - 0.17102206989234767
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 876910044
+  - positions:
+      - 1.2869386426761491
+      - -1.881399326202329
+      - 1.1063683483662536
+      - -1.5856396873659333
+      - -0.28347848821317234
+      - -0.22807338937326696
+      - -0.74493808644765813
+    velocities:
+      - 0.57402236102927395
+      - -0.34222179281660919
+      - 0.18018472736794719
+      - 0.029308628044065057
+      - 0.57233938442028309
+      - 0.21147476510594154
+      - 0.16920526870688102
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 883456086
+  - positions:
+      - 1.2903345441873348
+      - -1.8834143581969605
+      - 1.1074271323252969
+      - -1.5854663796527582
+      - -0.28008862336255475
+      - -0.22681885905094912
+      - -0.74393690247517164
+    velocities:
+      - 0.56705637966530231
+      - -0.33449959476954577
+      - 0.17659696489456431
+      - 0.028812629714750369
+      - 0.56775608006452272
+      - 0.20850159628442094
+      - 0.16739588791883489
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 890085419
+  - positions:
+      - 1.2936533692039727
+      - -1.885375227403846
+      - 1.1084496892878417
+      - -1.5852960686505153
+      - -0.27678400594513503
+      - -0.22558753153690397
+      - -0.74296003788138054
+    velocities:
+      - 0.5381993463075273
+      - -0.31929811311336648
+      - 0.16462686969203813
+      - 0.027815315581612932
+      - 0.53386537143597201
+      - 0.20207434598825738
+      - 0.15807795736877109
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 895767044
+  - positions:
+      - 1.2970586625154372
+      - -1.8873440564791799
+      - 1.1094962012971739
+      - -1.5851241792453294
+      - -0.27335841270593031
+      - -0.22435839244054576
+      - -0.74195357150706531
+    velocities:
+      - 0.58695730954841407
+      - -0.33613265755198074
+      - 0.18014876824760023
+      - 0.029453303382362846
+      - 0.59213552747931286
+      - 0.20783541751844442
+      - 0.17367365818658304
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 906770252
+  - positions:
+      - 1.3005539170015745
+      - -1.889368013578383
+      - 1.1105450209677064
+      - -1.5849455161117589
+      - -0.26987363076510101
+      - -0.22310349270445082
+      - -0.74092621402727588
+    velocities:
+      - 0.5762889594739159
+      - -0.33482299665230425
+      - 0.17126172163344971
+      - 0.029546443678504598
+      - 0.57364778989461851
+      - 0.20871393734558247
+      - 0.16912835403823592
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 913395627
+  - positions:
+      - 1.3040012096031945
+      - -1.8913585455933972
+      - 1.111560722263423
+      - -1.5847690951432496
+      - -0.26643559650464277
+      - -0.22185302289343953
+      - -0.73991481856824259
+    velocities:
+      - 0.56792798061091043
+      - -0.32797862560928748
+      - 0.16636012345559525
+      - 0.029166623374652798
+      - 0.56546043344980101
+      - 0.20729944454053878
+      - 0.16640551267869713
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 920050711
+  - positions:
+      - 1.3074110266076975
+      - -1.8933097033460282
+      - 1.1125537149527855
+      - -1.5845945150433158
+      - -0.26303190129945325
+      - -0.22061316972876571
+      - -0.73891574888916889
+    velocities:
+      - 0.56305575994942614
+      - -0.32193048452187817
+      - 0.16313073578033457
+      - 0.028907185440193404
+      - 0.5613145802828885
+      - 0.20550759430466101
+      - 0.16478000644435523
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 926671252
+  - positions:
+      - 1.3107879553324229
+      - -1.8952252922518562
+      - 1.1135253395991667
+      - -1.5844214012234787
+      - -0.25966078693881878
+      - -0.21938571978658791
+      - -0.73792817749687734
+    velocities:
+      - 0.5564402561532763
+      - -0.31576936422434371
+      - 0.15915287908425527
+      - 0.028639524905067756
+      - 0.55428951507717916
+      - 0.20320565182134256
+      - 0.16246860886864684
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 933478669
+  - positions:
+      - 1.3141019958019733
+      - -1.897090224551957
+      - 1.1144675708966558
+      - -1.5842513960990674
+      - -0.25635064603670887
+      - -0.21817986993992464
+      - -0.73696073532297401
+    velocities:
+      - 0.55122257768393768
+      - -0.30926643479760652
+      - 0.1561421809484029
+      - 0.028328407091117374
+      - 0.5500148004512293
+      - 0.20017473147121995
+      - 0.16073884171587344
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 940062919
+  - positions:
+      - 1.3173904558434224
+      - -1.8989234887713617
+      - 1.1153915259616545
+      - -1.5840826904435072
+      - -0.25306473539689195
+      - -0.21699021827504691
+      - -0.73600275283198668
+    velocities:
+      - 0.54834251007394819
+      - -0.30473718498002578
+      - 0.15342829280842302
+      - 0.028157069969723152
+      - 0.54760641326159354
+      - 0.19774723046055359
+      - 0.15957620055563754
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 945048336
+  - positions:
+      - 1.3206271395872891
+      - -1.9007166715160082
+      - 1.116288152765069
+      - -1.5839163724736045
+      - -0.24983217335030064
+      - -0.21582057379901876
+      - -0.73506237317962164
+    velocities:
+      - 0.5415282154940535
+      - -0.29792276124547928
+      - 0.14972089606083083
+      - 0.027674600992832699
+      - 0.54275458797097464
+      - 0.19431447932128607
+      - 0.15743060746283993
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 951659544
+  - positions:
+      - 1.3238471130039919
+      - -1.9024830238146038
+      - 1.1171693989920173
+      - -1.5837510290233041
+      - -0.24661406445617468
+      - -0.21466631355470395
+      - -0.73412905004960138
+    velocities:
+      - 0.53959416935125304
+      - -0.29421469675210399
+      - 0.14722428149386527
+      - 0.027586068116030558
+      - 0.54076000315487993
+      - 0.19209145607505262
+      - 0.15642181980543043
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 958351794
+  - positions:
+      - 1.327122357134531
+      - -1.9042577526321032
+      - 1.1180537431069144
+      - -1.5835832901011766
+      - -0.24333653512868134
+      - -0.21351096394868685
+      - -0.7331823675783341
+    velocities:
+      - 0.55062341727756414
+      - -0.29779425989379554
+      - 0.14772768563140615
+      - 0.028284434444762925
+      - 0.54979441344150592
+      - 0.19342371438518613
+      - 0.15880814152219094
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 968270961
+  - positions:
+      - 1.3304078574254743
+      - -1.9060319033392703
+      - 1.1189226972564046
+      - -1.5834143509279628
+      - -0.24005718621212463
+      - -0.21235274550382846
+      - -0.73223733510606837
+    velocities:
+      - 0.54645724715251798
+      - -0.29472015893589365
+      - 0.14361600504591193
+      - 0.028162239852182882
+      - 0.54473258042124062
+      - 0.19289850605612471
+      - 0.1569078307190277
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 973483002
+  - positions:
+      - 1.3336633239930897
+      - -1.907777386551208
+      - 1.1197698203144553
+      - -1.5832467384029483
+      - -0.23680799904028302
+      - -0.21120197203178034
+      - -0.73130436225366124
+    velocities:
+      - 0.54102071325672252
+      - -0.28931741590984067
+      - 0.14010526445471252
+      - 0.027900237463812271
+      - 0.53955804764631277
+      - 0.19132752108972365
+      - 0.15481422120067206
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 980044586
+  - positions:
+      - 1.3368617790314563
+      - -1.909478249881599
+      - 1.1205907299303977
+      - -1.5830818465932293
+      - -0.23361504590444493
+      - -0.21006753895730207
+      - -0.73039088938041052
+    velocities:
+      - 0.53472698505164773
+      - -0.28223244094238786
+      - 0.13697033638310449
+      - 0.027439592431817223
+      - 0.53549952928646072
+      - 0.18858351998945369
+      - 0.152710269807497
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 986608502
+  - positions:
+      - 1.3400072204881694
+      - -1.9111368123474739
+      - 1.1213876677677923
+      - -1.582919227580879
+      - -0.23047815503116412
+      - -0.20895161559679795
+      - -0.72949621191005176
+    velocities:
+      - 0.51811215452989845
+      - -0.27279427503811915
+      - 0.1306080515191288
+      - 0.026812810851950249
+      - 0.51658221651861669
+      - 0.18434066380043271
+      - 0.14718337201998247
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 993507919
+  - positions:
+      - 1.3431311064341374
+      - -1.9127636915044426
+      - 1.12217005462327
+      - -1.5827580838966366
+      - -0.22735680203182049
+      - -0.20785171781357489
+      - -0.72861066557137921
+    velocities:
+      - 0.51495151694290242
+      - -0.26829279050526961
+      - 0.12814814974618136
+      - 0.02666428810141469
+      - 0.51340752745341423
+      - 0.1819240041076812
+      - 0.14565850316178103
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 3
+      nanosec: 998618877
+  - positions:
+      - 1.3462093669005386
+      - -1.9143524281147277
+      - 1.1229304756677625
+      - -1.5825992372614164
+      - -0.22428020237169916
+      - -0.2067716395575705
+      - -0.72774169733998006
+    velocities:
+      - 0.51312788192590431
+      - -0.26385081401439919
+      - 0.1262219954265163
+      - 0.026503285864305597
+      - 0.51254785046707807
+      - 0.1793861414905718
+      - 0.14458993700667339
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 5043669
+  - positions:
+      - 1.3492716401686868
+      - -1.9159174511171539
+      - 1.1236761366900128
+      - -1.5824411539972307
+      - -0.22121988674790152
+      - -0.20570529221545839
+      - -0.72688142662277944
+    velocities:
+      - 0.51142187715347132
+      - -0.26034719496653908
+      - 0.12394759690967931
+      - 0.026406210218321728
+      - 0.51100370921521465
+      - 0.17735080483655305
+      - 0.14341021536433118
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 11697127
+  - positions:
+      - 1.3522959548979108
+      - -1.9174514277922869
+      - 1.1244008344835417
+      - -1.582284828708165
+      - -0.21819897092296459
+      - -0.20465498296699197
+      - -0.72603610717400135
+    velocities:
+      - 0.50676201956528855
+      - -0.25508089084001356
+      - 0.12108930019121295
+      - 0.026070040695512185
+      - 0.507744540494378
+      - 0.17467472326841477
+      - 0.14154948625034183
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 18542419
+  - positions:
+      - 1.3552824965967269
+      - -1.9189543422386679
+      - 1.1251056022069341
+      - -1.5821300826693312
+      - -0.21521933118587575
+      - -0.20362074649695544
+      - -0.72520583731751975
+    velocities:
+      - 0.49297824015650787
+      - -0.24759409932834039
+      - 0.11570320187800928
+      - 0.025544274756842476
+      - 0.49196788892507221
+      - 0.17097007907157244
+      - 0.13685596285974089
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 25071544
+  - positions:
+      - 1.3582739486519748
+      - -1.9204378800338271
+      - 1.1258029196103509
+      - -1.5819757516312112
+      - -0.21222599878166476
+      - -0.20259748581986065
+      - -0.72437777275489035
+    velocities:
+      - 0.50152505408459347
+      - -0.24743409429670968
+      - 0.11637935120719528
+      - 0.02583460903178739
+      - 0.50227195943230984
+      - 0.1705210144072106
+      - 0.13858928527347708
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 31803294
+  - positions:
+      - 1.3625221439674085
+      - -1.9225004491873203
+      - 1.1267734043659523
+      - -1.5817577115995369
+      - -0.20796731242772162
+      - -0.20119182750325998
+      - -0.72321193002464901
+    velocities:
+      - 0.54670027067216243
+      - -0.26503474389873355
+      - 0.12295619448521135
+      - 0.028159672408641246
+      - 0.54637136738553105
+      - 0.17979020017456418
+      - 0.14918467088263912
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 46600336
+  - positions:
+      - 1.3657313945877108
+      - -1.9240659821770401
+      - 1.1274797953203248
+      - -1.5815910180454473
+      - -0.20477446793185683
+      - -0.20011278943207653
+      - -0.72234110685184172
+    velocities:
+      - 0.52769750336225607
+      - -0.25733581481048473
+      - 0.11526326591454913
+      - 0.027446682954654879
+      - 0.52492579021614105
+      - 0.17914958004369491
+      - 0.14293167698400766
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 53310086
+  - positions:
+      - 1.3688919191855522
+      - -1.925587680604782
+      - 1.1281657094794317
+      - -1.5814269869194071
+      - -0.20162276473573118
+      - -0.19903980926734907
+      - -0.72148789687239523
+    velocities:
+      - 0.5206460012744607
+      - -0.25027986361321602
+      - 0.11239018910793072
+      - 0.027108277640904881
+      - 0.51843816160889067
+      - 0.17785012464748159
+      - 0.14022408769695474
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 59989211
+  - positions:
+      - 1.3720144728995156
+      - -1.9270697383715276
+      - 1.1288350960106854
+      - -1.581264775817758
+      - -0.19850660723588204
+      - -0.19797730461150315
+      - -0.7206501406993423
+    velocities:
+      - 0.51455265603174682
+      - -0.24394657920046195
+      - 0.10965997277147595
+      - 0.026822836563731342
+      - 0.51258422699681183
+      - 0.17593133517495416
+      - 0.13768936430149889
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 64935044
+  - positions:
+      - 1.3750698741522229
+      - -1.9285018287076194
+      - 1.1294814925303047
+      - -1.5811058790203685
+      - -0.19545583217760637
+      - -0.19693520067650802
+      - -0.71983554466480404
+    velocities:
+      - 0.50801587194541142
+      - -0.23695567132341114
+      - 0.10704320299998714
+      - 0.026458813772678991
+      - 0.50686097089735149
+      - 0.1728670588644225
+      - 0.13506940449546709
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 71660627
+  - positions:
+      - 1.3780903844825305
+      - -1.9298978921922962
+      - 1.1301115248318145
+      - -1.5809487158561664
+      - -0.19243895655112048
+      - -0.19591100012858922
+      - -0.71903620837672166
+    velocities:
+      - 0.50190389488811338
+      - -0.23108174386651983
+      - 0.10413069621667632
+      - 0.026159234440180833
+      - 0.50080169023928112
+      - 0.16981513801638048
+      - 0.13241851740104196
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 78280044
+  - positions:
+      - 1.3820117510716294
+      - -1.9316859685999259
+      - 1.1309146816738946
+      - -1.5807444465716458
+      - -0.18852188415185339
+      - -0.19458546348869335
+      - -0.71800740598581969
+    velocities:
+      - 0.49120337507083167
+      - -0.2217086876425805
+      - 0.10003383995272726
+      - 0.025594624516539207
+      - 0.49057294195615569
+      - 0.16436378411250685
+      - 0.12826999125771843
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 86581377
+  - positions:
+      - 1.385888876578423
+      - -1.933421283838586
+      - 1.1316909086915925
+      - -1.5805425538397322
+      - -0.18464742171130097
+      - -0.19329434051809363
+      - -0.71700203269638618
+    velocities:
+      - 0.48607070363600607
+      - -0.21495304258170783
+      - 0.09654994044696194
+      - 0.025147291056795227
+      - 0.48780744681542632
+      - 0.16011938990398458
+      - 0.12559156801151641
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 94937627
+  - positions:
+      - 1.3897130264046498
+      - -1.9351047954066636
+      - 1.1324375039650767
+      - -1.580343128514373
+      - -0.18082895961318646
+      - -0.19203640251179249
+      - -0.71602278511629858
+    velocities:
+      - 0.47333515759984685
+      - -0.20762039896428999
+      - 0.091417715631140262
+      - 0.024731874522899842
+      - 0.47208514095168358
+      - 0.15553112890853063
+      - 0.12062785706153448
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 103492961
+  - positions:
+      - 1.3934679071392833
+      - -1.9367309651860611
+      - 1.1331528535106477
+      - -1.5801473122011622
+      - -0.17707845789743901
+      - -0.19081348649946808
+      - -0.71507252113176401
+    velocities:
+      - 0.4644515929028063
+      - -0.20047472150593823
+      - 0.08762166864920351
+      - 0.024289828349697378
+      - 0.46310147248591893
+      - 0.15112635314536493
+      - 0.11698412204663337
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 113415336
+  - positions:
+      - 1.3972523471211713
+      - -1.9383376965444556
+      - 1.1338574222734648
+      - -1.5799503232562913
+      - -0.17329508406926461
+      - -0.18960582423855191
+      - -0.71412658243307203
+    velocities:
+      - 0.46861304736374049
+      - -0.1991944154135947
+      - 0.086141320163170443
+      - 0.024524265234567171
+      - 0.46680345514272398
+      - 0.14999429129423994
+      - 0.11651920530367085
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 123487961
+  - positions:
+      - 1.4009914788070728
+      - -1.9399089782625851
+      - 1.1345353384612231
+      - -1.5797551761786928
+      - -0.1695627715481251
+      - -0.18841610568433539
+      - -0.7132028790954873
+    velocities:
+      - 0.4600073571103514
+      - -0.1928048491603909
+      - 0.082627506704612386
+      - 0.024051477666584564
+      - 0.45881706834197816
+      - 0.14694764835143093
+      - 0.11319894673547967
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 132301252
+  - positions:
+      - 1.4047065997777082
+      - -1.9414404350668135
+      - 1.1351953905301904
+      - -1.5795614908698146
+      - -0.16585017745841044
+      - -0.18724598234788284
+      - -0.71229602600405861
+    velocities:
+      - 0.46167437745469403
+      - -0.18955280690897156
+      - 0.081236682725109588
+      - 0.024135362360000086
+      - 0.46057224597012819
+      - 0.14530538422208578
+      - 0.11208845204725491
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 140043752
+  - positions:
+      - 1.4083418774092442
+      - -1.9429174985119615
+      - 1.1358259003555686
+      - -1.5793714966052217
+      - -0.16222098527118475
+      - -0.18610344616096594
+      - -0.71142029536811313
+    velocities:
+      - 0.45235900079841063
+      - -0.182441776625548
+      - 0.077842721388124592
+      - 0.023673108668202141
+      - 0.45128764863419452
+      - 0.14163282038401376
+      - 0.10835951899810795
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 148472919
+  - positions:
+      - 1.4119376629757807
+      - -1.9443487807307325
+      - 1.1364350878516909
+      - -1.5791835309857074
+      - -0.15863019135429715
+      - -0.18498565934291583
+      - -0.71056708182075934
+    velocities:
+      - 0.44543071345400437
+      - -0.1764940589513847
+      - 0.07465538177485169
+      - 0.023341645874833068
+      - 0.44415916382790377
+      - 0.13839102255996794
+      - 0.1050478584117798
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 158494586
+  - positions:
+      - 1.4154694917535207
+      - -1.9457269394988592
+      - 1.1370183014724409
+      - -1.5789987881984937
+      - -0.15510294736794347
+      - -0.18389706716802781
+      - -0.70974226898759341
+    velocities:
+      - 0.43834931971404756
+      - -0.17001024456573632
+      - 0.07164013561574617
+      - 0.022977378185441578
+      - 0.43722361683456018
+      - 0.13475657646599207
+      - 0.10170086103454963
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 166903836
+  - positions:
+      - 1.4190472637409357
+      - -1.9470835152743302
+      - 1.1375930743106204
+      - -1.5788121868132745
+      - -0.15152343591537168
+      - -0.18282047946059951
+      - -0.70892287657235964
+    velocities:
+      - 0.45166198492085746
+      - -0.16994179441814949
+      - 0.071632163658995426
+      - 0.023629542484603586
+      - 0.45082138545110784
+      - 0.13480538823696267
+      - 0.10251813536305028
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 178363127
+  - positions:
+      - 1.4226485608957788
+      - -1.9484267326116853
+      - 1.1381503787173748
+      - -1.578623555481538
+      - -0.14793107412840886
+      - -0.18174604117471024
+      - -0.70811514475069848
+    velocities:
+      - 0.44198677164613998
+      - -0.16447477729878623
+      - 0.067490761711788416
+      - 0.023210662602867058
+      - 0.44035752317811633
+      - 0.13283639395267635
+      - 0.098566603184848472
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 186914002
+  - positions:
+      - 1.4261647232058641
+      - -1.949711749804603
+      - 1.138680006734337
+      - -1.5784391645193656
+      - -0.14442248120021492
+      - -0.18069638963332899
+      - -0.7073391703157984
+    velocities:
+      - 0.43094414333901493
+      - -0.15683810494612108
+      - 0.064280426253848444
+      - 0.022657155790724273
+      - 0.4295131146506288
+      - 0.12926133710449944
+      - 0.094599414994723019
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 194951919
+  - positions:
+      - 1.4297313696240792
+      - -1.9509756143905148
+      - 1.1392043915770562
+      - -1.5782527269178013
+      - -0.14085537955762403
+      - -0.17965505338968674
+      - -0.70656682161285644
+    velocities:
+      - 0.44338864915187459
+      - -0.15651967442511133
+      - 0.064276731783444688
+      - 0.023196120819138487
+      - 0.4432398088295415
+      - 0.12954023794266661
+      - 0.095392328826470474
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 204914919
+  - positions:
+      - 1.4332694788181048
+      - -1.9522072205427345
+      - 1.1397091520881033
+      - -1.5780673071392679
+      - -0.13732155566522783
+      - -0.17862725061983878
+      - -0.70581430321837035
+    velocities:
+      - 0.44288327416534184
+      - -0.15310044111791971
+      - 0.062508379650430962
+      - 0.023266935245608182
+      - 0.4416268168170901
+      - 0.12818190817453673
+      - 0.093482324146325541
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 215313961
+  - positions:
+      - 1.4367663574417457
+      - -1.9534022046395081
+      - 1.1401937107367961
+      - -1.577883587055156
+      - -0.13383302058898441
+      - -0.17761523967886217
+      - -0.70508400736806642
+    velocities:
+      - 0.43087044480090542
+      - -0.14654627879788198
+      - 0.05907077525039893
+      - 0.022691113793101823
+      - 0.42932050212716838
+      - 0.12507331086658927
+      - 0.089419055815521939
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 225118294
+  - positions:
+      - 1.4401644762155816
+      - -1.9545377285499497
+      - 1.1406537282124523
+      - -1.5777049519154016
+      - -0.13044097687352882
+      - -0.17663491869257902
+      - -0.70438623451632987
+    velocities:
+      - 0.41971138364514798
+      - -0.13928716600491567
+      - 0.056311570390693552
+      - 0.022103977588363674
+      - 0.41855599657605824
+      - 0.12092288933998999
+      - 0.085603512825966344
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 233311877
+  - positions:
+      - 1.4435173650175575
+      - -1.9556284179475321
+      - 1.1410969617409406
+      - -1.5775289151529397
+      - -0.1270900060751983
+      - -0.17568253014021198
+      - -0.70371105844638859
+    velocities:
+      - 0.41875331972721075
+      - -0.13459739867506931
+      - 0.054828818480531637
+      - 0.021939979551641856
+      - 0.41912187938962281
+      - 0.11792879916569091
+      - 0.083664376399666091
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 241880877
+  - positions:
+      - 1.4476184979781712
+      - -1.9569309715333219
+      - 1.1416223321824435
+      - -1.5773131523879866
+      - -0.1229959767940285
+      - -0.17453276706162354
+      - -0.70290402724558576
+    velocities:
+      - 0.40384453743970422
+      - -0.12712768480889902
+      - 0.05098486273086552
+      - 0.021280109953585175
+      - 0.40278539560517113
+      - 0.11287344856795836
+      - 0.078682186910523338
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 253328419
+  - positions:
+      - 1.4523736680809456
+      - -1.9583921802399502
+      - 1.142209839576722
+      - -1.577063090416754
+      - -0.1182453992658917
+      - -0.17322267975606537
+      - -0.70199396021445204
+    velocities:
+      - 0.39394521093979223
+      - -0.11882483649931744
+      - 0.047927921087547537
+      - 0.020719386731744237
+      - 0.3935146384790576
+      - 0.10697782086867899
+      - 0.074306163663753574
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 264946377
+  - positions:
+      - 1.4562703648261146
+      - -1.9595480617812864
+      - 1.1426723302650823
+      - -1.5768582430947442
+      - -0.11435208485642372
+      - -0.17217674699464292
+      - -0.70127151374950181
+    velocities:
+      - 0.38979696005128572
+      - -0.1136862231694757
+      - 0.045543657832759689
+      - 0.020422382214579534
+      - 0.39033013427090968
+      - 0.10331974248430126
+      - 0.071383555823949785
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 276537627
+  - positions:
+      - 1.4600745471388652
+      - -1.9606443994712559
+      - 1.1431065805692104
+      - -1.5766579772658167
+      - -0.11055437514020666
+      - -0.17117160042969704
+      - -0.70058631488278422
+    velocities:
+      - 0.37710642750145185
+      - -0.10722865298309342
+      - 0.042447022282341623
+      - 0.019868131290007425
+      - 0.37629148176428556
+      - 0.098880812218252442
+      - 0.067113445559330021
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 286550377
+  - positions:
+      - 1.463775387772916
+      - -1.9616772039678905
+      - 1.1435139630652795
+      - -1.5764631004915342
+      - -0.10685942526727303
+      - -0.17020990379296899
+      - -0.69993871562916943
+    velocities:
+      - 0.36536117406268009
+      - -0.10094901558061689
+      - 0.039588506905409417
+      - 0.019279024223335571
+      - 0.36427480433294362
+      - 0.09448505509824702
+      - 0.063180195026712141
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 298479752
+  - positions:
+      - 1.4675294430211003
+      - -1.9626793384712424
+      - 1.1439105068033917
+      - -1.5762661338452253
+      - -0.1031036654470139
+      - -0.16926793829458198
+      - -0.69930511451613819
+    velocities:
+      - 0.38062079329942539
+      - -0.1005386137751644
+      - 0.039316363331499023
+      - 0.020053311406057189
+      - 0.37951841400891589
+      - 0.094481418826410618
+      - 0.063165080263806003
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 311585669
+  - positions:
+      - 1.4712437696338512
+      - -1.9636465894525315
+      - 1.1442828721915668
+      - -1.5760703331969661
+      - -0.099399435357011248
+      - -0.16834167326453336
+      - -0.6986993212290239
+    velocities:
+      - 0.36609110182461801
+      - -0.094155432141066447
+      - 0.03612531541004943
+      - 0.01933213756572888
+      - 0.36481270764991536
+      - 0.091313920223967115
+      - 0.058982534633537691
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 321617086
+  - positions:
+      - 1.4748419420658028
+      - -1.9645479524685123
+      - 1.1446309711592728
+      - -1.5758807103568706
+      - -0.095806682275349525
+      - -0.16745141732486793
+      - -0.69813013843245442
+    velocities:
+      - 0.35884743698389504
+      - -0.087986513398117042
+      - 0.034242378369047054
+      - 0.018852840998208793
+      - 0.35914772124920563
+      - 0.087808170191921386
+      - 0.055989686722930081
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 333475002
+  - positions:
+      - 1.4790312746377616
+      - -1.9655560324057362
+      - 1.1450201411471623
+      - -1.5756596413491606
+      - -0.091625354855112806
+      - -0.16643140688455257
+      - -0.69749048148541382
+    velocities:
+      - 0.34235721102892913
+      - -0.081116896426861276
+      - 0.031188252401383596
+      - 0.018089814061961473
+      - 0.34146591316823205
+      - 0.082930541457112192
+      - 0.05146209657120606
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 346613377
+  - positions:
+      - 1.4832260923944216
+      - -1.9665152584666097
+      - 1.1453897439289116
+      - -1.5754386528781696
+      - -0.087434713427931951
+      - -0.16544279330803952
+      - -0.69687878791952174
+    velocities:
+      - 0.34179243998257103
+      - -0.077550360831382892
+      - 0.029301410170312632
+      - 0.018067836830950451
+      - 0.34069958251108312
+      - 0.08083033015895924
+      - 0.049038041092622199
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 361661544
+  - positions:
+      - 1.4872713503473765
+      - -1.9674024576236571
+      - 1.1457281582826941
+      - -1.575225187263233
+      - -0.0833957021345825
+      - -0.16449921776180254
+      - -0.6963129369655775
+    velocities:
+      - 0.33170826375279483
+      - -0.071303134696377571
+      - 0.027257898169691694
+      - 0.017529485927124642
+      - 0.33094696258807538
+      - 0.076813882391154525
+      - 0.04557427655276098
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 373469169
+  - positions:
+      - 1.4918536918034699
+      - -1.9683527136928398
+      - 1.1460920314103409
+      - -1.574983477749996
+      - -0.078817678865562443
+      - -0.16345800423237528
+      - -0.6957029132982181
+    velocities:
+      - 0.32666865490164265
+      - -0.065963867342625174
+      - 0.025219908305982482
+      - 0.017247052186468541
+      - 0.32609717371484659
+      - 0.073011134863878363
+      - 0.042347911580362309
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 389966544
+  - positions:
+      - 1.495646536932848
+      - -1.9691011064482107
+      - 1.1463754437472151
+      - -1.5747830444354705
+      - -0.075032595687450104
+      - -0.16261370227728608
+      - -0.69522347920466043
+    velocities:
+      - 0.31037412084003713
+      - -0.059979769145978988
+      - 0.022707651911177022
+      - 0.016417215997814674
+      - 0.30959398825611006
+      - 0.06856611969981638
+      - 0.038458801180687105
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 401629961
+  - positions:
+      - 1.4998956275174709
+      - -1.9698912141818796
+      - 1.1466755083366675
+      - -1.5745586417156077
+      - -0.070788940122616315
+      - -0.16169192408306718
+      - -0.69471443267932931
+    velocities:
+      - 0.29908917510738103
+      - -0.054076316018203757
+      - 0.020546580441776253
+      - 0.015809588193354006
+      - 0.29852045823318663
+      - 0.063953518293005385
+      - 0.034877185554545449
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 416678794
+  - positions:
+      - 1.5039888888899817
+      - -1.9706052694500191
+      - 1.1469455634958114
+      - -1.5743424364643939
+      - -0.066701257756861948
+      - -0.16083092403884355
+      - -0.69425371629711707
+    velocities:
+      - 0.28794602374115541
+      - -0.048836488380575867
+      - 0.018438824188965981
+      - 0.015223319056561768
+      - 0.28736760558136171
+      - 0.05976960194452683
+      - 0.031511091597686107
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 433253711
+  - positions:
+      - 1.508019962536874
+      - -1.9712588013947259
+      - 1.1471916276337972
+      - -1.5741298252698801
+      - -0.062672410656991223
+      - -0.16001346731236213
+      - -0.69383122283977916
+    velocities:
+      - 0.29073418588663552
+      - -0.044844091866304059
+      - 0.017103999980798989
+      - 0.015216901780910961
+      - 0.29212556828762015
+      - 0.057538143944689576
+      - 0.029382862758829932
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 449920794
+  - positions:
+      - 1.5120049437421748
+      - -1.9718582295359897
+      - 1.1474150706532271
+      - -1.5739193834981824
+      - -0.058693099634290195
+      - -0.159228147715922
+      - -0.693444405060888
+    velocities:
+      - 0.28573684206681038
+      - -0.041178156966944754
+      - 0.01543945394905127
+      - 0.015064481371962582
+      - 0.28567820356269064
+      - 0.055365781610040153
+      - 0.02672640353440801
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 466542336
+  - positions:
+      - 1.5174244812308455
+      - -1.9726057713776233
+      - 1.1476904986794094
+      - -1.5736322555120643
+      - -0.053289974657694791
+      - -0.15817851627724167
+      - -0.69296264522602469
+    velocities:
+      - 0.24886115823255991
+      - -0.033545858562108399
+      - 0.012121417285729318
+      - 0.01327582420706027
+      - 0.24713357999303423
+      - 0.048866147960898124
+      - 0.021425346805719163
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 491580836
+  - positions:
+      - 1.5216462146300338
+      - -1.9731195487001949
+      - 1.1478839806439896
+      - -1.5734096500746526
+      - -0.049065952364307448
+      - -0.15739966084308907
+      - -0.69262510558520052
+    velocities:
+      - 0.26420419403946954
+      - -0.030666608351923624
+      - 0.011562072788716432
+      - 0.01392295071638007
+      - 0.26442078891355503
+      - 0.047774401259940272
+      - 0.020208742184102325
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 508272294
+  - positions:
+      - 1.5257982149993048
+      - -1.9735870508507141
+      - 1.148055364128524
+      - -1.5731899610818421
+      - -0.044922528354472295
+      - -0.1566522044238586
+      - -0.69232136599247418
+    velocities:
+      - 0.25169592521270678
+      - -0.027595552141441139
+      - 0.010025857800295745
+      - 0.013346827910556641
+      - 0.25087396298895648
+      - 0.045366391193362984
+      - 0.017879694007875473
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 528581086
+  - positions:
+      - 1.5301342944381733
+      - -1.9740409785657085
+      - 1.1482228693470669
+      - -1.572960376663656
+      - -0.040594116888799166
+      - -0.15587807463580128
+      - -0.69202364512661863
+    velocities:
+      - 0.23437252702786537
+      - -0.023599742855033759
+      - 0.0088326013487433023
+      - 0.01236841782291055
+      - 0.23453705215924167
+      - 0.041279847104865035
+      - 0.015665218434129435
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 546558753
+  - positions:
+      - 1.5349982386444025
+      - -1.9745201246187392
+      - 1.1484003248206611
+      - -1.5727028058612804
+      - -0.035738779027269213
+      - -0.15502843966086891
+      - -0.69170771751257465
+    velocities:
+      - 0.21072548343828978
+      - -0.020429201026685582
+      - 0.0074492879031077324
+      - 0.011173979006658372
+      - 0.21012190806447167
+      - 0.036539643127214749
+      - 0.013369977754458867
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 571313128
+  - positions:
+      - 1.5391823523836796
+      - -1.974909776823881
+      - 1.1485434664398899
+      - -1.5724817757043823
+      - -0.031556961437889886
+      - -0.15431934306650402
+      - -0.69145085577506527
+    velocities:
+      - 0.21001319628817017
+      - -0.018853121653988284
+      - 0.0069503711155788723
+      - 0.011066873153676285
+      - 0.21020742585755348
+      - 0.034999362883124462
+      - 0.012481285470450212
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 595212794
+  - positions:
+      - 1.5435502073118248
+      - -1.9752889848168134
+      - 1.1486801904691275
+      - -1.5722506066044011
+      - -0.02719672941238711
+      - -0.15358602777491528
+      - -0.69120258201433005
+    velocities:
+      - 0.19434984583720477
+      - -0.015927048371536814
+      - 0.0058248747873681029
+      - 0.010306407576958013
+      - 0.19376082567397421
+      - 0.032149971933279051
+      - 0.010474044222344835
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 619917711
+  - positions:
+      - 1.547954500736467
+      - -1.9756200639764636
+      - 1.1488004457773788
+      - -1.5720174451521671
+      - -0.022798966708958611
+      - -0.15286633637748356
+      - -0.69098399711638869
+    velocities:
+      - 0.17840046241707044
+      - -0.012168964383076296
+      - 0.004427645223219116
+      - 0.0094297386983856982
+      - 0.17833655387566219
+      - 0.028477295611383228
+      - 0.0080740996075013612
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 648464378
+  - positions:
+      - 1.5525439905493563
+      - -1.975894866941583
+      - 1.1489009789717848
+      - -1.5717742906766463
+      - -0.018216901221345658
+      - -0.15214986393505564
+      - -0.69080074546802472
+    velocities:
+      - 0.15413658983429104
+      - -0.0078221922346962437
+      - 0.0029113494257833059
+      - 0.0081462281057429944
+      - 0.15416534411072805
+      - 0.023195879129801845
+      - 0.0053001202528364091
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 678489544
+  - positions:
+      - 1.5570943498874186
+      - -1.976078538618403
+      - 1.1489680551660115
+      - -1.5715333079984433
+      - -0.013673092823707827
+      - -0.15149595225941465
+      - -0.69067708251143367
+    velocities:
+      - 0.13074662487003641
+      - -0.0040715724250705151
+      - 0.0014536623586722751
+      - 0.0069374205302084879
+      - 0.13038052262567595
+      - 0.018084135360051382
+      - 0.0027284105264096478
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 713567378
+  - positions:
+      - 1.5617401346266875
+      - -1.9761763538570674
+      - 1.149003085619273
+      - -1.5712873217267072
+      - -0.0090340653841440594
+      - -0.15088572891814112
+      - -0.69060997909452093
+    velocities:
+      - 0.10339982047127382
+      - -0.0014575931116233324
+      - 0.00048062488765940868
+      - 0.0054821517048286991
+      - 0.10312464581901114
+      - 0.013055576297153933
+      - 0.00098000149342847981
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 756847003
+  - positions:
+      - 1.5663351416543254
+      - -1.9762098036462117
+      - 1.1490133082663889
+      - -1.5710440123262197
+      - -0.0044466178195014727
+      - -0.15031958547311358
+      - -0.69058612477774206
+    velocities:
+      - 0.070199627214894589
+      - -0.00013249197341545283
+      - 5.3125935743389367e-05
+      - 0.0037118957522179539
+      - 0.07018485175123032
+      - 0.0084658505038380637
+      - 0.0001592247038076093
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 818282544
+  - positions:
+      - 1.5703651189060226
+      - -1.9760106595315012
+      - 1.1488256961013474
+      - -1.5708340442062403
+      - -0.00047762708565305648
+      - -0.14906418853789299
+      - -0.69081816144128116
+    velocities:
+      - 0.01057211526020926
+      - 0.012376054708423033
+      - -0.0077014680463945526
+      - 0.00048078572382578654
+      - 0.0091548656433125594
+      - 0.044046892146977913
+      - -0.010660885589965668
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 4
+      nanosec: 953566253
+  - positions:
+      - 1.5707399819383363
+      - -1.9741742890271949
+      - 1.1480161017017261
+      - -1.5708177810155639
+      - -0.00016188244576417456
+      - -0.14347246346284184
+      - -0.69208835135747016
+    velocities:
+      - 0.0033890356748178378
+      - 0.046482541774319194
+      - -0.019055183191884199
+      - 0.00013351197107250588
+      - 0.002594626526851163
+      - 0.13632755201746377
+      - -0.030619636229977962
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 20026586
+  - positions:
+      - 1.5708301276280983
+      - -1.9720661684743952
+      - 1.1471689084306156
+      - -1.5708144702705071
+      - -9.6709669952947203e-05
+      - -0.13737217950521904
+      - -0.69345111685666605
+    velocities:
+      - 0.0014740426452812033
+      - 0.07019230098184874
+      - -0.02725676530224384
+      - 6.8579939734302888e-05
+      - 0.0012624351547279558
+      - 0.20036856218278487
+      - -0.044119330343546141
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 60189669
+  - positions:
+      - 1.5708756437381524
+      - -1.9699760622445566
+      - 1.1463310304160117
+      - -1.5708132311685472
+      - -7.1458213067471075e-05
+      - -0.13139449554958424
+      - -0.69478244133884826
+    velocities:
+      - 0.0014871411521488809
+      - 0.089544573043917122
+      - -0.035800464078122457
+      - 4.461702502469809e-05
+      - 0.00076216470862379654
+      - 0.25493854717702696
+      - -0.056543214398853653
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 91656836
+  - positions:
+      - 1.5709054562468414
+      - -1.9678101101579737
+      - 1.1454628649894381
+      - -1.5708125594490914
+      - -5.7973333934852937e-05
+      - -0.12524781428399823
+      - -0.6961432120279003
+    velocities:
+      - 0.0011847498312314612
+      - 0.10674172035270003
+      - -0.043195847610337339
+      - 4.0848887070132768e-05
+      - 0.00050606540240599599
+      - 0.30390595750837723
+      - -0.066799432119861168
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 118423794
+  - positions:
+      - 1.5709060952050615
+      - -1.9654949280153227
+      - 1.1445723984147202
+      - -1.5708121007915314
+      - -4.9607623085574518e-05
+      - -0.11878148939785228
+      - -0.69754359987793646
+    velocities:
+      - -0.00034599445608176775
+      - 0.12166328066370939
+      - -0.046298500504774125
+      - 2.4661917053250822e-05
+      - 0.00035534847511503538
+      - 0.33725244126457865
+      - -0.072726862000212
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 140315753
+  - positions:
+      - 1.5709048754314561
+      - -1.9631952568926774
+      - 1.1436797829367629
+      - -1.5708118240124158
+      - -4.4160613628228963e-05
+      - -0.11239906015446567
+      - -0.69892471500170594
+    velocities:
+      - -0.00084129408829576396
+      - 0.13273792479299323
+      - -0.050752976162700358
+      - 2.63552739040709e-05
+      - 0.00026416016072747722
+      - 0.36679799239046201
+      - -0.078388910423988184
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 161625544
+  - positions:
+      - 1.5709036221076942
+      - -1.9609878477737617
+      - 1.1428128232264267
+      - -1.5708117383009923
+      - -4.0445974749444771e-05
+      - -0.10629315917759448
+      - -0.70024322354916602
+    velocities:
+      - 0.0013600004344188103
+      - 0.1420034011927061
+      - -0.05782037433167745
+      - -4.7869193101871372e-05
+      - 0.00021187412464468777
+      - 0.39351978972512752
+      - -0.086715002635283298
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 180081419
+  - positions:
+      - 1.5709391913080366
+      - -1.9589226067013497
+      - 1.1419191554241819
+      - -1.5708116708256097
+      - -3.7779936497820019e-05
+      - -0.10045462906216486
+      - -0.70153177019462609
+    velocities:
+      - 0.0031494817252404597
+      - 0.15333388282112811
+      - -0.068591165038321703
+      - 2.7685730584967594e-05
+      - 0.00017685867884196222
+      - 0.43935888022067604
+      - -0.096660325698860419
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 198484336
+  - positions:
+      - 1.5709364227880793
+      - -1.9566441048914163
+      - 1.1409918842026878
+      - -1.5708113486194282
+      - -3.5536502093340889e-05
+      - -0.094093312251206068
+      - -0.70287944331867558
+    velocities:
+      - -0.0017447927663528408
+      - 0.16816374653715724
+      - -0.06537827412769176
+      - 1.9047184580897783e-05
+      - 0.00014909746592312758
+      - 0.46261702476307553
+      - -0.096436197825160441
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 213349211
+  - positions:
+      - 1.5709403382534917
+      - -1.9542218873149391
+      - 1.1399962573209941
+      - -1.5708113781376065
+      - -3.3578581502047343e-05
+      - -0.087374368552690526
+      - -0.704306886281442
+    velocities:
+      - 0.0011424833699067599
+      - 0.17735197117196105
+      - -0.074682591483593463
+      - -3.4581106379264345e-05
+      - 0.00013408508718496754
+      - 0.49397910334588629
+      - -0.10561086562346562
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 231749753
+  - positions:
+      - 1.5709280915949415
+      - -1.9520309410264718
+      - 1.1391113413786926
+      - -1.5708111990260898
+      - -3.2092759936479288e-05
+      - -0.081332108653955112
+      - -0.70556595743507255
+    velocities:
+      - -0.0016007682960905083
+      - 0.1857165218977587
+      - -0.073997864880705214
+      - 1.7491222150076216e-05
+      - 0.00011844598123538633
+      - 0.50910145693230247
+      - -0.10541449569771473
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 243304836
+  - positions:
+      - 1.5708972821491303
+      - -1.9497722081541111
+      - 1.1382324806862678
+      - -1.5708111290170923
+      - -3.0751122770903989e-05
+      - -0.075202783158348205
+      - -0.70682397974074063
+    velocities:
+      - -0.0021197133970657445
+      - 0.18926093102545544
+      - -0.074296518958944485
+      - -1.6655959077948355e-05
+      - 0.00010843862901013161
+      - 0.51237032420322359
+      - -0.10570230923901736
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 256900753
+  - positions:
+      - 1.5708703786651759
+      - -1.9474948493220012
+      - 1.1373323297046074
+      - -1.5708110688591614
+      - -2.9524366340772774e-05
+      - -0.069040603873001927
+      - -0.70809023177090291
+    velocities:
+      - -0.0025373568121282285
+      - 0.19013039717339855
+      - -0.075333490818944068
+      - 2.1362348187469345e-05
+      - 9.7950576779549342e-05
+      - 0.51458568152282291
+      - -0.10516789830928469
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 271665336
+  - positions:
+      - 1.5708337965347559
+      - -1.9452084282413917
+      - 1.1364371770926147
+      - -1.5708109884453942
+      - -2.8401458418272084e-05
+      - -0.062895943905994692
+      - -0.70933972432172321
+    velocities:
+      - -0.0033981291895728689
+      - 0.19023622520637812
+      - -0.073433552447144751
+      - -1.7768938710293047e-05
+      - 9.085317745555827e-05
+      - 0.50697125279564315
+      - -0.10291636397660395
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 283585795
+  - positions:
+      - 1.570788879329758
+      - -1.9429601410036308
+      - 1.1355735455627649
+      - -1.5708109907055554
+      - -2.7376411967316536e-05
+      - -0.056937431080305578
+      - -0.71054415987235819
+    velocities:
+      - -0.0027180380555262913
+      - 0.18480864660968735
+      - -0.072785494699924916
+      - -2.077784257925058e-05
+      - 8.3468821549399034e-05
+      - 0.49044223538723875
+      - -0.10017784152426665
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 296938503
+  - positions:
+      - 1.5707539105353592
+      - -1.9407842602560303
+      - 1.1347126495692355
+      - -1.5708109827107724
+      - -2.6432908524693703e-05
+      - -0.05116766985189268
+      - -0.71171720355689694
+    velocities:
+      - -0.0021078146736427409
+      - 0.17868502053975419
+      - -0.072151625808401967
+      - -1.0009945245135823e-05
+      - 7.6642067950476438e-05
+      - 0.47426560622006653
+      - -0.097177538126175605
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 309918628
+  - positions:
+      - 1.5707250867027183
+      - -1.9383510603463312
+      - 1.133720324804824
+      - -1.5708109445034548
+      - -2.5424386711590273e-05
+      - -0.044711500382520947
+      - -0.71303755202043184
+    velocities:
+      - -0.0019150863648532046
+      - 0.17046767240150829
+      - -0.071195072397548992
+      - 4.7448298745553297e-05
+      - 6.8009856536062054e-05
+      - 0.4563237117403392
+      - -0.092851045224871018
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 326306670
+  - positions:
+      - 1.57070263549615
+      - -1.936004135646163
+      - 1.1327378937376638
+      - -1.570810902219518
+      - -2.4494150223069193e-05
+      - -0.038460023248586772
+      - -0.71431426869731329
+    velocities:
+      - -0.0015115895454995653
+      - 0.16509946790765476
+      - -0.069036697953153842
+      - -1.865946360599424e-05
+      - 6.4805450144544117e-05
+      - 0.43780692161134249
+      - -0.089517597503712742
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 341862461
+  - positions:
+      - 1.5706889165038862
+      - -1.9337554636528247
+      - 1.1317748835286319
+      - -1.5708108597244743
+      - -2.3634166322212307e-05
+      - -0.032470482271664672
+      - -0.71554072198922924
+    velocities:
+      - -0.0013121183911511546
+      - 0.15783064384405179
+      - -0.067798328524271659
+      - 2.0301341728168181e-05
+      - 5.872592121685994e-05
+      - 0.4214403631354639
+      - -0.085596036746077034
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 360091003
+  - positions:
+      - 1.5706790387131693
+      - -1.931598829695836
+      - 1.1308365482434231
+      - -1.5708108752836807
+      - -2.2834349134165628e-05
+      - -0.026723846356913224
+      - -0.71671510101111047
+    velocities:
+      - 0.00014669687471775855
+      - 0.15093028784298362
+      - -0.067813657582448159
+      - 8.7406169979481868e-06
+      - 5.5513950957899003e-05
+      - 0.40541012631901835
+      - -0.083299866654055463
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 377355128
+  - positions:
+      - 1.570652283879667
+      - -1.9292457448418687
+      - 1.1298326004802759
+      - -1.5708107275666088
+      - -2.2004738791464347e-05
+      - -0.020503291915724545
+      - -0.71796560873184234
+    velocities:
+      - -0.002053869885471577
+      - 0.14279281264943888
+      - -0.059893894893902899
+      - 6.9435779446146812e-06
+      - 4.9378672274534836e-05
+      - 0.37379319955141865
+      - -0.074841549907544463
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 395057336
+  - positions:
+      - 1.5706222063179007
+      - -1.9268166512115716
+      - 1.1288003131578046
+      - -1.5708107141564811
+      - -2.1171125938294238e-05
+      - -0.01415958759331565
+      - -0.71924123937265605
+    velocities:
+      - -0.0014646594896476433
+      - 0.12841026270207653
+      - -0.055058810439967729
+      - 5.6270433258952435e-06
+      - 4.3497199110869848e-05
+      - 0.33458910177401635
+      - -0.067427586411627244
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 413490503
+  - positions:
+      - 1.5705976614095085
+      - -1.924436361863568
+      - 1.1277703236475962
+      - -1.5708107171995467
+      - -2.0370212921790157e-05
+      - -0.0079842186046028205
+      - -0.72049099106981906
+    velocities:
+      - -0.0003720514664759574
+      - 0.11129778638973657
+      - -0.049810180750576714
+      - -1.1315501024035119e-05
+      - 3.789542616762116e-05
+      - 0.28959869004614958
+      - -0.059406861475726207
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 434947670
+  - positions:
+      - 1.5705902351063201
+      - -1.9221900016366777
+      - 1.1267539015997006
+      - -1.5708106836582401
+      - -1.9620799557609891e-05
+      - -0.0021429881793931516
+      - -0.72168846461147762
+    velocities:
+      - 0.00014181551025170869
+      - 0.09477483186094264
+      - -0.043827371243143509
+      - -1.0332322286281601e-05
+      - 3.194078071341044e-05
+      - 0.24650014283838342
+      - -0.051019928326277122
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 458313753
+  - positions:
+      - 1.5705921452311391
+      - -1.9199763313801963
+      - 1.1257227676793924
+      - -1.5708106341923944
+      - -1.8891313664761962e-05
+      - 0.0036008243256730197
+      - -0.72287587065319714
+    velocities:
+      - 0.00035618411379883933
+      - 0.077210222932871789
+      - -0.036619388784356256
+      - -4.9566858986255599e-06
+      - 2.5583948956809914e-05
+      - 0.20008940182031071
+      - -0.041701336064986161
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 486931920
+  - positions:
+      - 1.57060168832026
+      - -1.9177344823870537
+      - 1.124652740734932
+      - -1.5708105724777273
+      - -1.8161733013521495e-05
+      - 0.0093728145319992608
+      - -0.72408263259546402
+    velocities:
+      - 0.00029160769600424457
+      - 0.056400024789809371
+      - -0.027276184688603641
+      - 4.9901544224360959e-06
+      - 1.8159231586274585e-05
+      - 0.14465791134213163
+      - -0.030412090462949581
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 526549253
+  - positions:
+      - 1.570600427140959
+      - -1.9158063830555672
+      - 1.1237481749983884
+      - -1.5708105222310234
+      - -1.7549101341530529e-05
+      - 0.014123493505043013
+      - -0.72510117552684206
+    velocities:
+      - -2.7464441616498501e-06
+      - 0.027896895091562735
+      - -0.013082410556315142
+      - -5.1095235205166627e-07
+      - 8.8795475043796061e-06
+      - 0.064601614715375344
+      - -0.014726679851370212
+    accelerations:
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+      - 0
+    effort: []
+    time_from_start:
+      sec: 5
+      nanosec: 578457211
diff --git a/src/lab_sim/objectives/stitch_multiple_point_clouds_together.xml b/src/lab_sim/objectives/stitch_multiple_point_clouds_together.xml
index e2e539bf7..f02747488 100644
--- a/src/lab_sim/objectives/stitch_multiple_point_clouds_together.xml
+++ b/src/lab_sim/objectives/stitch_multiple_point_clouds_together.xml
@@ -18,7 +18,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
-        position_xyz="0.5;0.3;0.5"
+        position_xyz="0.5;0.1;0.5"
         stamped_pose="{pose1}"
         reference_frame="world"
       />
@@ -30,7 +30,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
-        position_xyz="0.6;0.4;0.5"
+        position_xyz="0.6;0.2;0.5"
         stamped_pose="{pose2}"
         reference_frame="world"
       />
@@ -42,7 +42,7 @@
       <Action
         ID="CreateStampedPose"
         orientation_xyzw="0;-1;0;0"
-        position_xyz="0.4;0.5;0.5"
+        position_xyz="0.4;0.3;0.5"
         stamped_pose="{pose3}"
         reference_frame="world"
       />
@@ -209,17 +209,17 @@
         <Metadata runnable="true" />
       </MetadataFields>
       <!--Output ports-->
-      <output_port name="inspection_poses" default="{inspection_poses}" />
-      <output_port name="reachable_pose" default="{reachable_pose}" />
+      <output_port name="current_cloud" default="{current_cloud}" />
+      <output_port name="current_pose" default="{current_pose}" />
       <output_port name="first_solution" default="{first_solution}" />
+      <output_port name="image" default="{image}" />
+      <output_port name="inspection_poses" default="{inspection_poses}" />
       <output_port name="joint_traj" default="{joint_traj}" />
-      <output_port name="current_pose" default="{current_pose}" />
+      <output_port name="merged_cloud" default="{merged_cloud}" />
       <output_port name="move_solution" default="{move_solution}" />
-      <output_port name="image" default="{image}" />
       <output_port name="point_cloud_vector" default="{point_cloud_vector}" />
-      <output_port name="merged_cloud" default="{merged_cloud}" />
+      <output_port name="reachable_pose" default="{reachable_pose}" />
       <output_port name="replay_solution" default="{replay_solution}" />
-      <output_port name="current_cloud" default="{current_cloud}" />
       <output_port name="transformed_cloud" default="{transformed_cloud}" />
     </SubTree>
   </TreeNodesModel>
diff --git a/src/lab_sim/test/objectives_integration_test.py b/src/lab_sim/test/objectives_integration_test.py
index f973fec91..27f0165e7 100644
--- a/src/lab_sim/test/objectives_integration_test.py
+++ b/src/lab_sim/test/objectives_integration_test.py
@@ -40,49 +40,41 @@
 # Looping objectives to cancel partway through
 cancel_objectives = {
     "3 Waypoints Pick and Place",
-    "Stationary Admittance",
+    "Classical Pick and Place",
     "Cycle Between Waypoints",
-    "Grasp Planning",
+    "Get AprilTag Pose from Image",
     "Grasp Pose Tuning With April Tag",
     "Grasp Pose Using Yaml",
-    "Joint Diagnostic",
-    "Loop Detect AprilTag",
-    "Pick And Place Example",
     "Pick April Tag Labeled Object With Approval",
-    "Place Object",
     "Plan and Save Trajectory",
     "Record and Replay Scanning Motion",
-    "Classical Pick and Place",
-    "ML Grasp Object from Text Prompt",
+    "Stationary Admittance",
 }
 
 # Objectives to skip entirely from integration testing
 skip_objectives = {
-    "Grasp Object from Text Prompt",  # https://github.com/PickNikRobotics/moveit_pro/issues/13236
+    "AddBottlesToPlanningScene",
+    "Get Grasp from Text Prompt Subtree",  # https://github.com/PickNikRobotics/moveit_pro/issues/13236
     "Grasp Planning",
     "Joint Diagnostic",
-    "Scan Multiple Views",
-    "MPC Pose Tracking",
-    "MPC Pose Tracking With Point Cloud Avoidance",
-    "MPC Pose Tracking With Static Sphere Point Cloud Avoidance",
-    "MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample",
-    "MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample",
+    "ML Auto Grasp Object from Clicked Point",  # Skipped because there is no primary ui to switch to in ci
     "ML Find Bottles on Table from Image Exemplar",  # Skipped because it looks for a file on a home path
     "ML Segment Bottles from File",  # Skipped because it looks for a file on a home path
     "ML Segment Image",
     "ML Segment Point Cloud from Clicked Point",
-    "ML Auto Grasp Object from Clicked Point",  # Skipped because there is no primary ui to switch to in ci
+    "MPC Pose Tracking",
+    "MPC Pose Tracking With Point Cloud Avoidance",
+    "Octomap Example",  # Requires user input to clear the octomap.
+    "Pick 1 Pill Bottle with ML",
+    "Pick All Bottles with AprilTags",
     "Pick All Pill Bottles",
-    "Pick up Cube",
+    "Pick up Object",
     "Place Object",
     "Record Square Trajectory",
+    "Scan Scene - Multiple Point Clouds",
     "Stack Objects with ICP",  # Skipped because there is no primary ui to switch to in ci
-    "Teleoperate",
     "Stitch Multiple Point Clouds Together",
-    "Pick 1 Pill Bottle",
-    "AddBottlesToPlanningScene",
     "Marker Visualization Example",  # Server not available for GetTextFromUser
-    "Octomap Example",  # Requires user input to clear the octomap.
     "Register CAD Part",  # TODO: remove this for 9.2.0 and fix it correctly
 }
 
@@ -110,4 +102,13 @@ def test_all_objectives(
     execute_objective_resource: ExecuteObjectiveResource,
 ):
     wait_for_gripper_action_server()
-    run_objective(objective_id, should_cancel, execute_objective_resource)
+    try:
+        run_objective(objective_id, should_cancel, execute_objective_resource)
+    except AssertionError as e:
+        mode = "cancel" if should_cancel else "execute"
+        pytest.fail(f"Objective '{objective_id}' failed to {mode}: {e}")
+    except Exception as e:
+        mode = "cancel" if should_cancel else "execute"
+        pytest.fail(
+            f"Objective '{objective_id}' hit an unexpected error during {mode}: {type(e).__name__}: {e}"
+        )
diff --git a/src/lab_sim/waypoints/ur_waypoints.yaml b/src/lab_sim/waypoints/ur_waypoints.yaml
index c03513913..65d7c40ea 100644
--- a/src/lab_sim/waypoints/ur_waypoints.yaml
+++ b/src/lab_sim/waypoints/ur_waypoints.yaml
@@ -21,57 +21,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.06656709465883658
-    - 0.31742962464478663
-    - -0.07518824419393247
-    - -0.7207568718596512
-    - 0.8665668564352057
-    - -1.6837956156664982
-    - -1.5707507158288945
-    - -0.07517349313281775
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Above Pick Object
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.06656710337674145
-    - 0.1162925924535442
-    - 0.2054240147028693
-    - -0.987070953194143
-    - 1.3146849599773849
-    - -1.8698622612624394
-    - -1.5707050706852668
-    - 0.20529114323213696
+    - 0.0026038249605273597
+    - -0.2150255024029047
+    - 0.11192462801491523
+    - -2.18036249863573
+    - 1.6587113787793821
+    - -1.4687347456884468
+    - -1.5919159056315824
+    - 0.08471562792466861
     velocity: []
   multi_dof_joint_state:
     header:
@@ -83,7 +40,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Above Place Cube
+  name: 3 Waypoint Mid Point
 - description: ''
   favorite: false
   joint_group_names:
@@ -107,14 +64,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639671853765404
-    - 0.6551230244120644
-    - -2.597276923961381
-    - -1.7207992223871735
-    - -1.8054851963579275
-    - -2.9105970459798978
-    - -1.012936176528235
-    - 3.2513966966550787
+    - 0.052556448066187256
+    - 0.14423170107824168
+    - -0.07537379607283551
+    - -1.2866092520867383
+    - 2.438370923671909
+    - -2.7991165107946703
+    - -1.552715310086053
+    - -0.33811216558077
     velocity: []
   multi_dof_joint_state:
     header:
@@ -126,7 +83,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 1a - Pre Pick
+  name: 3 Waypoint Pick Pose
 - description: ''
   favorite: false
   joint_group_names:
@@ -150,14 +107,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639662602883518
-    - 0.6416717485181368
-    - -2.4872028523038185
-    - -2.2797066973266604
-    - -2.135135409489318
-    - -2.0350474070457296
-    - -0.9043501601840921
-    - 3.2728332839692875
+    - 0.6020243803964747
+    - -0.5137181686901243
+    - -0.2723648029022852
+    - -1.0052256415555374
+    - 1.8573472354338991
+    - -2.4941180496510045
+    - -1.53809881022558
+    - -0.5347223269502246
     velocity: []
   multi_dof_joint_state:
     header:
@@ -169,7 +126,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 1b - Pick
+  name: 3 Waypoint Place Pose
 - description: ''
   favorite: false
   joint_group_names:
@@ -193,100 +150,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639676351734388
-    - -0.14695746092055925
-    - -2.8650633002008017
-    - -1.8611906531500544
-    - -1.7478027758503683
-    - -2.810467735524929
-    - -1.2774332801079342
-    - 3.2091308552581377
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 1c - Pre Place
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639636129146917
-    - -0.14375285311965005
-    - -2.8600433649921495
-    - -1.9461602101793198
-    - -1.8756681071759462
-    - -2.597844115240611
-    - -1.272458370221303
-    - 3.209846268837379
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 1d - Place
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.7639644752524631
-    - 0.8130633024853088
-    - -2.726859433670606
-    - -1.8891510728754208
-    - -1.424665285030513
-    - -3.112945310941763
-    - -1.1412179091798282
-    - 3.2296261212002166
+    - 0.06656709465883658
+    - 0.31742962464478663
+    - -0.07518824419393247
+    - -0.7207568718596512
+    - 0.8665668564352057
+    - -1.6837956156664982
+    - -1.5707507158288945
+    - -0.07517349313281775
     velocity: []
   multi_dof_joint_state:
     header:
@@ -298,7 +169,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 2a - Pre Pick
+  name: Above Pick Object
 - description: ''
   favorite: false
   joint_group_names:
@@ -322,14 +193,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639671534487822
-    - 0.7786119481205361
-    - -2.5914980762265736
-    - -2.386004216551112
-    - -1.882582057672306
-    - -2.1695739396137577
-    - -1.0073664867894194
-    - 3.25251851297434
+    - 0.447969045650818
+    - 0.6474400118806204
+    - -0.06033511474114565
+    - -1.2791772736274891
+    - 2.3829614234150545
+    - -2.8734344275394026
+    - -1.5569395003854831
+    - -0.059925837606245605
     velocity: []
   multi_dof_joint_state:
     header:
@@ -341,7 +212,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 2b - Pick
+  name: Flask 1 - Pick
 - description: ''
   favorite: false
   joint_group_names:
@@ -365,14 +236,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639660563824677
-    - -0.2402305144870719
-    - -2.8832984482674955
-    - -1.8018009466305653
-    - -1.685247569441112
-    - -2.956647634694288
-    - -1.2898622616661428
-    - 3.2014481210116723
+    - 0.47520773972835983
+    - -0.17547027905239
+    - -0.20267583823127783
+    - -1.2046710251373107
+    - 1.9032859450982782
+    - -2.4569993707131843
+    - -1.5292842611843418
+    - -0.19960068225580938
     velocity: []
   multi_dof_joint_state:
     header:
@@ -384,7 +255,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 2c - Pre Place
+  name: Flask 1 - Place
 - description: ''
   favorite: false
   joint_group_names:
@@ -408,14 +279,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639675199131791
-    - -0.24086053204199623
-    - -2.876977231591623
-    - -1.939806049514917
-    - -1.8984021894234757
-    - -2.6063762242952886
-    - -1.2836686932398813
-    - 3.202498456119905
+    - 0.05255644806618728
+    - 0.8012030897493988
+    - -0.07032966095174241
+    - -1.2073944811122455
+    - 2.183090332371707
+    - -2.7463093672335517
+    - -1.5551496517533123
+    - -0.06950241310132783
     velocity: []
   multi_dof_joint_state:
     header:
@@ -427,7 +298,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 2d - Place
+  name: Flask 2 - Pick
 - description: ''
   favorite: false
   joint_group_names:
@@ -451,14 +322,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639685849147584
-    - 0.6446458760506458
-    - -2.7780078487258892
-    - -1.9155374976991195
-    - -1.3864706183602202
-    - -3.1216050652579663
-    - -1.191550429146523
-    - 3.221797617570794
+    - 0.5536302060922608
+    - -0.23131956337150142
+    - -0.2893885581395494
+    - -1.2116786062171627
+    - 1.8954143536642922
+    - -2.4344194461107844
+    - -1.5133904193700953
+    - -0.2845874079784073
     velocity: []
   multi_dof_joint_state:
     header:
@@ -470,9 +341,9 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3a - Pre Pick
+  name: Flask 2 - Place
 - description: ''
-  favorite: false
+  favorite: true
   joint_group_names:
   - gripper
   - linear_actuator
@@ -494,14 +365,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639682422101212
-    - 0.6144316834131173
-    - -2.6658657366983283
-    - -2.367501290161342
-    - -1.887225784038829
-    - -2.176385304935698
-    - -1.0805693269230385
-    - 3.239557435454622
+    - 0.020668448949024514
+    - 0.20879448072947002
+    - 0.048630656629801594
+    - -2.062190505651845
+    - 1.3002462715341856
+    - -1.278104242210415
+    - -1.592754310292377
+    - 0.043377416825533394
     velocity: []
   multi_dof_joint_state:
     header:
@@ -513,7 +384,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3b - Pick
+  name: Home
 - description: ''
   favorite: false
   joint_group_names:
@@ -537,14 +408,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639614914449789
-    - -0.24670890913790466
-    - -2.9097897371763177
-    - -2.057135798403447
-    - -1.2794632064231153
-    - -3.1064892677695757
-    - -1.3160558724133762
-    - 3.197049363506155
+    - 0.0125324474858072
+    - -2.869335412055937e-07
+    - 1.570785841301689
+    - -1.8849672195965093
+    - 1.107847094370458
+    - -0.7406642377583046
+    - -1.5708034344833448
+    - -1.8687903073962402e-09
     velocity: []
   multi_dof_joint_state:
     header:
@@ -556,8 +427,8 @@
     transforms: []
     twist: []
     wrench: []
-  name: Flask 3c - Pre Place
-- description: ''
+  name: Look at Base
+- description: Location left of the burner to look for bottles
   favorite: false
   joint_group_names:
   - gripper
@@ -580,57 +451,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639590920670264
-    - -0.25674010578815215
-    - -2.904024374297564
-    - -2.1809624103341
-    - -1.4905916971704432
-    - -2.771777026484768
-    - -1.3103568767884126
-    - 3.198001370172111
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Flask 3d - Place
-- description: ''
-  favorite: true
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.01688327588273299
-    - -7.398158482916995e-11
-    - -3.8075799469380365e-09
-    - -1.8849146307875049
-    - 1.1008590123422952
-    - -1.256467912694488
-    - -1.5707500007156492
-    - 2.510085262493143e-12
+    - 0.052587508554489834
+    - 0.2968578480078406
+    - -0.006148632895028194
+    - -0.8175631700397911
+    - 1.2592096237510881
+    - -2.2864772465639494
+    - -1.548620949533172
+    - 0.0017334387814990154
     velocity: []
   multi_dof_joint_state:
     header:
@@ -642,7 +470,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Home
+  name: Look at Bottles Left
 - description: ''
   favorite: false
   joint_group_names:
@@ -666,57 +494,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.0125324474858072
-    - -2.869335412055937e-07
-    - 1.570785841301689
-    - -1.8849672195965093
-    - 1.107847094370458
-    - -0.7406642377583046
-    - -1.5708034344833448
-    - -1.8687903073962402e-09
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: Look at Base
-- description: Location left of the burner to look for bottles
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.052587508554489834
-    - 0.2968578480078406
-    - -0.006148632895028194
-    - -0.8175631700397911
-    - 1.2592096237510881
-    - -2.2864772465639494
-    - -1.548620949533172
-    - 0.0017334387814990154
+    - 0.005250736667361686
+    - 0.28905780343437465
+    - -2.080646628339534
+    - -2.141055377241851
+    - -1.2137155511655011
+    - 2.1016864355675033
+    - -1.197419807165355
+    - -2.330740475762169
     velocity: []
   multi_dof_joint_state:
     header:
@@ -728,7 +513,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Look at Bottles Left
+  name: Look at Flasks
 - description: ''
   favorite: false
   joint_group_names:
@@ -752,14 +537,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.005250736667361686
-    - 0.28905780343437465
-    - -2.080646628339534
-    - -2.141055377241851
-    - -1.2137155511655011
-    - 2.1016864355675033
-    - -1.197419807165355
-    - -2.330740475762169
+    - 0.024529733190018374
+    - 0.8640238854000203
+    - 0.12365606841447244
+    - -1.2368601606650749
+    - 0.471969963412724
+    - -0.8370484925009108
+    - -1.1498548799391797
+    - 0.1430141654430969
     velocity: []
   multi_dof_joint_state:
     header:
@@ -771,7 +556,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Look at Flasks
+  name: Look at Microscope
 - description: ''
   favorite: false
   joint_group_names:
@@ -924,14 +709,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.36093425387337075
-    - 0.3468469487674742
-    - -0.08122232656648681
-    - -1.1118986430431266
-    - 0.7735698103006698
-    - -1.2414135949062157
-    - -1.5575126426436692
-    - -0.0803135811027271
+    - 0.05510852026671219
+    - 0.28444944718193443
+    - 0.014734089293982098
+    - -1.363461962694742
+    - 1.1221354194996502
+    - -1.3396454745687125
+    - -1.5584288659596743
+    - 0.015645930749496093
     velocity: []
   multi_dof_joint_state:
     header:
@@ -944,7 +729,7 @@
     twist: []
     wrench: []
   name: Look at Table
-- description: Move out of view of scene camera
+- description: ''
   favorite: false
   joint_group_names:
   - gripper
@@ -967,14 +752,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.00780498767879874
-    - -1.7207208458780023
-    - -2.9355287074907658e-05
-    - -1.8849275826089145
-    - 1.102019084998803
-    - -1.2563366868253756
-    - -1.5707679924921147
-    - 1.5931846964135707e-07
+    - 0.013384299003678032
+    - 0.166019314519922
+    - 0.15757550190047065
+    - -1.345468374557612
+    - 1.4445329342541495
+    - -1.6491569813676528
+    - -1.566378390573665
+    - 0.15622741523103717
     velocity: []
   multi_dof_joint_state:
     header:
@@ -986,8 +771,8 @@
     transforms: []
     twist: []
     wrench: []
-  name: Park Far Right
-- description: ''
+  name: MPC Pose Tracking
+- description: Move out of view of scene camera
   favorite: false
   joint_group_names:
   - gripper
@@ -1010,14 +795,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.06656709575770764
-    - 0.2544520984236789
-    - 0.00297097958947393
-    - -0.5454836524482858
-    - 1.028930149174636
-    - -2.02088483587436
-    - -1.5709145918870968
-    - 0.002734726536987722
+    - 0.00780498767879874
+    - -1.7207208458780023
+    - -2.9355287074907658e-05
+    - -1.8849275826089145
+    - 1.102019084998803
+    - -1.2563366868253756
+    - -1.5707679924921147
+    - 1.5931846964135707e-07
     velocity: []
   multi_dof_joint_state:
     header:
@@ -1029,7 +814,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Pick Cube
+  name: Park Far Right
 - description: ''
   favorite: false
   joint_group_names:
@@ -1053,14 +838,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.06656710453529632
-    - -0.026311129005491703
-    - 0.391188076851874
-    - -0.6421557305136849
-    - 1.2200227002982904
-    - -2.1172063445999307
-    - -1.5707036597762196
-    - 0.39119709035689026
+    - 0.7684105789223642
+    - 0.03463677679065625
+    - 0.3822704394086136
+    - -0.8309305843034361
+    - 1.0446811438476495
+    - -1.759429041466107
+    - -1.5599967460954534
+    - 0.3889991676916786
     velocity: []
   multi_dof_joint_state:
     header:
@@ -1072,7 +857,7 @@
     transforms: []
     twist: []
     wrench: []
-  name: Place Cube
+  name: Pose Tracking
 - description: Location for the robot to go to before placing bottle.
   favorite: false
   joint_group_names:
@@ -1096,14 +881,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.005274374788506071
-    - -0.65899502183384
-    - -0.011044571105883248
-    - -0.9740961688696632
-    - 1.2354891461984063
-    - -2.0150613197113008
-    - -1.5477140380091456
-    - -0.0030875579024600766
+    - 0.05510852026866443
+    - -0.7189055036814227
+    - 0.19457050986833238
+    - -2.087568874212717
+    - 1.7974325760598713
+    - -1.6887096601663198
+    - -1.627206520781829
+    - 0.19188249774293678
     velocity: []
   multi_dof_joint_state:
     header:
@@ -1139,14 +924,14 @@
     - wrist_2_joint
     - wrist_3_joint
     position:
-    - 0.7639616943900628
-    - 0.5467729038282597
-    - -1.1106700374717813
-    - -0.6197663519343527
-    - 1.2680653231682326
-    - -2.801491408619309
-    - -1.2801931991207176
-    - -0.8417481239202634
+    - 0.7639710982123997
+    - 0.5980280019173311
+    - -1.4045027646333692
+    - -0.8308328421640113
+    - 1.4619205464444147
+    - -2.548159794869791
+    - -1.1038983831662605
+    - -1.218510576251628
     velocity: []
   multi_dof_joint_state:
     header:
@@ -1288,89 +1073,3 @@
     twist: []
     wrench: []
   name: Wrist 2 Min
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.01575741574061428
-    - 0.8900780780594006
-    - -2.802295813677834
-    - -2.68730855580549
-    - -1.2398951805562326
-    - -2.462048876440746
-    - -1.2180411722914284
-    - 3.2264553538354073
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: behind_flask
-- description: ''
-  favorite: false
-  joint_group_names:
-  - gripper
-  - linear_actuator
-  - manipulator
-  joint_state:
-    effort: []
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    name:
-    - robotiq_85_left_knuckle_joint
-    - linear_rail_joint
-    - shoulder_pan_joint
-    - shoulder_lift_joint
-    - elbow_joint
-    - wrist_1_joint
-    - wrist_2_joint
-    - wrist_3_joint
-    position:
-    - 0.01434864013918258
-    - 0.6726165309151089
-    - 0.12515773780769576
-    - -0.41585738104189296
-    - 0.7247962836696739
-    - -2.9740281980535777
-    - -1.63154143414899
-    - 0.06364200257035163
-    velocity: []
-  multi_dof_joint_state:
-    header:
-      frame_id: ''
-      stamp:
-        nanosec: 0
-        sec: 0
-    joint_names: []
-    transforms: []
-    twist: []
-    wrench: []
-  name: near_rack
diff --git a/src/picknik_accessories b/src/picknik_accessories
index 876d0d2a1..8b3999fe9 160000
--- a/src/picknik_accessories
+++ b/src/picknik_accessories
@@ -1 +1 @@
-Subproject commit 876d0d2a13cabce81fbac045631e61d445641728
+Subproject commit 8b3999fe9da19de455feb2da8d1d0ea0edb2af28

From 060b2cfc39b4ba35494a7f50f01cf01bca58123e Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Thu, 2 Apr 2026 08:39:23 -0600
Subject: [PATCH 2/3] fix(lab_sim): Increase GetPointCloud timeout in Register
 CAD Part

Increase message and publisher timeouts from 5s to 10s to reduce
flaky CI failures when MuJoCo rendering is slow.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
---
 src/lab_sim/objectives/register_cad_part.xml | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/lab_sim/objectives/register_cad_part.xml b/src/lab_sim/objectives/register_cad_part.xml
index a760fbdfd..bcafb5063 100644
--- a/src/lab_sim/objectives/register_cad_part.xml
+++ b/src/lab_sim/objectives/register_cad_part.xml
@@ -40,7 +40,12 @@
         _collapsed="true"
         waypoint_name="Look at Microscope"
       />
-      <Action ID="GetPointCloud" message_out="{wrist_point_cloud}" />
+      <Action
+        ID="GetPointCloud"
+        message_out="{wrist_point_cloud}"
+        message_timeout_sec="10.0"
+        publisher_timeout_sec="10.0"
+      />
       <Action
         ID="TransformPointCloudFrame"
         input_cloud="{wrist_point_cloud}"

From 4003c6ff50a3f095c994697d143740b55380587a Mon Sep 17 00:00:00 2001
From: Dave Coleman <dave@picknik.ai>
Date: Thu, 2 Apr 2026 11:22:00 -0600
Subject: [PATCH 3/3] fix(lab_sim): Add Teleoperate to cancel_objectives in
 integration test

Teleoperate runs indefinitely waiting for user input, so it should be
cancelled after a timeout rather than expected to complete.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
---
 src/lab_sim/test/objectives_integration_test.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/lab_sim/test/objectives_integration_test.py b/src/lab_sim/test/objectives_integration_test.py
index 27f0165e7..2888c6091 100644
--- a/src/lab_sim/test/objectives_integration_test.py
+++ b/src/lab_sim/test/objectives_integration_test.py
@@ -49,6 +49,7 @@
     "Plan and Save Trajectory",
     "Record and Replay Scanning Motion",
     "Stationary Admittance",
+    "Teleoperate",
 }
 
 # Objectives to skip entirely from integration testing