From eec15553408f7e84ffc76a2d31147cf6970c1b06 Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Tue, 7 Jan 2025 18:40:49 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96pro=20600=20moveit=E5=8C=85-?= =?UTF-8?q?=E4=BF=AE=E5=A4=8Drviz=E6=B7=BB=E5=8A=A0=E5=9C=B0=E9=9D=A2?= =?UTF-8?q?=E5=90=8E=E6=97=A0=E6=B3=95=E8=BF=90=E5=8A=A8=E8=A7=84=E5=88=92?= =?UTF-8?q?=E9=97=AE=E9=A2=98=EF=BC=8C=E6=9B=B4=E6=96=B0URDF=E6=A8=A1?= =?UTF-8?q?=E5=9E=8B=E5=88=9D=E5=A7=8B=E5=A7=BF=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mycobot_pro_600_moveit.urdf | 183 +++++++++++++ .../mycobot_600_moveit/.setup_assistant | 4 +- mycobot_pro/mycobot_600_moveit/CMakeLists.txt | 15 +- .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 8 +- .../config/fake_controllers.yaml | 6 +- .../config/firefighter.srdf | 69 +++-- .../config/gazebo_controllers.yaml | 4 + .../config/gazebo_firefighter.urdf | 257 ++++++++++++++++++ .../config/joint_limits.yaml | 42 +-- .../mycobot_600_moveit/config/kinematics.yaml | 4 +- .../config/ompl_planning.yaml | 58 +++- .../config/ros_controllers.yaml | 52 ++-- .../mycobot_600_moveit/config/sensors_3d.yaml | 1 - .../config/simple_moveit_controllers.yaml | 12 + .../config/stomp_planning.yaml | 39 +++ .../launch/chomp_planning_pipeline.launch.xml | 23 +- .../launch/default_warehouse_db.launch | 2 +- .../mycobot_600_moveit/launch/demo.launch | 71 ++--- .../launch/demo_gazebo.launch | 79 +----- .../fake_moveit_controller_manager.launch.xml | 5 +- .../mycobot_600_moveit/launch/gazebo.launch | 37 ++- .../launch/move_group.launch | 60 ++-- .../mycobot_600_moveit/launch/moveit.rviz | 49 ++-- .../launch/moveit_rviz.launch | 9 +- .../launch/mycobot600_moveit.launch | 72 ++--- .../ompl-chomp_planning_pipeline.launch.xml | 20 ++ .../launch/ompl_planning_pipeline.launch.xml | 26 +- ...otion_planner_planning_pipeline.launch.xml | 15 + .../launch/planning_context.launch | 4 +- .../launch/planning_pipeline.launch.xml | 2 +- ...ntrol_moveit_controller_manager.launch.xml | 4 + .../launch/ros_controllers.launch | 2 +- .../launch/run_benchmark_ompl.launch | 5 +- .../launch/sensor_manager.launch.xml | 2 +- .../launch/setup_assistant.launch | 7 +- ...imple_moveit_controller_manager.launch.xml | 8 + .../launch/stomp_planning_pipeline.launch.xml | 23 ++ .../launch/trajectory_execution.launch.xml | 13 +- .../launch/warehouse.launch | 2 +- mycobot_pro/mycobot_600_moveit/package.xml | 18 +- .../mycobot_600_moveit/scripts/sync_plan.py | 3 + 42 files changed, 982 insertions(+), 338 deletions(-) create mode 100755 mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf create mode 100644 mycobot_pro/mycobot_600_moveit/config/cartesian_limits.yaml create mode 100644 mycobot_pro/mycobot_600_moveit/config/gazebo_controllers.yaml create mode 100644 mycobot_pro/mycobot_600_moveit/config/gazebo_firefighter.urdf create mode 100644 mycobot_pro/mycobot_600_moveit/config/simple_moveit_controllers.yaml create mode 100644 mycobot_pro/mycobot_600_moveit/config/stomp_planning.yaml create mode 100644 mycobot_pro/mycobot_600_moveit/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100644 mycobot_pro/mycobot_600_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 mycobot_pro/mycobot_600_moveit/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 mycobot_pro/mycobot_600_moveit/launch/simple_moveit_controller_manager.launch.xml create mode 100644 mycobot_pro/mycobot_600_moveit/launch/stomp_planning_pipeline.launch.xml diff --git a/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf b/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf new file mode 100755 index 0000000..ec05b4b --- /dev/null +++ b/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_moveit.urdf @@ -0,0 +1,183 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/.setup_assistant b/mycobot_pro/mycobot_600_moveit/.setup_assistant index 22756f1..dba7a13 100644 --- a/mycobot_pro/mycobot_600_moveit/.setup_assistant +++ b/mycobot_pro/mycobot_600_moveit/.setup_assistant @@ -1,11 +1,11 @@ moveit_setup_assistant_config: URDF: package: mycobot_description - relative_path: urdf/mycobot_pro_600/mycobot_pro_600.urdf + relative_path: urdf/mycobot_pro_600/mycobot_pro_600_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: 1736245598 \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_moveit/CMakeLists.txt b/mycobot_pro/mycobot_600_moveit/CMakeLists.txt index 3a55cbf..15cc2dd 100644 --- a/mycobot_pro/mycobot_600_moveit/CMakeLists.txt +++ b/mycobot_pro/mycobot_600_moveit/CMakeLists.txt @@ -1,21 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(mycobot_600_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_600_moveit/config/cartesian_limits.yaml b/mycobot_pro/mycobot_600_moveit/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/mycobot_pro/mycobot_600_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_600_moveit/config/chomp_planning.yaml b/mycobot_pro/mycobot_600_moveit/config/chomp_planning.yaml index 75258e5..eb9c912 100644 --- a/mycobot_pro/mycobot_600_moveit/config/chomp_planning.yaml +++ b/mycobot_pro/mycobot_600_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_600_moveit/config/fake_controllers.yaml b/mycobot_pro/mycobot_600_moveit/config/fake_controllers.yaml index 47a2eb3..74fdd55 100644 --- a/mycobot_pro/mycobot_600_moveit/config/fake_controllers.yaml +++ b/mycobot_pro/mycobot_600_moveit/config/fake_controllers.yaml @@ -1,9 +1,13 @@ controller_list: - name: fake_arm_group_controller + type: $(arg fake_execution_type) joints: - joint2_to_joint1 - joint3_to_joint2 - joint4_to_joint3 - joint5_to_joint4 - joint6_to_joint5 - - joint6output_to_joint6 \ No newline at end of file + - joint6output_to_joint6 +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_600_moveit/config/firefighter.srdf b/mycobot_pro/mycobot_600_moveit/config/firefighter.srdf index 60a6e0e..9b3f5bd 100644 --- a/mycobot_pro/mycobot_600_moveit/config/firefighter.srdf +++ b/mycobot_pro/mycobot_600_moveit/config/firefighter.srdf @@ -1,4 +1,4 @@ - + - - - - - - + + + + + + - - - - - - + + + + + + - - - - + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/config/gazebo_controllers.yaml b/mycobot_pro/mycobot_600_moveit/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/mycobot_pro/mycobot_600_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_600_moveit/config/gazebo_firefighter.urdf b/mycobot_pro/mycobot_600_moveit/config/gazebo_firefighter.urdf new file mode 100644 index 0000000..93f26df --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/config/gazebo_firefighter.urdf @@ -0,0 +1,257 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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_600_moveit/config/joint_limits.yaml b/mycobot_pro/mycobot_600_moveit/config/joint_limits.yaml index a99e764..964c16b 100644 --- a/mycobot_pro/mycobot_600_moveit/config/joint_limits.yaml +++ b/mycobot_pro/mycobot_600_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: 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 + max_acceleration: 0 joint6output_to_joint6: - 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_600_moveit/config/kinematics.yaml b/mycobot_pro/mycobot_600_moveit/config/kinematics.yaml index 3c16bbc..2f06d6e 100644 --- a/mycobot_pro/mycobot_600_moveit/config/kinematics.yaml +++ b/mycobot_pro/mycobot_600_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_600_moveit/config/ompl_planning.yaml b/mycobot_pro/mycobot_600_moveit/config/ompl_planning.yaml index 82f7d9d..17c2ccb 100644 --- a/mycobot_pro/mycobot_600_moveit/config/ompl_planning.yaml +++ b/mycobot_pro/mycobot_600_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 + - AITstar + - ABITstar + - BITstar projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2) - longest_valid_segment_fraction: 0.005 \ No newline at end of file + longest_valid_segment_fraction: 0.005 diff --git a/mycobot_pro/mycobot_600_moveit/config/ros_controllers.yaml b/mycobot_pro/mycobot_600_moveit/config/ros_controllers.yaml index 6b814df..6f1eab5 100644 --- a/mycobot_pro/mycobot_600_moveit/config/ros_controllers.yaml +++ b/mycobot_pro/mycobot_600_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 - joint2_to_joint1 - joint3_to_joint2 - joint4_to_joint3 - joint5_to_joint4 - joint6_to_joint5 - joint6output_to_joint6 - 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: + 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 + joint6output_to_joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_moveit/config/sensors_3d.yaml b/mycobot_pro/mycobot_600_moveit/config/sensors_3d.yaml index a4bb13e..01538cc 100644 --- a/mycobot_pro/mycobot_600_moveit/config/sensors_3d.yaml +++ b/mycobot_pro/mycobot_600_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_600_moveit/config/simple_moveit_controllers.yaml b/mycobot_pro/mycobot_600_moveit/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..44f4a50 --- /dev/null +++ b/mycobot_pro/mycobot_600_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: + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_joint6 \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_moveit/config/stomp_planning.yaml b/mycobot_pro/mycobot_600_moveit/config/stomp_planning.yaml new file mode 100644 index 0000000..fa999a9 --- /dev/null +++ b/mycobot_pro/mycobot_600_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_600_moveit/launch/chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/chomp_planning_pipeline.launch.xml index 03d4520..e96d2c9 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/chomp_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/chomp_planning_pipeline.launch.xml @@ -1,10 +1,21 @@ - - - + + + + - - + + + + - + diff --git a/mycobot_pro/mycobot_600_moveit/launch/default_warehouse_db.launch b/mycobot_pro/mycobot_600_moveit/launch/default_warehouse_db.launch index 3b82435..ffcbbd6 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/default_warehouse_db.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/default_warehouse_db.launch @@ -5,7 +5,7 @@ - + diff --git a/mycobot_pro/mycobot_600_moveit/launch/demo.launch b/mycobot_pro/mycobot_600_moveit/launch/demo.launch index 5953d25..7128186 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/demo.launch +++ b/mycobot_pro/mycobot_600_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_600_moveit/launch/demo_gazebo.launch b/mycobot_pro/mycobot_600_moveit/launch/demo_gazebo.launch index 4004d05..0ef8f95 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/demo_gazebo.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/demo_gazebo.launch @@ -1,70 +1,21 @@ + + + - - - - + + + + + - - + + - - - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - [/joint_states] - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_pro/mycobot_600_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/fake_moveit_controller_manager.launch.xml index e572f2d..c68fc20 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/fake_moveit_controller_manager.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/mycobot_pro/mycobot_600_moveit/launch/gazebo.launch b/mycobot_pro/mycobot_600_moveit/launch/gazebo.launch index 1b61334..7829fef 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/gazebo.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/gazebo.launch @@ -1,23 +1,34 @@ - - - + + + + + + - - - - - + + + - - + + - - + + + + - + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/move_group.launch b/mycobot_pro/mycobot_600_moveit/launch/move_group.launch index 3457e11..2330bcb 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/move_group.launch +++ b/mycobot_pro/mycobot_600_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_600_moveit/launch/moveit.rviz b/mycobot_pro/mycobot_600_moveit/launch/moveit.rviz index 4292ffe..f9c1318 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/moveit.rviz +++ b/mycobot_pro/mycobot_600_moveit/launch/moveit.rviz @@ -9,8 +9,8 @@ Panels: - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 - Splitter Ratio: 0.742560029 - Tree Height: 195 + Splitter Ratio: 0.7425600290298462 + Tree Height: 197 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -18,6 +18,8 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -29,7 +31,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -49,12 +51,10 @@ Visualization Manager: MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 MoveIt_Workspace: Center: X: 0 @@ -118,18 +118,19 @@ Visualization Manager: State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.0799999982 + TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.109999999 + Interactive Marker Size: 0.10999999940395355 Joint Violation Color: 255; 0; 255 Planning Group: arm_group Query Goal State: true @@ -142,7 +143,7 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.200000003 + Scene Display Time: 0.20000000298023224 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -210,30 +211,30 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 1.39158678 + Distance: 1.8773764371871948 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 0.113567002 - Y: 0.105920002 - Z: 2.23518001e-07 + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-07 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.354796648 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3547966778278351 Target Frame: base - Value: XYOrbit (rviz) - Yaw: 2.02676558 + Yaw: 2.836765766143799 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 898 + Height: 906 Help: collapsed: false Hide Left Dock: false @@ -242,9 +243,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000002ad0000033cfc0200000007fb000000100044006900730070006c006100790073010000002800000158000000d700fffffffb0000000800480065006c00700000000342000000bb0000007300fffffffb0000000a0056006900650077007300000003b0000000b0000000ad00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000001860000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d60000018e0000018300ffffff000004710000033c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d00000156000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000199000000420000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001e10000018c0000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1828 - X: 60 - Y: 15 + Width: 1848 + X: 72 + Y: 27 diff --git a/mycobot_pro/mycobot_600_moveit/launch/moveit_rviz.launch b/mycobot_pro/mycobot_600_moveit/launch/moveit_rviz.launch index c9ea170..a4605c0 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/moveit_rviz.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/moveit_rviz.launch @@ -4,13 +4,12 @@ - - - + + + - + args="$(arg command_args)" output="screen"> diff --git a/mycobot_pro/mycobot_600_moveit/launch/mycobot600_moveit.launch b/mycobot_pro/mycobot_600_moveit/launch/mycobot600_moveit.launch index a5395fc..7128186 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/mycobot600_moveit.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/mycobot600_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_600_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..5aa7b4e --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/ompl_planning_pipeline.launch.xml index 47c5f82..7b31555 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/ompl_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/ompl_planning_pipeline.launch.xml @@ -1,21 +1,23 @@ - - + + - - + + - - - + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/planning_context.launch b/mycobot_pro/mycobot_600_moveit/launch/planning_context.launch index 28dd1fc..46db7b0 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/planning_context.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/planning_context.launch @@ -6,7 +6,7 @@ - + @@ -14,11 +14,13 @@ + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/planning_pipeline.launch.xml index fe2cf4d..4b4d0d6 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/mycobot_pro/mycobot_600_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/ros_controllers.launch b/mycobot_pro/mycobot_600_moveit/launch/ros_controllers.launch index 03de6bf..f3a08eb 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/ros_controllers.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/ros_controllers.launch @@ -6,6 +6,6 @@ + output="screen" args="arm_group_controller "/> diff --git a/mycobot_pro/mycobot_600_moveit/launch/run_benchmark_ompl.launch b/mycobot_pro/mycobot_600_moveit/launch/run_benchmark_ompl.launch index 94e8fc3..040b6e6 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/run_benchmark_ompl.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/run_benchmark_ompl.launch @@ -4,18 +4,17 @@ - + - + - diff --git a/mycobot_pro/mycobot_600_moveit/launch/sensor_manager.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/sensor_manager.launch.xml index d7c9ca8..e7115c1 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/sensor_manager.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/sensor_manager.launch.xml @@ -12,6 +12,6 @@ - + diff --git a/mycobot_pro/mycobot_600_moveit/launch/setup_assistant.launch b/mycobot_pro/mycobot_600_moveit/launch/setup_assistant.launch index 040d4f6..62bfd85 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/setup_assistant.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -8,8 +8,9 @@ diff --git a/mycobot_pro/mycobot_600_moveit/launch/simple_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..4827d50 --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/stomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..1588cba --- /dev/null +++ b/mycobot_pro/mycobot_600_moveit/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/trajectory_execution.launch.xml b/mycobot_pro/mycobot_600_moveit/launch/trajectory_execution.launch.xml index 2207f7c..20c3dfc 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/trajectory_execution.launch.xml +++ b/mycobot_pro/mycobot_600_moveit/launch/trajectory_execution.launch.xml @@ -1,8 +1,11 @@ + - + + + - + @@ -13,8 +16,8 @@ - - - + + diff --git a/mycobot_pro/mycobot_600_moveit/launch/warehouse.launch b/mycobot_pro/mycobot_600_moveit/launch/warehouse.launch index 827a551..0712e67 100644 --- a/mycobot_pro/mycobot_600_moveit/launch/warehouse.launch +++ b/mycobot_pro/mycobot_600_moveit/launch/warehouse.launch @@ -4,7 +4,7 @@ - + diff --git a/mycobot_pro/mycobot_600_moveit/package.xml b/mycobot_pro/mycobot_600_moveit/package.xml index 3283fd7..477b36c 100644 --- a/mycobot_pro/mycobot_600_moveit/package.xml +++ b/mycobot_pro/mycobot_600_moveit/package.xml @@ -3,7 +3,7 @@ mycobot_600_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_600_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py index 81c3abb..a35ecda 100755 --- a/mycobot_pro/mycobot_600_moveit/scripts/sync_plan.py +++ b/mycobot_pro/mycobot_600_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, 1000)