Fixed configs and added real ratios for J1 and J2

src/ar3_config/config/ar3_controllers.yaml

@@ -1,14 +1,16 @@
-controller_names:
+moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
-  - joint_trajectory_controller
+moveit_simple_controller_manager:
+  controller_names:
+    - joint_trajectory_controller
 
-joint_trajectory_controller:
+  joint_trajectory_controller:
-  action_ns: follow_joint_trajectory
+    action_ns: follow_joint_trajectory
-  type: FollowJointTrajectory
+    type: FollowJointTrajectory
-  default: true
+    default: true
-  joints:
+    joints:
-    - joint_1
+      - joint_1
-    - joint_2
+      - joint_2
-    - joint_3
+      - joint_3
-    - joint_4
+      - joint_4
-    - joint_5
+      - joint_5
-    - joint_6
+      - joint_6

src/ar3_config/config/ompl_planning.yaml

@@ -1,149 +1,155 @@
-planner_configs:
+move_group:
-  SBLkConfigDefault:
+  planning_plugin: ompl_interface/OMPLPlanner
-    type: geometric::SBL
+  request_adapters:
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints
-  ESTkConfigDefault:
+  start_state_max_bounds_error: 0.1
-    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 expresssion. default: 0.0 set in setup()
-  PRMkConfigDefault:
-    type: geometric::PRM
-    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
-  PRMstarkConfigDefault:
-    type: geometric::PRMstar
-  FMTkConfigDefault:
-    type: geometric::FMT
-    num_samples: 1000  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
-    nearest_k: 1  # use Knearest strategy. default: 1
-    cache_cc: 1  # use collision checking cache. default: 1
-    heuristics: 0  # activate cost to go heuristics. default: 0
-    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  BFMTkConfigDefault:
-    type: geometric::BFMT
-    num_samples: 1000  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
-    nearest_k: 1  # use the Knearest strategy. default: 1
-    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
-    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
-    heuristics: 1  # activates cost to go heuristics. default: 1
-    cache_cc: 1  # use the collision checking cache. default: 1
-    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
-  PDSTkConfigDefault:
-    type: geometric::PDST
-  STRIDEkConfigDefault:
-    type: geometric::STRIDE
-    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
-    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
-    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
-    max_degree: 18  # max degree of a node in the GNAT. default: 12
-    min_degree: 12  # min degree of a node in the GNAT. default: 12
-    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
-    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
-    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
-  BiTRRTkConfigDefault:
-    type: geometric::BiTRRT
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
-    init_temperature: 100  # initial temperature. default: 100
-    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
-    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
-    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
-  LBTRRTkConfigDefault:
-    type: geometric::LBTRRT
-    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
-    epsilon: 0.4  # optimality approximation factor. default: 0.4
-  BiESTkConfigDefault:
-    type: geometric::BiEST
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  ProjESTkConfigDefault:
-    type: geometric::ProjEST
-    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
-  LazyPRMkConfigDefault:
-    type: geometric::LazyPRM
-    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-  LazyPRMstarkConfigDefault:
-    type: geometric::LazyPRMstar
-  SPARSkConfigDefault:
-    type: geometric::SPARS
-    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-    max_failures: 1000  # maximum consecutive failure limit. default: 1000
-  SPARStwokConfigDefault:
-    type: geometric::SPARStwo
-    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
-    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
-    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
-    max_failures: 5000  # maximum consecutive failure limit. default: 5000
 
-ar3_arm:
-  default_planner_config: RRTConnectkConfigDefault
   planner_configs:
-    - SBLkConfigDefault
+    SBLkConfigDefault:
-    - ESTkConfigDefault
+      type: geometric::SBL
-    - LBKPIECEkConfigDefault
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    - BKPIECEkConfigDefault
+    ESTkConfigDefault:
-    - KPIECEkConfigDefault
+      type: geometric::EST
-    - RRTkConfigDefault
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
-    - RRTConnectkConfigDefault
+      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-    - RRTstarkConfigDefault
+    LBKPIECEkConfigDefault:
-    - TRRTkConfigDefault
+      type: geometric::LBKPIECE
-    - PRMkConfigDefault
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    - PRMstarkConfigDefault
+      border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
-    - FMTkConfigDefault
+      min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-    - BFMTkConfigDefault
+    BKPIECEkConfigDefault:
-    - PDSTkConfigDefault
+      type: geometric::BKPIECE
-    - STRIDEkConfigDefault
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    - BiTRRTkConfigDefault
+      border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
-    - LBTRRTkConfigDefault
+      failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
-    - BiESTkConfigDefault
+      min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
-    - ProjESTkConfigDefault
+    KPIECEkConfigDefault:
-    - LazyPRMkConfigDefault
+      type: geometric::KPIECE
-    - LazyPRMstarkConfigDefault
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
-    - SPARSkConfigDefault
+      goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
-    - SPARStwokConfigDefault
+      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 expresssion. default: 0.0 set in setup()
+    PRMkConfigDefault:
+      type: geometric::PRM
+      max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+    PRMstarkConfigDefault:
+      type: geometric::PRMstar
+    FMTkConfigDefault:
+      type: geometric::FMT
+      num_samples: 1000  # number of states that the planner should sample. default: 1000
+      radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+      nearest_k: 1  # use Knearest strategy. default: 1
+      cache_cc: 1  # use collision checking cache. default: 1
+      heuristics: 0  # activate cost to go heuristics. default: 0
+      extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+    BFMTkConfigDefault:
+      type: geometric::BFMT
+      num_samples: 1000  # number of states that the planner should sample. default: 1000
+      radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+      nearest_k: 1  # use the Knearest strategy. default: 1
+      balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+      optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
+      heuristics: 1  # activates cost to go heuristics. default: 1
+      cache_cc: 1  # use the collision checking cache. default: 1
+      extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+    PDSTkConfigDefault:
+      type: geometric::PDST
+    STRIDEkConfigDefault:
+      type: geometric::STRIDE
+      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
+      use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
+      degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+      max_degree: 18  # max degree of a node in the GNAT. default: 12
+      min_degree: 12  # min degree of a node in the GNAT. default: 12
+      max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+      estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+      min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+    BiTRRTkConfigDefault:
+      type: geometric::BiTRRT
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+      temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+      init_temperature: 100  # initial temperature. default: 100
+      frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+      frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+      cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+    LBTRRTkConfigDefault:
+      type: geometric::LBTRRT
+      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
+      epsilon: 0.4  # optimality approximation factor. default: 0.4
+    BiESTkConfigDefault:
+      type: geometric::BiEST
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    ProjESTkConfigDefault:
+      type: geometric::ProjEST
+      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
+    LazyPRMkConfigDefault:
+      type: geometric::LazyPRM
+      range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    LazyPRMstarkConfigDefault:
+      type: geometric::LazyPRMstar
+    SPARSkConfigDefault:
+      type: geometric::SPARS
+      stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+      sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+      dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+      max_failures: 1000  # maximum consecutive failure limit. default: 1000
+    SPARStwokConfigDefault:
+      type: geometric::SPARStwo
+      stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+      sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+      dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+      max_failures: 5000  # maximum consecutive failure limit. default: 5000
+
+  ar3_arm:
+    default_planner_config: RRTConnectkConfigDefault
+    planner_configs:
+      - SBLkConfigDefault
+      - ESTkConfigDefault
+      - LBKPIECEkConfigDefault
+      - BKPIECEkConfigDefault
+      - KPIECEkConfigDefault
+      - RRTkConfigDefault
+      - RRTConnectkConfigDefault
+      - RRTstarkConfigDefault
+      - TRRTkConfigDefault
+      - PRMkConfigDefault
+      - PRMstarkConfigDefault
+      - FMTkConfigDefault
+      - BFMTkConfigDefault
+      - PDSTkConfigDefault
+      - STRIDEkConfigDefault
+      - BiTRRTkConfigDefault
+      - LBTRRTkConfigDefault
+      - BiESTkConfigDefault
+      - ProjESTkConfigDefault
+      - LazyPRMkConfigDefault
+      - LazyPRMstarkConfigDefault
+      - SPARSkConfigDefault
+      - SPARStwokConfigDefault

src/ar3_config/launch/dev.launch.py

@@ -72,8 +72,10 @@ def generate_launch_description():
                 robot_description,
                 robot_description_semantic,
                 robot_description_kinematics,
-                {'move_group': join(config_root, 'ompl_planning.yaml')},
+                get_yaml(join(config_root, 'ompl_planning.yaml')),
-                {'moveit_simple_controller_manager': join(config_root, "ar3_controllers.yaml")},
+                get_yaml(join(config_root, 'trajectory.yaml')),
+                get_yaml(join(config_root, 'planning_scene.yaml')),
+                get_yaml(join(config_root, "ar3_controllers.yaml")),
             ],
         ),
 

src/cobot_platform/cobot_platform/move.py

@@ -46,11 +46,11 @@ class CobotMove(Node):
         print(self._serial.readline())
 
     def _move_j1_cb(self, msg):
-        self._cur[0] = int(msg.data / pi / 2. * 2000)
+        self._cur[0] = int(msg.data / pi / 2. * 8000)
         self._move_servo()
 
     def _move_j2_cb(self, msg):
-        self._cur[1] = int(msg.data / pi / 2. * 3840.64)
+        self._cur[1] = int(msg.data / pi / 2. * 10000)
         self._move_servo()
 
     def _move_j3_cb(self, msg):