Updating MoveIt! to get demo working in master!

moma_bringup/launch/components/sensors.launch

@@ -12,7 +12,8 @@
     <arg name="depth_fps" value="30" />
     <arg name="enable_pointcloud" value="true" />
     <arg name="publish_tf" value="true" />
-    <arg name="serial_no" value="828112072238" />
+    <arg name="initial_reset" value="true"/>
+    <!-- <arg name="serial_no" value="828112072238" /> -->
   </include>
 
   <!-- Luanch fixed camera -->

moma_bringup/launch/panda_real.launch

@@ -5,7 +5,7 @@
   <arg name="publish_world_link" default="true" />
   <arg name="moveit" default="true" />
   <arg name="arm_id" default="panda" />
-  <arg name="controller" default="effort_joint_trajectory_controller" />
+  <arg name="controller" default="position_joint_trajectory_controller" />
   <arg name="table" default="true" />
   <arg name="use_bota" default="false" />
 

moma_bringup/scripts/icp_test_calibration.py

@@ -0,0 +1,82 @@
+import open3d as o3d
+import copy
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+def draw_registration_result_original_color(source, target, transformation):
+    source_temp = copy.deepcopy(source)
+    source_temp.transform(transformation)
+    o3d.visualization.draw_geometries([source_temp, target],
+                                      zoom=0.5,
+                                      front=[-0.2458, -0.8088, 0.5342],
+                                      lookat=[1.7745, 2.2305, 0.9787],
+                                      up=[0.3109, -0.5878, -0.7468])
+
+print("1. Load two point clouds and show initial pose")
+# source = o3d.io.read_point_cloud("/root/data/run_1/rs_435_2.pcd")
+# target = o3d.io.read_point_cloud("/root/data/run_1/rs_435_3.pcd")
+# source = o3d.io.read_point_cloud("/root/data/run_2/rs_435_2.pcd")
+# target = o3d.io.read_point_cloud("/root/data/run_2/rs_435_3.pcd")
+source = o3d.io.read_point_cloud("/root/data/run_3/rs_435_2.pcd")
+target = o3d.io.read_point_cloud("/root/data/run_3/rs_435_3.pcd")
+
+# draw initial alignment
+translation = [-0.340, -0.096, 0.014]  # Example translation
+quaternion = [-0.028, 0.063, 0.997, -0.039]  # Example quaternion (x, y, z, w)
+rotation_matrix = R.from_quat(quaternion).as_matrix()
+current_transformation = np.eye(4)
+current_transformation[:3, :3] = rotation_matrix
+current_transformation[:3, 3] = translation
+# current_transformation = np.identity(4)
+
+draw_registration_result_original_color(source, target, current_transformation)
+
+# initial_pcd = o3d.geometry.PointCloud(source)  # Copy using the constructor
+# initial_pcd = initial_pcd.transform(current_transformation)
+# draw_registration_result_original_color(source, target, current_transformation)
+
+# # point to plane ICP
+# print("2. Point-to-plane ICP registration is applied on original point")
+# print("   clouds to refine the alignment. Distance threshold 0.02.")
+# result_icp = o3d.pipelines.registration.registration_icp(
+#     source, target, 0.02, current_transformation,
+#     o3d.pipelines.registration.TransformationEstimationPointToPoint())
+# print(result_icp)
+# draw_registration_result_original_color(source, target,
+#                                         result_icp.transformation)
+
+print("3. Colored point cloud registration")
+voxel_radius = [0.04, 0.02, 0.01, 0.05]
+max_iter = [50, 30, 14, 50]
+for scale in range(3):
+    iter = max_iter[scale]
+    radius = voxel_radius[scale]
+    print([iter, radius, scale])
+
+    print("3-1. Downsample with a voxel size %.2f" % radius)
+    source_down = source.voxel_down_sample(radius)
+    target_down = target.voxel_down_sample(radius)
+
+    print("3-2. Estimate normal.")
+    source_down.estimate_normals(
+        o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
+    target_down.estimate_normals(
+        o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
+
+    print("3-3. Applying colored point cloud registration")
+    result_icp = o3d.pipelines.registration.registration_colored_icp(
+        source_down, target_down, radius, current_transformation,
+        o3d.pipelines.registration.TransformationEstimationForColoredICP(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
+                                                          relative_rmse=1e-6,
+                                                          max_iteration=iter))
+
+    # result_icp = o3d.pipelines.registration.registration_icp(
+    #     source_down, target_down, 0.02, current_transformation,
+    #     o3d.pipelines.registration.TransformationEstimationPointToPlane())
+
+
+    current_transformation = result_icp.transformation
+    print(result_icp)
+draw_registration_result_original_color(source, target,
+                                        result_icp.transformation)

moma_demos/grasp_demo/config/grasp_demo.rviz

@@ -4,12 +4,9 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /TF1/Frames1
-        - /RobotModel1
-        - /Quality Cloud1
         - /MarkerArray1/Namespaces1
-      Splitter Ratio: 0.545271635055542
-    Tree Height: 802
+      Splitter Ratio: 0.48490944504737854
+    Tree Height: 808
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -56,20 +53,158 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
         All Enabled: false
+        fixed_camera_link:
+          Value: false
+        panda_EE:
+          Value: false
+        panda_K:
+          Value: false
+        panda_NE:
+          Value: false
+        panda_default_ee:
+          Value: true
+        panda_hand:
+          Value: false
+        panda_hand_sc:
+          Value: false
+        panda_hand_tcp:
+          Value: false
+        panda_leftfinger:
+          Value: false
+        panda_link0:
+          Value: false
+        panda_link0_sc:
+          Value: false
+        panda_link1:
+          Value: false
+        panda_link1_sc:
+          Value: false
+        panda_link2:
+          Value: false
+        panda_link2_sc:
+          Value: false
+        panda_link3:
+          Value: false
+        panda_link3_sc:
+          Value: false
+        panda_link4:
+          Value: false
+        panda_link4_sc:
+          Value: false
+        panda_link5:
+          Value: false
+        panda_link5_sc:
+          Value: false
+        panda_link6:
+          Value: false
+        panda_link6_sc:
+          Value: false
+        panda_link7:
+          Value: false
+        panda_link7_sc:
+          Value: false
+        panda_link8:
+          Value: true
+        panda_rightfinger:
+          Value: false
+        task:
+          Value: false
+        world:
+          Value: false
+        wrist_camera_bottom_screw_frame:
+          Value: false
+        wrist_camera_color_frame:
+          Value: false
+        wrist_camera_color_optical_frame:
+          Value: false
+        wrist_camera_depth_frame:
+          Value: false
+        wrist_camera_depth_optical_frame:
+          Value: false
+        wrist_camera_infra1_frame:
+          Value: false
+        wrist_camera_infra1_optical_frame:
+          Value: false
+        wrist_camera_infra2_frame:
+          Value: false
+        wrist_camera_infra2_optical_frame:
+          Value: false
+        wrist_camera_link:
+          Value: false
       Marker Alpha: 1
       Marker Scale: 0.30000001192092896
       Name: TF
       Show Arrows: false
       Show Axes: true
       Show Names: false
       Tree:
-        {}
+        world:
+          fixed_camera_link:
+            {}
+          panda_link0:
+            panda_link0_sc:
+              {}
+            panda_link1:
+              panda_link1_sc:
+                {}
+              panda_link2:
+                panda_link2_sc:
+                  {}
+                panda_link3:
+                  panda_link3_sc:
+                    {}
+                  panda_link4:
+                    panda_link4_sc:
+                      {}
+                    panda_link5:
+                      panda_link5_sc:
+                        {}
+                      panda_link6:
+                        panda_link6_sc:
+                          {}
+                        panda_link7:
+                          panda_link7_sc:
+                            {}
+                          panda_link8:
+                            panda_NE:
+                              panda_EE:
+                                panda_K:
+                                  {}
+                            panda_default_ee:
+                              {}
+                            panda_hand:
+                              panda_hand_sc:
+                                {}
+                              panda_hand_tcp:
+                                {}
+                              panda_leftfinger:
+                                {}
+                              panda_rightfinger:
+                                {}
+                            wrist_camera_bottom_screw_frame:
+                              wrist_camera_link:
+                                wrist_camera_color_frame:
+                                  wrist_camera_color_optical_frame:
+                                    {}
+                                wrist_camera_depth_frame:
+                                  wrist_camera_depth_optical_frame:
+                                    {}
+                                wrist_camera_infra1_frame:
+                                  wrist_camera_infra1_optical_frame:
+                                    {}
+                                wrist_camera_infra2_frame:
+                                  wrist_camera_infra2_optical_frame:
+                                    {}
+            task:
+              {}
       Update Interval: 0
-      Value: false
+      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -345,7 +480,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: AxisColor
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -360,7 +495,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
@@ -447,6 +582,22 @@ Visualization Manager:
         roi: true
       Queue Size: 100
       Value: true
+    - Alpha: 1
+      Axes Length: 0.05000000074505806
+      Axes Radius: 0.009999999776482582
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic: /target
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -475,7 +626,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.401139497756958
+      Distance: 1.2434059381484985
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -491,9 +642,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7197965979576111
+      Pitch: -0.06520330905914307
       Target Frame: <Fixed Frame>
-      Yaw: 3.8698341846466064
+      Yaw: 5.464844703674316
     Saved: ~
 Window Geometry:
   Displays:

moma_demos/grasp_demo/config/grasp_demo.yaml

@@ -1,8 +1,9 @@
 base_frame_id: panda_link0
-ee_grasp_offset_z: -0.04 #-0.04
+ee_grasp_offset_z: -0.04 #-0.02 #-0.04 # More negative is LOWER into the object
+# ee_grasp_offset_x: 0.010
 
 task_frame_id: task
-table_height: 0.14  # Measured from the base frame specified above
+table_height: 0.20 #0.25 #-0.05 #0.14  # Measured from the base frame specified above
 
 workspaces:
 - scan_joints: 

moma_demos/grasp_demo/launch/grasp_demo.launch

@@ -8,27 +8,33 @@
     <!-- ======== Launch everything ======== -->
 
     <!-- Parameters -->
-    <rosparam command="load" file="$(find grasp_demo)/config/grasp_demo.yaml" ns="manipulator/moma_demo" subst_value="true"/>
+    <rosparam command="load" file="$(find grasp_demo)/config/grasp_demo.yaml" ns="moma_demo" subst_value="true"/>
 
     <!-- Panda -->
-    <include file="$(find moma_bringup)/launch/panda_moveit_real_camera.launch">
+    <!-- <include file="$(find moma_bringup)/launch/panda_moveit_real_camera.launch">
         <arg name="simulation_mode" value="$(arg simulation_mode)"/>
+    </include> -->
+    <include file="$(find moma_bringup)/launch/panda_real.launch">
+        <arg name="rviz" value="false"/>
     </include>
+    <include file="$(find moma_bringup)/launch/components/sensors.launch">
+    </include>
+
 
     <!-- VGN nodes -->
     <rosparam command="load" file="$(find grasp_demo)/config/vgn_nodes.yaml" subst_value="true" />
     <node unless="$(arg semantic)" pkg="vgn" type="tsdf_server.py" name="tsdf_server" />
 
     <!-- Action nodes -->
-    <node pkg="grasp_demo" type="reset.py" name="reset" ns="manipulator" output="screen"/>
-    <node pkg="grasp_demo" type="reconstruct_scene.py" name="scan_action_node" args="$(arg semantic)" ns="manipulator" output="screen" />
-    <node pkg="grasp_demo" type="plan_grasp.py" name="plan_grasp" ns="manipulator" output="screen"/>
-    <node pkg="grasp_demo" type="execute_grasp.py" name="execute_grasp" ns="manipulator" output="screen"/>
-    <node pkg="grasp_demo" type="drop_object.py" name="drop_object" ns="manipulator" output="screen"/>
+    <node pkg="grasp_demo" type="reset.py" name="reset" output="screen"/>
+    <node pkg="grasp_demo" type="reconstruct_scene.py" name="scan_action_node" args="$(arg semantic)" output="screen" />
+    <node pkg="grasp_demo" type="plan_grasp.py" name="plan_grasp" output="screen"/>
+    <node pkg="grasp_demo" type="execute_grasp.py" name="execute_grasp" output="screen"/>
+    <node pkg="grasp_demo" type="drop_object.py" name="drop_object" output="screen"/>
 
     <!-- Launch demo script-->
-    <node pkg="grasp_demo" type="run_plan.py" name="grasp_demo" ns="manipulator" output="screen"/>
+    <node pkg="grasp_demo" type="run_plan.py" name="grasp_demo" output="screen"/>
 
     <!-- Open rviz if desired -->
-    <node if="$(arg launch_rviz)" type="rviz" name="rviz" pkg="rviz" args="-d $(find grasp_demo)/config/grasp_demo.rviz" ns="manipulator"/>
+    <node if="$(arg launch_rviz)" type="rviz" name="rviz" pkg="rviz" args="-d $(find grasp_demo)/config/grasp_demo.rviz"/>
 </launch>