Update Octomap to work with servo for bite transfer (#33)

* Update octomap to work with servo for bite transfer

* Increase singularity threshold to account for head pose far to the left of nominal

* Further increased singularity thresholds

Author: amalnanavati

ada_moveit/config/mock_servo.yaml

@@ -40,6 +40,7 @@ servo_node:
       # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
       # then is_primary_planning_scene_monitor needs to be set to false.
       is_primary_planning_scene_monitor: false
+      monitored_planning_scene_topic: "monitored_planning_scene"
 
       ## MoveIt properties
       move_group_name:  jaco_arm  # Often 'manipulator' or 'arm'
@@ -56,8 +57,8 @@ servo_node:
       num_outgoing_halt_msgs_to_publish: 0
 
       ## Configure handling of singularities and joint limits
-      lower_singularity_threshold:  30.0  # Start decelerating when the condition number hits this (close to singularity)
-      hard_stop_singularity_threshold: 90.0 # Stop when the condition number hits this
+      lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
+      hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
       joint_limit_margin: 0.2 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
       leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
 

ada_moveit/config/real_servo.yaml

@@ -40,6 +40,7 @@ servo_node:
       # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
       # then is_primary_planning_scene_monitor needs to be set to false.
       is_primary_planning_scene_monitor: false
+      monitored_planning_scene_topic: "monitored_planning_scene"
 
       ## MoveIt properties
       move_group_name:  jaco_arm  # Often 'manipulator' or 'arm'
@@ -56,8 +57,8 @@ servo_node:
       num_outgoing_halt_msgs_to_publish: 0
 
       ## Configure handling of singularities and joint limits
-      lower_singularity_threshold:  30.0  # Start decelerating when the condition number hits this (close to singularity)
-      hard_stop_singularity_threshold: 90.0 # Stop when the condition number hits this
+      lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
+      hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
       joint_limit_margin: 0.2 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
       leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
 
@@ -72,4 +73,4 @@ servo_node:
       check_collisions: true # Check collisions?
       collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
       self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
-      scene_collision_proximity_threshold: 0.0001 # Start decelerating when a scene collision is this far [m]
+      scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m]

ada_moveit/config/sensors_3d.yaml

@@ -9,8 +9,8 @@ default_sensor:
   near_clipping_plane_distance: 0.02
   far_clipping_plane_distance: 5.0
   shadow_threshold: 0.2
-  padding_scale: 4.0
-  padding_offset: 0.03
+  padding_scale: 1.0
+  padding_offset: 0.00
   max_update_rate: 3.0
   filtered_cloud_topic: filtered_cloud