diff --git a/mycobot_description/urdf/mycobot_pro_630/mycobot_pro_630_moveit.urdf b/mycobot_description/urdf/mycobot_pro_630/mycobot_pro_630_moveit.urdf
new file mode 100755
index 0000000..3b109d6
--- /dev/null
+++ b/mycobot_description/urdf/mycobot_pro_630/mycobot_pro_630_moveit.urdf
@@ -0,0 +1,175 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/.setup_assistant b/mycobot_pro/mycobot_630_moveit/.setup_assistant
index baebb43..660b509 100644
--- a/mycobot_pro/mycobot_630_moveit/.setup_assistant
+++ b/mycobot_pro/mycobot_630_moveit/.setup_assistant
@@ -1,11 +1,11 @@
moveit_setup_assistant_config:
URDF:
package: mycobot_description
- relative_path: urdf/mycobot_pro_630/mycobot_pro_630.urdf
+ relative_path: urdf/mycobot_pro_630/mycobot_pro_630_moveit.urdf
xacro_args: "--inorder "
SRDF:
relative_path: config/firefighter.srdf
CONFIG:
author_name: wangweijian
author_email: weijian.wang@elephantrobotics.com
- generated_timestamp: 1652406964
\ No newline at end of file
+ generated_timestamp: 1736239899
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/CMakeLists.txt b/mycobot_pro/mycobot_630_moveit/CMakeLists.txt
index c6b79de..0f9c3c0 100644
--- a/mycobot_pro/mycobot_630_moveit/CMakeLists.txt
+++ b/mycobot_pro/mycobot_630_moveit/CMakeLists.txt
@@ -1,21 +1,10 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.1.3)
project(mycobot_630_moveit)
-find_package(catkin REQUIRED
- rospy
- std_msgs
- moveit_msgs
-)
+find_package(catkin REQUIRED)
catkin_package()
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-catkin_install_python(PROGRAMS
- scripts/sync_plan.py
- DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/mycobot_pro/mycobot_630_moveit/config/cartesian_limits.yaml b/mycobot_pro/mycobot_630_moveit/config/cartesian_limits.yaml
new file mode 100644
index 0000000..7df72f6
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+ max_trans_vel: 1
+ max_trans_acc: 2.25
+ max_trans_dec: -5
+ max_rot_vel: 1.57
diff --git a/mycobot_pro/mycobot_630_moveit/config/chomp_planning.yaml b/mycobot_pro/mycobot_630_moveit/config/chomp_planning.yaml
index 75258e5..eb9c912 100644
--- a/mycobot_pro/mycobot_630_moveit/config/chomp_planning.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/chomp_planning.yaml
@@ -7,12 +7,12 @@ learning_rate: 0.01
smoothness_cost_velocity: 0.0
smoothness_cost_acceleration: 1.0
smoothness_cost_jerk: 0.0
-ridge_factor: 0.01
+ridge_factor: 0.0
use_pseudo_inverse: false
pseudo_inverse_ridge_factor: 1e-4
joint_update_limit: 0.1
-collision_clearence: 0.2
+collision_clearance: 0.2
collision_threshold: 0.07
use_stochastic_descent: true
-enable_failure_recovery: true
-max_recovery_attempts: 5
\ No newline at end of file
+enable_failure_recovery: false
+max_recovery_attempts: 5
diff --git a/mycobot_pro/mycobot_630_moveit/config/fake_controllers.yaml b/mycobot_pro/mycobot_630_moveit/config/fake_controllers.yaml
index aa4aef9..3e6f342 100644
--- a/mycobot_pro/mycobot_630_moveit/config/fake_controllers.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/fake_controllers.yaml
@@ -1,9 +1,13 @@
controller_list:
- name: fake_arm_group_controller
+ type: $(arg fake_execution_type)
joints:
- joint1_to_base
- joint2_to_joint1
- joint3_to_joint2
- joint4_to_joint3
- joint5_to_joint4
- - joint6_to_joint5
\ No newline at end of file
+ - joint6_to_joint5
+initial: # Define initial robot poses per group
+ - group: arm_group
+ pose: init_pose
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/config/firefighter.srdf b/mycobot_pro/mycobot_630_moveit/config/firefighter.srdf
index 9609c58..27ae337 100644
--- a/mycobot_pro/mycobot_630_moveit/config/firefighter.srdf
+++ b/mycobot_pro/mycobot_630_moveit/config/firefighter.srdf
@@ -1,4 +1,4 @@
-
+
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
-
-
-
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/config/gazebo_controllers.yaml b/mycobot_pro/mycobot_630_moveit/config/gazebo_controllers.yaml
new file mode 100644
index 0000000..e4d2eb0
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/config/gazebo_controllers.yaml
@@ -0,0 +1,4 @@
+# Publish joint_states
+joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
diff --git a/mycobot_pro/mycobot_630_moveit/config/gazebo_firefighter.urdf b/mycobot_pro/mycobot_630_moveit/config/gazebo_firefighter.urdf
new file mode 100644
index 0000000..381af5c
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/config/gazebo_firefighter.urdf
@@ -0,0 +1,250 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+ transmission_interface/SimpleTransmission
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
+
+ /
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/config/joint_limits.yaml b/mycobot_pro/mycobot_630_moveit/config/joint_limits.yaml
index 8bc72c6..47328ee 100644
--- a/mycobot_pro/mycobot_630_moveit/config/joint_limits.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/joint_limits.yaml
@@ -1,34 +1,40 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 0.1
+default_acceleration_scaling_factor: 0.1
+
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
joint1_to_base:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
+ max_acceleration: 0
joint2_to_joint1:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
+ max_acceleration: 0
joint3_to_joint2:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
+ max_acceleration: 0
joint4_to_joint3:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
+ max_acceleration: 0
joint5_to_joint4:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
+ max_acceleration: 0
joint6_to_joint5:
- has_velocity_limits: true
- max_velocity: 3.14
+ has_velocity_limits: false
+ max_velocity: 0
has_acceleration_limits: false
- max_acceleration: 30
\ No newline at end of file
+ max_acceleration: 0
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/config/kinematics.yaml b/mycobot_pro/mycobot_630_moveit/config/kinematics.yaml
index 3c16bbc..2f06d6e 100644
--- a/mycobot_pro/mycobot_630_moveit/config/kinematics.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/kinematics.yaml
@@ -2,4 +2,6 @@ arm_group:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
+ goal_joint_tolerance: 0.0001
+ goal_position_tolerance: 0.0001
+ goal_orientation_tolerance: 0.001
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/config/ompl_planning.yaml b/mycobot_pro/mycobot_630_moveit/config/ompl_planning.yaml
index 82f7d9d..7f30514 100644
--- a/mycobot_pro/mycobot_630_moveit/config/ompl_planning.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/ompl_planning.yaml
@@ -1,4 +1,11 @@
planner_configs:
+ AnytimePathShortening:
+ type: geometric::AnytimePathShortening
+ shortcut: true # Attempt to shortcut all new solution paths
+ hybridize: true # Compute hybrid solution trajectories
+ max_hybrid_paths: 24 # Number of hybrid paths generated per iteration
+ num_planners: 4 # The number of default planners to use for planning
+ planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
SBL:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -44,8 +51,8 @@ planner_configs:
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
+ frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+ frontier_node_ratio: 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()
PRM:
type: geometric::PRM
@@ -88,8 +95,8 @@ planner_configs:
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
+ frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+ frontier_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
LBTRRT:
type: geometric::LBTRRT
@@ -120,9 +127,47 @@ 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:
+ - AnytimePathShortening
- SBL
- EST
- LBKPIECE
@@ -146,5 +191,8 @@ arm_group:
- LazyPRMstar
- SPARS
- SPARStwo
- projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2)
- longest_valid_segment_fraction: 0.005
\ No newline at end of file
+ - AITstar
+ - ABITstar
+ - BITstar
+ projection_evaluator: joints(joint1_to_base,joint2_to_joint1)
+ longest_valid_segment_fraction: 0.005
diff --git a/mycobot_pro/mycobot_630_moveit/config/ros_controllers.yaml b/mycobot_pro/mycobot_630_moveit/config/ros_controllers.yaml
index 491f450..efc7c6c 100644
--- a/mycobot_pro/mycobot_630_moveit/config/ros_controllers.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/ros_controllers.yaml
@@ -1,26 +1,40 @@
-# Simulation settings for using moveit_sim_controllers
-moveit_sim_hw_interface:
- joint_model_group: arm_group
- joint_model_group_pose: init_pose
-# Settings for ros_control_boilerplate control loop
-generic_hw_control_loop:
- loop_hz: 300
- cycle_time_error_threshold: 0.01
-# Settings for ros_control hardware interface
-hardware_interface:
+arm_group_controller:
+ type: effort_controllers/JointTrajectoryController
joints:
- - vitual_joint
- joint1_to_base
- joint2_to_joint1
- joint3_to_joint2
- joint4_to_joint3
- joint5_to_joint4
- joint6_to_joint5
- sim_control_mode: 1 # 0: position, 1: velocity
-# Publish all joint states
-# Creates the /joint_states topic necessary in ROS
-joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 50
-controller_list:
- []
\ No newline at end of file
+ gains:
+ joint1_to_base:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
+ joint2_to_joint1:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
+ joint3_to_joint2:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
+ joint4_to_joint3:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
+ joint5_to_joint4:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
+ joint6_to_joint5:
+ p: 100
+ d: 1
+ i: 1
+ i_clamp: 1
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/config/sensors_3d.yaml b/mycobot_pro/mycobot_630_moveit/config/sensors_3d.yaml
index a4bb13e..01538cc 100644
--- a/mycobot_pro/mycobot_630_moveit/config/sensors_3d.yaml
+++ b/mycobot_pro/mycobot_630_moveit/config/sensors_3d.yaml
@@ -1,4 +1,3 @@
-# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
- filtered_cloud_topic: filtered_cloud
max_range: 5.0
diff --git a/mycobot_pro/mycobot_630_moveit/config/simple_moveit_controllers.yaml b/mycobot_pro/mycobot_630_moveit/config/simple_moveit_controllers.yaml
new file mode 100644
index 0000000..68c9469
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/config/simple_moveit_controllers.yaml
@@ -0,0 +1,12 @@
+controller_list:
+ - name: arm_group_controller
+ action_ns: follow_joint_trajectory
+ type: FollowJointTrajectory
+ default: True
+ joints:
+ - joint1_to_base
+ - joint2_to_joint1
+ - joint3_to_joint2
+ - joint4_to_joint3
+ - joint5_to_joint4
+ - joint6_to_joint5
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/config/stomp_planning.yaml b/mycobot_pro/mycobot_630_moveit/config/stomp_planning.yaml
new file mode 100644
index 0000000..fa999a9
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/config/stomp_planning.yaml
@@ -0,0 +1,39 @@
+stomp/arm_group:
+ group_name: arm_group
+ optimization:
+ num_timesteps: 60
+ num_iterations: 40
+ num_iterations_after_valid: 0
+ num_rollouts: 30
+ max_rollouts: 30
+ initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+ control_cost_weight: 0.0
+ task:
+ noise_generator:
+ - class: stomp_moveit/NormalDistributionSampling
+ stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
+ cost_functions:
+ - class: stomp_moveit/CollisionCheck
+ collision_penalty: 1.0
+ cost_weight: 1.0
+ kernel_window_percentage: 0.2
+ longest_valid_joint_move: 0.05
+ noisy_filters:
+ - class: stomp_moveit/JointLimits
+ lock_start: True
+ lock_goal: True
+ - class: stomp_moveit/MultiTrajectoryVisualization
+ line_width: 0.02
+ rgb: [255, 255, 0]
+ marker_array_topic: stomp_trajectories
+ marker_namespace: noisy
+ update_filters:
+ - class: stomp_moveit/PolynomialSmoother
+ poly_order: 6
+ - class: stomp_moveit/TrajectoryVisualization
+ line_width: 0.05
+ rgb: [0, 191, 255]
+ error_rgb: [255, 0, 0]
+ publish_intermediate: True
+ marker_topic: stomp_trajectory
+ marker_namespace: optimized
\ No newline at end of file
diff --git a/mycobot_pro/mycobot_630_moveit/launch/chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/chomp_planning_pipeline.launch.xml
index d660d07..3748356 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/chomp_planning_pipeline.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/chomp_planning_pipeline.launch.xml
@@ -1,10 +1,21 @@
-
-
-
+
+
+
+
-
-
+
+
+
+
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/default_warehouse_db.launch b/mycobot_pro/mycobot_630_moveit/launch/default_warehouse_db.launch
index e2fc2f8..c85e7fe 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/default_warehouse_db.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/default_warehouse_db.launch
@@ -5,7 +5,7 @@
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/demo.launch b/mycobot_pro/mycobot_630_moveit/launch/demo.launch
index c47c540..87652b8 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/demo.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/demo.launch
@@ -1,65 +1,66 @@
+
+
+
-
-
-
-
+
- MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
- The latter one maintains and publishes the current joint configuration of the simulated robot.
- It also provides a GUI to move the simulated robot around "manually".
- This corresponds to moving around the real robot without the use of MoveIt.
- -->
+
+
+
+
+
+
-
-
-
-
-
-
+
-
+
-
-
-
-
- [move_group/fake_controller_joint_states]
-
+
+
+
+ [move_group/fake_controller_joint_states]
+
+
+
+ [move_group/fake_controller_joint_states]
+
-
-
-
+
+
+
-
-
-
+
+
-
+
+
+
+
-
-
-
+
+
-
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/demo_gazebo.launch b/mycobot_pro/mycobot_630_moveit/launch/demo_gazebo.launch
index 3ee6039..0ef8f95 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/demo_gazebo.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/demo_gazebo.launch
@@ -1,70 +1,21 @@
+
+
+
-
-
-
-
+
+
+
+
+
-
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
- [/joint_states]
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml
index 06e4891..dc584b0 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml
@@ -1,9 +1,12 @@
+
+
+
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/gazebo.launch b/mycobot_pro/mycobot_630_moveit/launch/gazebo.launch
index 444a8d3..e145891 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/gazebo.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/gazebo.launch
@@ -1,23 +1,34 @@
-
-
-
+
+
+
+
+
+
-
-
-
-
-
+
+
+
-
-
+
+
-
-
+
+
+
+
-
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/move_group.launch b/mycobot_pro/mycobot_630_moveit/launch/move_group.launch
index 9f58ce1..2330bcb 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/move_group.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/move_group.launch
@@ -1,12 +1,10 @@
-
-
+ value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
@@ -14,10 +12,11 @@
+
-
+
+
-
@@ -38,20 +37,46 @@
" />
-->
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
-
+
+
-
+
@@ -61,11 +86,14 @@
-
-
-
-
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz b/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz
index 61506b6..e4b03b9 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz
+++ b/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz
@@ -211,7 +211,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/XYOrbit
- Distance: 1.3915867805480957
+ Distance: 1.955079197883606
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -227,9 +227,9 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.3547966480255127
+ Pitch: 0.2597966492176056
Target Frame: base
- Yaw: 2.0267655849456787
+ Yaw: 2.8617656230926514
Saved: ~
Window Geometry:
Displays:
@@ -243,7 +243,7 @@ Window Geometry:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d00000156000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000199000000420000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001e10000018c0000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d00000156000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000199000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001e00000018d0000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1848
diff --git a/mycobot_pro/mycobot_630_moveit/launch/moveit_rviz.launch b/mycobot_pro/mycobot_630_moveit/launch/moveit_rviz.launch
index a375679..a4605c0 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/moveit_rviz.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/moveit_rviz.launch
@@ -4,13 +4,12 @@
-
-
-
+
+
+
-
+ args="$(arg command_args)" output="screen">
diff --git a/mycobot_pro/mycobot_630_moveit/launch/mycobot630_moveit.launch b/mycobot_pro/mycobot_630_moveit/launch/mycobot630_moveit.launch
index 0a9be57..87652b8 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/mycobot630_moveit.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/mycobot630_moveit.launch
@@ -1,64 +1,66 @@
+
+
+
-
-
-
+
-
+
- MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
- The latter one maintains and publishes the current joint configuration of the simulated robot.
- It also provides a GUI to move the simulated robot around "manually".
- This corresponds to moving around the real robot without the use of MoveIt.
- -->
+
+
+
+
+
+
-
-
-
-
-
-
+
-
+
-
-
-
-
- [move_group/fake_controller_joint_states]
-
+
+
+
+ [move_group/fake_controller_joint_states]
+
+
+
+ [move_group/fake_controller_joint_states]
+
-
-
-
+
+
+
-
-
-
+
+
-
+
+
+
+
-
-
-
+
+
-
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/ompl-chomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..8ff1cd7
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/ompl_planning_pipeline.launch.xml
index ccbe152..79f6f44 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/ompl_planning_pipeline.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/ompl_planning_pipeline.launch.xml
@@ -1,21 +1,23 @@
-
-
+
+
-
-
+
+
-
-
-
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
new file mode 100644
index 0000000..c7c4cf5
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch b/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch
index 36ba3d9..4f54be0 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch
@@ -6,7 +6,7 @@
-
+
@@ -14,11 +14,13 @@
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/planning_pipeline.launch.xml
index 6640f40..4b4d0d6 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/planning_pipeline.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/planning_pipeline.launch.xml
@@ -5,6 +5,6 @@
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/ros_control_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..9ebc91c
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/launch/ros_control_moveit_controller_manager.launch.xml
@@ -0,0 +1,4 @@
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/ros_controllers.launch b/mycobot_pro/mycobot_630_moveit/launch/ros_controllers.launch
index 939670e..faece43 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/ros_controllers.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/ros_controllers.launch
@@ -6,6 +6,6 @@
+ output="screen" args="arm_group_controller "/>
diff --git a/mycobot_pro/mycobot_630_moveit/launch/run_benchmark_ompl.launch b/mycobot_pro/mycobot_630_moveit/launch/run_benchmark_ompl.launch
index bf9c8b6..3fb79fa 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/run_benchmark_ompl.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/run_benchmark_ompl.launch
@@ -4,18 +4,17 @@
-
+
-
+
-
diff --git a/mycobot_pro/mycobot_630_moveit/launch/sensor_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/sensor_manager.launch.xml
index 66a8fb4..d89d281 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/sensor_manager.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/sensor_manager.launch.xml
@@ -12,6 +12,6 @@
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/setup_assistant.launch b/mycobot_pro/mycobot_630_moveit/launch/setup_assistant.launch
index bba5569..a37d34a 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/setup_assistant.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/setup_assistant.launch
@@ -1,4 +1,4 @@
-
+
@@ -8,8 +8,9 @@
diff --git a/mycobot_pro/mycobot_630_moveit/launch/simple_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/simple_moveit_controller_manager.launch.xml
new file mode 100644
index 0000000..9ad259c
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/launch/simple_moveit_controller_manager.launch.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/stomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..b70333c
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/launch/stomp_planning_pipeline.launch.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/trajectory_execution.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/trajectory_execution.launch.xml
index 20e6e19..20c3dfc 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/trajectory_execution.launch.xml
+++ b/mycobot_pro/mycobot_630_moveit/launch/trajectory_execution.launch.xml
@@ -1,8 +1,11 @@
+
-
+
+
+
-
+
@@ -13,8 +16,8 @@
-
-
-
+
+
diff --git a/mycobot_pro/mycobot_630_moveit/launch/warehouse.launch b/mycobot_pro/mycobot_630_moveit/launch/warehouse.launch
index 1496e6f..0712e67 100644
--- a/mycobot_pro/mycobot_630_moveit/launch/warehouse.launch
+++ b/mycobot_pro/mycobot_630_moveit/launch/warehouse.launch
@@ -4,7 +4,7 @@
-
+
diff --git a/mycobot_pro/mycobot_630_moveit/package.xml b/mycobot_pro/mycobot_630_moveit/package.xml
index fbdd127..6b2a499 100644
--- a/mycobot_pro/mycobot_630_moveit/package.xml
+++ b/mycobot_pro/mycobot_630_moveit/package.xml
@@ -3,7 +3,7 @@
mycobot_630_moveit
0.3.0
- An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt! Motion Planning Framework
+ An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt Motion Planning Framework
wangweijian
wangweijian
@@ -19,20 +19,22 @@
moveit_ros_move_group
moveit_fake_controller_manager
moveit_kinematics
- moveit_planners_ompl
+ moveit_planners
moveit_ros_visualization
moveit_setup_assistant
+ moveit_simple_controller_manager
joint_state_publisher
+ joint_state_publisher_gui
robot_state_publisher
+ rviz
+ tf2_ros
xacro
+
+
+
- roscpp
- rospy
- std_msgs
- actionlib
- moveit_msgs
- mycobot_description
mycobot_description
diff --git a/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py
index 661f825..c0db4ea 100755
--- a/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py
+++ b/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py
@@ -23,6 +23,9 @@ def callback(data):
radians_to_angles = round(math.degrees(value), 2)
data_list.append(radians_to_angles)
+ data_list[1] = data_list[1] - 90
+ data_list[3] = data_list[3] - 90
+
rospy.loginfo(rospy.get_caller_id() + "%s", data_list)
mc.write_angles(data_list, 800)
diff --git a/mycobot_pro/mycobot_630_moveit/scripts/test_add_ground.py b/mycobot_pro/mycobot_630_moveit/scripts/test_add_ground.py
new file mode 100755
index 0000000..36f7f48
--- /dev/null
+++ b/mycobot_pro/mycobot_630_moveit/scripts/test_add_ground.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import rospy
+from moveit_commander import PlanningSceneInterface
+from geometry_msgs.msg import PoseStamped
+
+class GroundAdder:
+ def __init__(self):
+ # 初始化 ROS 节点
+ rospy.init_node('add_ground_node', anonymous=True)
+
+ # 初始化 PlanningSceneInterface
+ self.scene = PlanningSceneInterface()
+ rospy.sleep(2) # 等待场景初始化
+
+ def add_ground(self):
+ # 定义地面的姿态
+ ground_pose = PoseStamped()
+ ground_pose.header.frame_id = "base" # 基座坐标系
+ ground_pose.pose.position.x = 0.0
+ ground_pose.pose.position.y = 0.0
+ ground_pose.pose.position.z = -0.01 # 地面位于基座下方 0.01 米
+ ground_pose.pose.orientation.w = 1.0
+
+ # 添加地面到场景中
+ self.scene.add_box("ground", ground_pose, size=(1.5, 1.5, 0.005))
+ rospy.loginfo("Ground added to the planning scene.")
+
+if __name__ == "__main__":
+ try:
+ ground_adder = GroundAdder()
+ ground_adder.add_ground()
+ rospy.spin() # 保持节点运行，防止退出
+ except rospy.ROSInterruptException:
+ pass