Merge pull request #105 from AGH-CEAI/feature/servo_calibration

`aegis_moveit_config`: MoveIt Servo calibration & SRDF fixes

Author: macmacal

aegis_moveit_config/CHANGELOG.md

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* [PR-105](https://github.com/AGH-CEAI/aegis_ros/pull/105) - Calibrated servo configuration for Sim2Real experiments.
 * [PR-77](https://github.com/AGH-CEAI/aegis_ros/pull/77) - Changed `ur_base` frame to the `world` frame (simulation simplification).
 * [PR-75](https://github.com/AGH-CEAI/aegis_ros/pull/75) - Updated servo max linear, angular and joints velocities.
 * [PR-71](https://github.com/AGH-CEAI/aegis_ros/pull/71) - Updated collision checking.

aegis_moveit_config/config/aegis.srdf

@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="aegis_arm">
-         <chain base_link="base_link" tip_link="tool0" />
+         <chain base_link="base_link" tip_link="robotiq_hande_end" />
     </group>
     <group name="aegis_manipulator">
         <link name="adapter_to_sensor"/>
@@ -45,8 +45,19 @@
     <group_state name="close" group="aegis_manipulator">
         <joint name="robotiq_hande_left_finger_joint" value="0"/>
     </group_state>
+    <group_state name="home" group="aegis">
+        <joint name="shoulder_pan_joint" value="0"/>
+        <joint name="shoulder_lift_joint" value="-2.094395"/>
+        <joint name="elbow_joint" value="2.094395"/>
+        <joint name="wrist_1_joint" value="-1.570796"/>
+        <joint name="wrist_2_joint" value="-1.570796"/>
+        <joint name="wrist_3_joint" value="0"/>
+        <joint name="robotiq_hande_left_finger_joint" value="0.025"/>
+    </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="aegis_manipulator" parent_link="tool0" group="aegis_manipulator" parent_group="aegis_arm"/>
+    <!-- <end_effector name="aegis_manipulator" parent_link="tool0" group="aegis_manipulator" parent_group="aegis_arm"/> -->
+    <!-- <end_effector name="robotiq_hand" parent_link="robotiq_hande_end" group="aegis_manipulator" parent_group="aegis"/> -->
+    <end_effector name="aegis_manipulator" parent_link="robotiq_hande_end" group="aegis_manipulator" parent_group="aegis_arm"/>
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->

aegis_moveit_config/config/move_group/ompl_planning.yaml

@@ -1,10 +1,9 @@
 move_group:
   planning_plugin: ompl_interface/OMPLPlanner
-  planner_configs:
-    RRTConnectkConfigDefault:
-      type: geometric::RRTConnect
-      range: 0.0
+  projection_evaluator: jodints(shoulder_pan_joint,shoulder_lift_joint)
+  longest_valid_segment_fraction: 0.005
   start_state_max_bounds_error: 0.1
+  enforce_joint_model_state_space: true
 
   # Request adapters are described in the MoveIt2 docs:
   # https://moveit.picknik.ai/humble/doc/concepts/motion_planning.html#motion-planning-adapters
@@ -14,3 +13,61 @@ move_group:
     default_planner_request_adapters/FixStartStateBounds
     default_planner_request_adapters/FixStartStateCollision
     default_planner_request_adapters/FixStartStatePathConstraints
+
+  response_adapters: >-
+    default_planner_request_adapters/ValidateSolution
+
+  planner_configs:
+    SBLkConfigDefault:
+      type: geometric::SBL
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    ESTkConfigDefault:
+      type: geometric::EST
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+      goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
+    LBKPIECEkConfigDefault:
+      type: geometric::LBKPIECE
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
+      min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+    BKPIECEkConfigDefault:
+      type: geometric::BKPIECE
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9
+      failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
+      min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+    KPIECEkConfigDefault:
+      type: geometric::KPIECE
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05
+      border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+      failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5
+      min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5
+    RRTkConfigDefault:
+      type: geometric::RRT
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
+    RRTConnectkConfigDefault:
+      type: geometric::RRTConnect
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    RRTstarkConfigDefault:
+      type: geometric::RRTstar
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
+      delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1
+    TRRTkConfigDefault:
+      type: geometric::TRRT
+      range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05
+      max_states_failed: 10 # when to start increasing temp. default: 10
+      temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0
+      min_temperature: 10e-10 # lower limit of temp change. default: 10e-10
+      init_temperature: 10e-6 # initial temperature. default: 10e-6
+      frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+      frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+      k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup()
+    PRMkConfigDefault:
+      type: geometric::PRM
+      max_nearest_neighbors: 10 # use k nearest neighbors. default: 10
+    PRMstarkConfigDefault:
+      type: geometric::PRMstar

aegis_moveit_config/config/move_group/ur_servo.yaml

@@ -8,14 +8,32 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm
 command_in_type: "unitless"
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear: 0.5 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational: 0.35 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  # linear: 0.5 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  # rotational: 0.35 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
-  joint: 0.35 # ~ 20 deg/s
+  # joint: 0.35 # ~ 20 deg/s
+
+  # For publlisnihg 25 msgs with 25Hz (for "Robot Limit 300W")
+  # target: 2.0 m/s measured: 0.113m/s for publish_period 1/250
+  # target: 1.0 m/s measured: 0.102 m/s for publish_period 1/250
+  # target: 3.1416 rad/s measured: 0,302 rad/s for publish_period 1/250
+  # target: 0.6 m/s measured: 0.054m/s for publish_period 1/250
+  # target: 1.0 m/s measured: 0.052m/s for publish_period 1/500
+  # target: 2.5 m/s measured: 0.068 m/s for publish_period 1/500
+  # target: 5.0 m/s measured: 0.071 m/s for publish_period 1/500
+  # For publlisnihg 25 msgs with 25Hz (for "Robot Limit 1000W")
+  # target: 0.6 m/s measured: 0.048m/s for publish_period 1/250
+  # target: 0.6 m/s measured: 0.032m/s for publish_period 1/500
+  # target: 1.0 m/s measured: 0,098m/s for publish_period 1/250
+  # target: 1.0 m/s measured: 0,053m/s for publish_period 1/500
+  linear: 1.0
+  rotational: 3.1416
+  joint: 3.1416
+
 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
 # controller performance. It essentially increases the timestep when calculating the target pose, to move the target
 # pose farther away. [seconds]
-system_latency_compensation: 0.04
+system_latency_compensation: 0.0
 
 ## Properties of outgoing commands
 publish_period: 0.004 # 1/Nominal publish rate [seconds]
@@ -68,6 +86,7 @@ joint_topic: /joint_states
 status_topic: ~/status # Publish status to this topic
 # command_out_topic: /scaled_joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
 command_out_topic: /forward_position_controller/commands
+# command_out_topic: /forward_velocity_controller/commands
 # command_out_topic: /ur_controller/joint_trajectory # Publish outgoing commands here
 monitored_planning_scene_topic: /monitored_planning_scene
 
@@ -84,4 +103,4 @@ self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collis
 scene_collision_proximity_threshold: 0.01 # Start decelerating when a scene collision is this far [m]
 # Parameters for "stop_distance"-type collision checking
 collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
-min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
+min_allowable_collision_distance: 0.007 # Stop if a collision is closer than this [m]