Added 4 larger april tags to each corner. Grappling objective works.

Author: rlpratt12

src/site_configs/picknik_kinova_satellite_sim/config/config.yaml

@@ -26,7 +26,7 @@ hardware:
     urdf_params:
       - arm: "gen3"
       - dof: "7"
-      - mujoco_keyframe: "view_tag"
+      - mujoco_keyframe: "moving_satellite"
       - mujoco_model: "description/mujoco/scene.xml"
       - mujoco_viewer: "true"
 

src/site_configs/picknik_kinova_satellite_sim/config/fuse/fuse.rviz

@@ -51,7 +51,7 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
-    - Angle Tolerance: 0.10000000149011612
+    - Angle Tolerance: 0.0010000000474974513
       Class: rviz_default_plugins/Odometry
       Covariance:
         Orientation:
@@ -69,9 +69,9 @@ Visualization Manager:
           Value: true
         Value: true
       Enabled: true
-      Keep: 100
+      Keep: 1
       Name: Odometry
-      Position Tolerance: 0.10000000149011612
+      Position Tolerance: 0.0010000000474974513
       Shape:
         Alpha: 1
         Axes Length: 1
@@ -299,6 +299,8 @@ Visualization Manager:
           Value: false
         half_arm_2_link:
           Value: false
+        map:
+          Value: true
         mj_world:
           Value: false
         odom:
@@ -367,7 +369,66 @@ Visualization Manager:
       Show Axes: true
       Show Names: false
       Tree:
-        {}
+        mj_world:
+          fts:
+            {}
+          pinch:
+            {}
+          scene_camera_optical_frame:
+            {}
+          world:
+            base_link:
+              shoulder_link:
+                half_arm_1_link:
+                  half_arm_2_link:
+                    forearm_link:
+                      spherical_wrist_1_link:
+                        spherical_wrist_2_link:
+                          bracelet_link:
+                            end_effector_link:
+                              fts_link:
+                                robotiq_85_base_link:
+                                  finger_tip_closed_point:
+                                    {}
+                                  grasp_link:
+                                    {}
+                                  robotiq_85_left_inner_knuckle_link:
+                                    {}
+                                  robotiq_85_left_knuckle_link:
+                                    robotiq_85_left_finger_link:
+                                      robotiq_85_left_finger_tip_link:
+                                        {}
+                                  robotiq_85_right_inner_knuckle_link:
+                                    {}
+                                  robotiq_85_right_knuckle_link:
+                                    robotiq_85_right_finger_link:
+                                      robotiq_85_right_finger_tip_link:
+                                        {}
+            external_camera_link:
+              scene_camera_bottom_screw_frame:
+                scene_camera_link:
+                  scene_camera_color_frame:
+                    scene_camera_color_optical_frame:
+                      {}
+                  scene_camera_depth_frame:
+                    scene_camera_depth_optical_frame:
+                      {}
+                  scene_camera_infra1_frame:
+                    scene_camera_infra1_optical_frame:
+                      {}
+                  scene_camera_infra2_frame:
+                    scene_camera_infra2_optical_frame:
+                      {}
+            map:
+              {}
+            odom:
+              {}
+            table:
+              {}
+          wrist_camera_optical_frame:
+            satellite_tag_0:
+              target_frame_satellite_tag_0:
+                {}
       Update Interval: 0
       Value: true
   Enabled: true
@@ -416,7 +477,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 5.894608974456787
+      Distance: 3.2315683364868164
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -431,10 +492,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4603983163833618
+      Pitch: 0.4003983736038208
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.8403979539871216
+      Yaw: 5.708583354949951
     Saved: ~
 Window Geometry:
   Displays:

src/site_configs/picknik_kinova_satellite_sim/config/fuse/fuse.yaml

@@ -38,17 +38,15 @@ state_estimator:
       # the target frame should be the center of mass of the object for optimal performance, as the motion model assumes that it is operating on the center of mass of the object
       # the april tag (`satellite_tag_x`) will be a descendent of the estimation frame (`servicer_camera_optical_frame`) in the tf tree
       # thus, we need a different leaf in the tf tree to represent each satellite tag's correspondence to the target frame
-      # transforms: ['satellite_tag_0', 'satellite_tag_1', 'satellite_tag_2']
-      transforms: ['satellite_tag_0']
+      transforms: ['satellite_tag_0', 'satellite_tag_1', 'satellite_tag_2', 'satellite_tag_3']
 
       # this frame is the frame that we are actually estimating
       # fuse will publish an odom from the `estimation_frame` to this frame (and optionally a tf, too)
       target_frame: 'target_frame'
 
       # this is the frame that all transforms should be parented to for fuse to use as a measurement
       # thus a measurement will be a tf from `estimation_frame` to any frame in `transforms` (see above)
-      estimation_frame: 'wrist_camera_optical_frame'
-      # estimation_frame: 'scene_camera_optical_frame'
+      estimation_frame: 'scene_camera_optical_frame'
       position_dimensions: ['x', 'y', 'z']
       orientation_dimensions: ['roll', 'pitch', 'yaw']
       # TODO(anyone): set these empirically (collect data and estimate covariances) instead of just with arbitrary values
@@ -73,11 +71,9 @@ state_estimator:
       odom_frame_id: 'odom'
 
       # this should be the same as `estimation_frame` in `transform_sensor` above
-      map_frame_id: 'wrist_camera_optical_frame'
-      # map_frame_id: 'scene_camera_optical_frame'
+      map_frame_id: 'scene_camera_optical_frame'
       # this should be the same as `map_frame_id`
-      world_frame_id: 'wrist_camera_optical_frame'
-      # world_frame_id: 'scene_camera_optical_frame'
+      world_frame_id: 'scene_camera_optical_frame'
 
       # optionally publish a tf
       publish_tf: true

src/site_configs/picknik_kinova_satellite_sim/description/mujoco/scene.xml

@@ -42,14 +42,6 @@
       texrepeat="5 5"
       reflectance="0.2"
     />
-    <texture
-      type="2d"
-      name="april_tag"
-      file="assets/black_white.png"
-      width="32"
-      height="32"
-    />
-    <material name="april_tag" texture="april_tag" />
     <texture
       type="2d"
       name="satellite"
@@ -58,6 +50,14 @@
       height="32"
     />
     <material name="satellite" texture="satellite" />
+    <texture type="2d" name="ar_marker_0" file="assets/tag36_11_00000.png"/>
+    <texture type="2d" name="ar_marker_1" file="assets/tag36_11_00001.png"/>
+    <texture type="2d" name="ar_marker_2" file="assets/tag36_11_00002.png"/>
+    <texture type="2d" name="ar_marker_3" file="assets/tag36_11_00003.png"/>
+    <material name="ar_marker_0" texture="ar_marker_0" texuniform="false"/>
+    <material name="ar_marker_1" texture="ar_marker_1" texuniform="false"/>
+    <material name="ar_marker_2" texture="ar_marker_2" texuniform="false"/>
+    <material name="ar_marker_3" texture="ar_marker_3" texuniform="false"/>
   </asset>
 
   <worldbody>
@@ -74,13 +74,12 @@
     <site
       name="scene_camera_optical_frame"
       pos="-0.3 0.3 1.0"
-      euler="3.1416 -1.0472 -1.5708"
+      euler="3.142 1.047 1.571"
       size="0.03"
     />
   </worldbody>
 
   <asset>
-    <mesh name="april_tag" file="april_tag.obj" />
     <mesh name="satellite" file="satellite.obj" />
     <mesh name="satellite_box" file="satellite_box.obj" />
     <mesh name="rafti_collision_1" file="rafti_collision_1.obj" />
@@ -170,6 +169,34 @@
       <geom class="collision" mesh="ring_collision_30" />
       <geom class="collision" mesh="ring_collision_31" />
       <geom class="collision" mesh="ring_collision_32" />
+      <body name="ar_marker_0" pos="-0.09 -0.09 .132" quat="1 0 0 0">
+        <geom
+          type="box"
+          size="0.04 0.04 0.002"
+          rgba="1 1 1 1"
+          material="ar_marker_0"/>
+      </body>
+      <body name="ar_marker_1" pos="0.09 0.09 .132" quat="1 0 0 0">
+        <geom
+          type="box"
+          size="0.04 0.04 0.002"
+          rgba="1 1 1 1"
+          material="ar_marker_1"/>
+      </body>
+      <body name="ar_marker_2" pos="-0.09 0.09 .132" quat="1 0 0 0">
+        <geom
+          type="box"
+          size="0.04 0.04 0.002"
+          rgba="1 1 1 1"
+          material="ar_marker_2"/>
+      </body>
+      <body name="ar_marker_3" pos="0.09 -0.09 .132" quat="1 0 0 0">
+        <geom
+          type="box"
+          size="0.04 0.04 0.002"
+          rgba="1 1 1 1"
+          material="ar_marker_3"/>
+      </body>
     </body>
     <body name="gen3_7dof" pos="0 0 0" quat="1 0 0 0">
       <include file="gen3_7dof_body.xml"/>
@@ -201,7 +228,7 @@
         0 0 0 0 0 0 0 0"
       />
     <key
-      name="view_tag"
+      name="moving_satellite"
       qpos="
         1 0 0.5 0.7071 0 -0.7071 0
         0 -0.5236 0 2.3562 0 -0.2618 -1.5708 0

src/site_configs/picknik_kinova_satellite_sim/launch/36h11.launch.yml

@@ -15,11 +15,9 @@ launch:
       namespace: apriltag
       remap:
       - from: /apriltag/image_rect
-        to: /wrist_camera/color
-        # to: /scene_camera/color
+        to: /scene_camera/color
       - from: /apriltag/camera_info
-        to: /wrist_camera/camera_info
-        # to: /scene_camera/camera_info
+        to: /scene_camera/camera_info
       param:
       - from: $(find-pkg-share picknik_kinova_satellite_sim)/launch/tags_36h11.yaml
       extra_arg:

src/site_configs/picknik_kinova_satellite_sim/launch/fuse.launch.py

@@ -42,7 +42,22 @@ def generate_launch_description():
             Node(
                 package="tf2_ros",
                 executable="static_transform_publisher",
-                arguments=["0.15", "-0.15", "0", "0", "0", "0", "satellite_tag_0", "target_frame_satellite_tag_0"],
+                arguments=["-0.09", "-0.09", "0", "0", "0", "0", "satellite_tag_0", "target_frame_satellite_tag_0"],
+            ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                arguments=["0.09", "0.09", "0", "0", "0", "0", "satellite_tag_1", "target_frame_satellite_tag_1"],
+            ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                arguments=["-0.09", "0.09", "0", "0", "0", "0", "satellite_tag_2", "target_frame_satellite_tag_2"],
+            ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                arguments=["0.09", "-0.09", "0", "0", "0", "0", "satellite_tag_3", "target_frame_satellite_tag_3"],
             ),
 
             # run our estimator

src/site_configs/picknik_kinova_satellite_sim/launch/tags_36h11.yaml

@@ -16,6 +16,6 @@
 
         # optional list of tags
         tag:
-            ids: [5]            # tag ID
-            frames: [satellite_tag_0]  # optional frame name
-            sizes: [0.051]   # optional tag-specific edge size
+            ids: [0, 1, 2, 3]            # tag ID
+            frames: [satellite_tag_0, satellite_tag_1, satellite_tag_2, satellite_tag_3]  # optional frame name
+            sizes: [0.064, 0.064, 0.064, 0.064]   # optional tag-specific edge size

src/site_configs/picknik_kinova_satellite_sim/waypoints/waypoints.yaml

@@ -122,3 +122,49 @@
     twist: []
     wrench: []
   name: Home
+- description: ''
+  favorite: false
+  joint_group_names:
+  - gripper
+  - manipulator
+  joint_state:
+    effort: []
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    name:
+    - robotiq_85_left_knuckle_joint
+    - robotiq_85_left_finger_joint
+    - robotiq_85_right_finger_joint
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+    - joint_7
+    position:
+    - 0.002603373347255934
+    - 0.00014645411016043903
+    - 0.00014645426688290016
+    - 0.09425935592481213
+    - 0.021586867333101556
+    - -0.17035126324683006
+    - 2.3180565133500926
+    - -0.013744056148720065
+    - -0.7381075423563572
+    - -1.512343082000213
+    velocity: []
+  multi_dof_joint_state:
+    header:
+      frame_id: ''
+      stamp:
+        nanosec: 0
+        sec: 0
+    joint_names: []
+    transforms: []
+    twist: []
+    wrench: []
+  name: home2