update urdf

myArm/myarm_moveit/.setup_assistant

@@ -8,4 +8,4 @@ moveit_setup_assistant_config:
   CONFIG:
     author_name: zachary
     author_email: [email protected]
-    generated_timestamp: 1686548761
+    generated_timestamp: 1687147323

myArm/myarm_moveit/config/kinematics.yaml

@@ -1,4 +1,7 @@
 arm_group:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
+  kinematics_solver_timeout: 0.005
+  goal_joint_tolerance: 0.0001
+  goal_position_tolerance: 0.0001
+  goal_orientation_tolerance: 0.001

myArm/myarm_moveit/config/ompl_planning.yaml

@@ -127,6 +127,43 @@ planner_configs:
     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
+  AITstar:
+    type: geometric::AITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
+    set_max_num_goals: 1  # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1
+  ABITstar:
+    type: geometric::ABITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
+    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0
+    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
+    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
+    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
+    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
+    initial_inflation_factor: 1000000  # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000
+    inflation_scaling_parameter: 10  # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
+    truncation_scaling_parameter: 5.0  # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
+  BITstar:
+    type: geometric::BITstar
+    use_k_nearest: 1  # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
+    rewire_factor: 1.001  # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
+    samples_per_batch: 100  # batch size. Valid values: [1:1:1000]. Default: 100
+    use_graph_pruning: 1  # enable graph pruning (1) or not (0). Default: 1
+    prune_threshold_as_fractional_cost_change: 0.1  # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
+    delay_rewiring_to_first_solution: 0  # delay (1) or not (0) rewiring until a solution is found. Default: 0
+    use_just_in_time_sampling: 0  # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
+    drop_unconnected_samples_on_prune: 0  # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
+    stop_on_each_solution_improvement: 0  # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
+    use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
+    find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
 arm_group:
   default_planner_config: RRTConnect
   planner_configs:
@@ -154,5 +191,8 @@ arm_group:
     - LazyPRMstar
     - SPARS
     - SPARStwo
+    - AITstar
+    - ABITstar
+    - BITstar
   projection_evaluator: joints(joint1_to_base,joint2_to_joint1)
   longest_valid_segment_fraction: 0.005

myArm/myarm_moveit/config/sensors_3d.yaml

@@ -1,9 +1,2 @@
 sensors:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /head_mount_kinect/depth_registered/points
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+  []

myArm/myarm_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 
 import rospy, roslib, sys
@@ -32,7 +32,7 @@ def __init__(self):
 
         # Set the reference coordinate system used for the target position
         # 设置目标位置所使用的参考坐标系
-        self.reference_frame = "joint1"
+        self.reference_frame = "base"
         self.arm.set_pose_reference_frame(self.reference_frame)
 
         # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划
@@ -72,10 +72,9 @@ def moving(self):
         # Set the target pose of the terminal motion of the robotic arm
         # 设置机械臂终端运动的目标位姿
         self.arm.set_pose_target(target_pose, self.end_effector_link)
-
         # Plan the movement path,规划运动路径
         traj = self.arm.plan()
-
+        print('traj:', traj)
         # Control the motion of the robotic arm according to the planned motion path
         # 按照规划的运动路径控制机械臂运动
         self.arm.execute(traj)

mycobot_description/urdf/myarm/myarm_urdf.urdf

@@ -30,7 +30,7 @@
      <geometry>
        <mesh filename="package://mycobot_description/urdf/myarm/j1.dae"/>
       </geometry>
-      <origin xyz = "0.0 0 -0.06096 " rpy = " 0 0 0"/>
+      <origin xyz = "0.0 0 -0.085 " rpy = " 0 0 0"/>
     </collision>
   </link>
 
@@ -59,10 +59,9 @@
     </visual>
     <collision>
       <geometry>
-
        <mesh filename="package://mycobot_description/urdf/myarm/j3.dae"/>
       </geometry>
-      <origin xyz = "0.0 0 0.054" rpy = " 0 0 3.14159"/>
+      <origin xyz = "0.0 0 -0.054" rpy = " 0 0 3.14159"/>
     </collision>
   </link>
 
@@ -78,7 +77,7 @@
        <geometry>
        <mesh filename="package://mycobot_description/urdf/myarm/j4.dae"/>
       </geometry>
-      <origin xyz = "0.0 0 -0.03356 " rpy = " 0 0 -1.5708"/>
+      <origin xyz = "0.0 0 0 " rpy = " 0 0 -1.5708"/>
     </collision>
   </link>
 
@@ -96,7 +95,7 @@
 
        <mesh filename="package://mycobot_description/urdf/myarm/j5.dae"/>
       </geometry>
-    <origin xyz = "0 0.00 -0.038 " rpy = " 0 0 3.14159"/>
+    <origin xyz = "0 0.00 -0.051 " rpy = " 0 0 3.14159"/>
     </collision>
   </link>
 
@@ -131,15 +130,15 @@
 
        <mesh filename="package://mycobot_description/urdf/myarm/j7.dae"/>
       </geometry>
-    <origin xyz = "0.0 0 -0.0 " rpy = " 0 0 -1.5708"/>
+    <origin xyz = "0.0 0 -0.009 " rpy = " 0 0 -1.5708"/>
     </collision>
   </link>
 
 
 
   <joint name="joint1_to_base" type="revolute">
     <axis xyz="0 0 1"/>
-    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
     <parent link="base"/>
     <child link="joint1"/>
     <origin xyz= "0 0 0.165" rpy = "0 0 0"/>  
@@ -157,7 +156,7 @@
 
   <joint name="joint3_to_joint2" type="revolute">
     <axis xyz=" 0 0 1"/>
-    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
     <parent link="joint2"/>
     <child link="joint3"/>
     <origin xyz= "0 -0.11 0   " rpy = "1.5708 0 0"/>  
@@ -176,7 +175,7 @@
 
   <joint name="joint5_to_joint4" type="revolute">
     <axis xyz="0 0 1"/>
-    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
     <parent link="joint4"/>
     <child link="joint5"/>
     <origin xyz= "0 -0.126 0" rpy = "1.5708 0 0"/>  
@@ -193,7 +192,7 @@
 
   <joint name="joint7_to_joint6" type="revolute">
     <axis xyz="0 0 1"/>
-    <limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/>
+    <limit effort = "1000.0" lower = "-3.14" upper = "3.14" velocity = "0"/>
     <parent link="joint6"/>
     <child link="joint7"/>
     <origin xyz= "0 -0.056 0" rpy = "1.5708 0 0"/>