more organization, and mostly fixed the frame issue

Author: taylorott

launch/publishers.launch

@@ -28,6 +28,17 @@
  <!--  <node pkg="tf" type="static_transform_publisher" name="world_to_camera" args="5.06423387e-01 6.15130675e-01 1.47602280e-01 -7.77802963e-03 -7.01780459e-01 7.12291922e-01 9.16006536e-03
    base /camera_color_optical_frame 100" /> -->
 
+<!--  <node pkg="tf" type="static_transform_publisher" name="world_to_manipulation_frame" args="0.4, 0.2, 0.0, 0.5, 0.5, 0.5, -0.5
+   base /world_manipulation_frame 100" /> -->
+
+ <node pkg="tf" type="static_transform_publisher" name="world_to_manipulation_frame" args="0.4, 0.2, 0.0, 0.5, -0.5, 0.5, 0.5
+   base /world_manipulation_frame 100" /> 
+
+<!--  <node pkg="tf" type="static_transform_publisher" name="world_to_manipulation_frame" args="0.4, 0.2, 0.0, 0.6532815, 0.270598, 0.6532815, -0.270598
+   base /world_manipulation_frame 100" /> 
+ -->   
+
+
  <node pkg="tf" type="static_transform_publisher" name="world_to_far_cam" args="0.4980355178911215, 0.6018168729441976, 0.1394853205379789, 0.0034037964297700574, -0.6987986850579011, 0.7152145472836245, 0.011703131422520828
    base /far_camera_color_optical_frame 100" />
 

msg/ControlCommand.msg

@@ -4,10 +4,10 @@ int8 mode                       # primitive
 
 # relative commands
 float32 delta_theta
-float32 delta_x_pivot
-float32 delta_s
+float32 delta_s_pivot
+float32 delta_s_hand
 
 # absolute commands
 float32 theta
-float32 x_pivot
-float32 s
+float32 s_pivot
+float32 s_hand

rviz/pbal_settings.rviz

@@ -19,6 +19,7 @@ Panels:
         - /Axes1
         - /Axes2
         - /Axes3
+        - /Pose1
       Splitter Ratio: 0.488469601
     Tree Height: 397
   - Class: rviz/Selection
@@ -39,7 +40,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: Image
+    SyncSource: ""
 Toolbars:
   toolButtonStyle: 2
 Visualization Manager:
@@ -241,29 +242,11 @@ Visualization Manager:
         All Enabled: false
         base:
           Value: false
-        camera_color_optical_frame:
-          Value: false
-        far_cam_color_frame:
-          Value: true
-        far_cam_color_optical_frame:
-          Value: true
-        far_cam_depth_frame:
-          Value: true
-        far_cam_depth_optical_frame:
-          Value: true
-        far_cam_link:
+        far_camera_color_optical_frame:
           Value: true
         ft_sensor:
           Value: false
-        near_cam_color_frame:
-          Value: true
-        near_cam_color_optical_frame:
-          Value: true
-        near_cam_depth_frame:
-          Value: true
-        near_cam_depth_optical_frame:
-          Value: true
-        near_cam_link:
+        near_camera_color_optical_frame:
           Value: true
         panda_EE:
           Value: false
@@ -295,6 +278,8 @@ Visualization Manager:
           Value: false
         world:
           Value: false
+        world_manipulation_frame:
+          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: false
@@ -303,7 +288,9 @@ Visualization Manager:
       Tree:
         world:
           base:
-            camera_color_optical_frame:
+            far_camera_color_optical_frame:
+              {}
+            near_camera_color_optical_frame:
               {}
             panda_link0:
               panda_link1:
@@ -323,6 +310,8 @@ Visualization Manager:
                                 {}
                               panda_hand:
                                 {}
+            world_manipulation_frame:
+              {}
           pbal_origin:
             {}
       Update Interval: 0
@@ -348,6 +337,21 @@ Visualization Manager:
       Radius: 0.00999999978
       Reference Frame: <Fixed Frame>
       Value: true
+    - Alpha: 1
+      Axes Length: 0.100000001
+      Axes Radius: 0.00999999978
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0
+      Head Radius: 0
+      Name: Pose
+      Shaft Length: 0.100000001
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /ee_pose_in_base_from_franka_publisher
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -373,25 +377,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.16289568
+      Distance: 1.06909716
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.503581524
-        Y: 0.124556668
-        Z: 0.159649462
+        X: 0.387553841
+        Y: 0.130680561
+        Z: -0.0345899686
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.0197977386
+      Pitch: 0.155203208
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.62041187
+      Yaw: 0.725394905
     Saved: ~
 Window Geometry:
   Displays:
@@ -411,5 +415,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1855
-  X: 1986
+  X: 1985
   Y: 40