From 70e59b56503112ffba7384ee1cb73aee2f45ceba Mon Sep 17 00:00:00 2001 From: wangWking <842749351@qq.com> Date: Thu, 12 Sep 2024 17:08:10 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0280JN=20moveit?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mycobot_280jn_moveit/.setup_assistant | 2 +- .../mycobot_280jn_moveit/CMakeLists.txt | 16 +- .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 8 +- .../config/fake_controllers.yaml | 6 +- .../config/firefighter.srdf | 60 ++-- .../config/gazebo_controllers.yaml | 4 + .../config/gazebo_firefighter.urdf | 276 ++++++++++++++++++ .../config/joint_limits.yaml | 6 + .../config/kinematics.yaml | 4 +- .../config/ompl_planning.yaml | 59 +++- .../config/ros_controllers.yaml | 52 ++-- .../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_280jn_moveit/launch/demo.launch | 71 ++--- .../launch/demo_gazebo.launch | 79 +---- .../fake_moveit_controller_manager.launch.xml | 5 +- .../mycobot_280jn_moveit/launch/gazebo.launch | 37 ++- .../launch/move_group.launch | 60 +++- .../mycobot_280jn_moveit/launch/moveit.rviz | 56 ++-- .../launch/moveit_rviz.launch | 9 +- .../launch/mycobot_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 | 2 + .../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_280/mycobot_280jn_moveit/package.xml | 19 +- ...th_planning_and_obstacle_avoidance_demo.py | 137 --------- 41 files changed, 807 insertions(+), 444 deletions(-) create mode 100644 mycobot_280/mycobot_280jn_moveit/config/cartesian_limits.yaml create mode 100644 mycobot_280/mycobot_280jn_moveit/config/gazebo_controllers.yaml create mode 100644 mycobot_280/mycobot_280jn_moveit/config/gazebo_firefighter.urdf create mode 100644 mycobot_280/mycobot_280jn_moveit/config/simple_moveit_controllers.yaml create mode 100644 mycobot_280/mycobot_280jn_moveit/config/stomp_planning.yaml create mode 100644 mycobot_280/mycobot_280jn_moveit/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100644 mycobot_280/mycobot_280jn_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 mycobot_280/mycobot_280jn_moveit/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 mycobot_280/mycobot_280jn_moveit/launch/simple_moveit_controller_manager.launch.xml create mode 100644 mycobot_280/mycobot_280jn_moveit/launch/stomp_planning_pipeline.launch.xml delete mode 100644 mycobot_280/mycobot_280jn_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py diff --git a/mycobot_280/mycobot_280jn_moveit/.setup_assistant b/mycobot_280/mycobot_280jn_moveit/.setup_assistant index 4e22200..d450c9c 100644 --- a/mycobot_280/mycobot_280jn_moveit/.setup_assistant +++ b/mycobot_280/mycobot_280jn_moveit/.setup_assistant @@ -8,4 +8,4 @@ moveit_setup_assistant_config: CONFIG: author_name: zachary author_email: lijun.zhang@elephantrobotics.com - generated_timestamp: 1626074107 + generated_timestamp: 1726131211 \ No newline at end of file diff --git a/mycobot_280/mycobot_280jn_moveit/CMakeLists.txt b/mycobot_280/mycobot_280jn_moveit/CMakeLists.txt index 6d02a2a..62f56f9 100644 --- a/mycobot_280/mycobot_280jn_moveit/CMakeLists.txt +++ b/mycobot_280/mycobot_280jn_moveit/CMakeLists.txt @@ -1,22 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1.3) project(mycobot_280jn_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 - scripts/path_planning_and_obstacle_avoidance_demo.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_280/mycobot_280jn_moveit/config/cartesian_limits.yaml b/mycobot_280/mycobot_280jn_moveit/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/chomp_planning.yaml b/mycobot_280/mycobot_280jn_moveit/config/chomp_planning.yaml index 75258e5..eb9c912 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/chomp_planning.yaml +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/fake_controllers.yaml b/mycobot_280/mycobot_280jn_moveit/config/fake_controllers.yaml index 47a2eb3..74fdd55 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/fake_controllers.yaml +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/firefighter.srdf b/mycobot_280/mycobot_280jn_moveit/config/firefighter.srdf index 1c94891..f3830d9 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/firefighter.srdf +++ b/mycobot_280/mycobot_280jn_moveit/config/firefighter.srdf @@ -1,4 +1,4 @@ - + - - - - - - + + + + + + - - - - - - + + + + + + - + - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/config/gazebo_controllers.yaml b/mycobot_280/mycobot_280jn_moveit/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/gazebo_firefighter.urdf b/mycobot_280/mycobot_280jn_moveit/config/gazebo_firefighter.urdf new file mode 100644 index 0000000..ed086b1 --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/config/gazebo_firefighter.urdf @@ -0,0 +1,276 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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_280/mycobot_280jn_moveit/config/joint_limits.yaml b/mycobot_280/mycobot_280jn_moveit/config/joint_limits.yaml index 6e9612a..964c16b 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/joint_limits.yaml +++ b/mycobot_280/mycobot_280jn_moveit/config/joint_limits.yaml @@ -1,4 +1,10 @@ # 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: diff --git a/mycobot_280/mycobot_280jn_moveit/config/kinematics.yaml b/mycobot_280/mycobot_280jn_moveit/config/kinematics.yaml index 031b273..2f06d6e 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/kinematics.yaml +++ b/mycobot_280/mycobot_280jn_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 \ No newline at end of file + 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_280/mycobot_280jn_moveit/config/ompl_planning.yaml b/mycobot_280/mycobot_280jn_moveit/config/ompl_planning.yaml index 3f4d0c2..4828ba8 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/ompl_planning.yaml +++ b/mycobot_280/mycobot_280jn_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,46 @@ 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: None planner_configs: + - AnytimePathShortening - SBL - EST - LBKPIECE @@ -146,5 +190,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_280/mycobot_280jn_moveit/config/ros_controllers.yaml b/mycobot_280/mycobot_280jn_moveit/config/ros_controllers.yaml index 6b814df..6f1eab5 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/ros_controllers.yaml +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/sensors_3d.yaml b/mycobot_280/mycobot_280jn_moveit/config/sensors_3d.yaml index a4bb13e..01538cc 100644 --- a/mycobot_280/mycobot_280jn_moveit/config/sensors_3d.yaml +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/simple_moveit_controllers.yaml b/mycobot_280/mycobot_280jn_moveit/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..44f4a50 --- /dev/null +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/config/stomp_planning.yaml b/mycobot_280/mycobot_280jn_moveit/config/stomp_planning.yaml new file mode 100644 index 0000000..fa999a9 --- /dev/null +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/launch/chomp_planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/chomp_planning_pipeline.launch.xml index f9900e0..3c4a66f 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/chomp_planning_pipeline.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/chomp_planning_pipeline.launch.xml @@ -1,10 +1,21 @@ - - - + + + + - - + + + + - + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/default_warehouse_db.launch b/mycobot_280/mycobot_280jn_moveit/launch/default_warehouse_db.launch index f73ad4c..fe1857b 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/default_warehouse_db.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/default_warehouse_db.launch @@ -5,7 +5,7 @@ - + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/demo.launch b/mycobot_280/mycobot_280jn_moveit/launch/demo.launch index c4c7843..c0c6ede 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/demo.launch +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/launch/demo_gazebo.launch b/mycobot_280/mycobot_280jn_moveit/launch/demo_gazebo.launch index 874d7b8..0ef8f95 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/demo_gazebo.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/demo_gazebo.launch @@ -1,70 +1,21 @@ + + + - - - - + + + + + - - + + - - - - - - - - - - - - - - + + + + + - - - - - - - - - - - - - [/joint_states] - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mycobot_280/mycobot_280jn_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/fake_moveit_controller_manager.launch.xml index 6212c52..edeb038 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/fake_moveit_controller_manager.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/gazebo.launch b/mycobot_280/mycobot_280jn_moveit/launch/gazebo.launch index ba310e6..5098c29 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/gazebo.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/gazebo.launch @@ -1,23 +1,34 @@ - - - + + + + + + - - - - - + + + - - + + - - + + + + - + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/move_group.launch b/mycobot_280/mycobot_280jn_moveit/launch/move_group.launch index 258f295..2330bcb 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/move_group.launch +++ b/mycobot_280/mycobot_280jn_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_280/mycobot_280jn_moveit/launch/moveit.rviz b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz index c122915..dfc3d12 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz +++ b/mycobot_280/mycobot_280jn_moveit/launch/moveit.rviz @@ -5,12 +5,10 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Scene Geometry1 - - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 - Splitter Ratio: 0.7425600290298462 - Tree Height: 172 + Splitter Ratio: 0.5 + Tree Height: 209 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -43,18 +41,18 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.5 + - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: true + MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false MoveIt_Allow_Sensor_Positioning: false MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Use_Constraint_Aware_IK: false MoveIt_Workspace: Center: X: 0 @@ -74,6 +72,11 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + g_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -130,11 +133,11 @@ Visualization Manager: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.12999999523162842 + Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 Planning Group: arm_group Query Goal State: true - Query Start State: true + Query Start State: false Show Workspace: false Start State Alpha: 1 Start State Color: 0; 255; 0 @@ -143,7 +146,7 @@ Visualization Manager: Scene Geometry: Scene Alpha: 1 Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -155,6 +158,11 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + g_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -192,14 +200,14 @@ Visualization Manager: Value: true Robot Alpha: 0.5 Show Robot Collision: false - Show Robot Visual: false + Show Robot Visual: true Value: true - Velocity_Scaling_Factor: 0.5 + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: joint1 + Fixed Frame: g_base Frame Rate: 30 Name: root Tools: @@ -210,31 +218,31 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/XYOrbit - Distance: 1.3915867805480957 + Class: rviz/Orbit + Distance: 1.1993907690048218 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 + Field of View: 0.75 Focal Point: - X: 0.11356700211763382 - Y: 0.10592000186443329 - Z: 2.2351800055275817e-07 + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.0647968053817749 - Target Frame: joint1 - Yaw: 3.026766300201416 + Pitch: 0.7100000381469727 + Target Frame: g_base + Yaw: 2.124948024749756 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 876 + Height: 906 Help: collapsed: false Hide Left Dock: false @@ -243,7 +251,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f300000312fc0200000007fb000000100044006900730070006c006100790073010000003d0000013d000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000180000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c7000001880000017d00ffffff0000053f0000031200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f300000330fc0200000007fb000000100044006900730070006c006100790073010000003d00000162000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000001a5000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ec000001810000017d00ffffff0000053f0000033000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1848 diff --git a/mycobot_280/mycobot_280jn_moveit/launch/moveit_rviz.launch b/mycobot_280/mycobot_280jn_moveit/launch/moveit_rviz.launch index 8b23018..a4605c0 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/moveit_rviz.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/moveit_rviz.launch @@ -4,13 +4,12 @@ - - - + + + - + args="$(arg command_args)" output="screen"> diff --git a/mycobot_280/mycobot_280jn_moveit/launch/mycobot_moveit.launch b/mycobot_280/mycobot_280jn_moveit/launch/mycobot_moveit.launch index dd341dc..c0c6ede 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/mycobot_moveit.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/mycobot_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_280/mycobot_280jn_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..50433b8 --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/ompl_planning_pipeline.launch.xml index da999df..533a45b 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/ompl_planning_pipeline.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/ompl_planning_pipeline.launch.xml @@ -1,21 +1,23 @@ - - + + - - + + - - - + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/planning_context.launch b/mycobot_280/mycobot_280jn_moveit/launch/planning_context.launch index 7b26ea3..5685b44 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/planning_context.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/planning_context.launch @@ -14,11 +14,13 @@ + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/planning_pipeline.launch.xml index 9c5ea2b..4b4d0d6 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/planning_pipeline.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/ros_controllers.launch b/mycobot_280/mycobot_280jn_moveit/launch/ros_controllers.launch index decd853..7523434 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/ros_controllers.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/ros_controllers.launch @@ -6,6 +6,6 @@ + output="screen" args="arm_group_controller "/> diff --git a/mycobot_280/mycobot_280jn_moveit/launch/run_benchmark_ompl.launch b/mycobot_280/mycobot_280jn_moveit/launch/run_benchmark_ompl.launch index 652f50e..811fe37 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/run_benchmark_ompl.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/run_benchmark_ompl.launch @@ -4,18 +4,17 @@ - + - + - diff --git a/mycobot_280/mycobot_280jn_moveit/launch/sensor_manager.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/sensor_manager.launch.xml index f29b1fd..e1595e0 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/sensor_manager.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/sensor_manager.launch.xml @@ -12,6 +12,6 @@ - + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/setup_assistant.launch b/mycobot_280/mycobot_280jn_moveit/launch/setup_assistant.launch index 17bbfec..726a859 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/setup_assistant.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/setup_assistant.launch @@ -1,4 +1,4 @@ - + @@ -8,8 +8,9 @@ diff --git a/mycobot_280/mycobot_280jn_moveit/launch/simple_moveit_controller_manager.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..6ea7493 --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/stomp_planning_pipeline.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..edc52fb --- /dev/null +++ b/mycobot_280/mycobot_280jn_moveit/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/trajectory_execution.launch.xml b/mycobot_280/mycobot_280jn_moveit/launch/trajectory_execution.launch.xml index c574d5c..20c3dfc 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/trajectory_execution.launch.xml +++ b/mycobot_280/mycobot_280jn_moveit/launch/trajectory_execution.launch.xml @@ -1,8 +1,11 @@ + - + + + - + @@ -13,8 +16,8 @@ - - - + + diff --git a/mycobot_280/mycobot_280jn_moveit/launch/warehouse.launch b/mycobot_280/mycobot_280jn_moveit/launch/warehouse.launch index 8736cf3..0712e67 100644 --- a/mycobot_280/mycobot_280jn_moveit/launch/warehouse.launch +++ b/mycobot_280/mycobot_280jn_moveit/launch/warehouse.launch @@ -4,7 +4,7 @@ - + diff --git a/mycobot_280/mycobot_280jn_moveit/package.xml b/mycobot_280/mycobot_280jn_moveit/package.xml index 772dd75..4b1dd2d 100644 --- a/mycobot_280/mycobot_280jn_moveit/package.xml +++ b/mycobot_280/mycobot_280jn_moveit/package.xml @@ -3,7 +3,7 @@ mycobot_280jn_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 zachary zachary @@ -19,21 +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_280/mycobot_280jn_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py b/mycobot_280/mycobot_280jn_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py deleted file mode 100644 index cd4024d..0000000 --- a/mycobot_280/mycobot_280jn_moveit/scripts/path_planning_and_obstacle_avoidance_demo.py +++ /dev/null @@ -1,137 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import rospy, roslib, sys -import moveit_commander -from moveit_msgs.msg import RobotTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint - -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import euler_from_quaternion, quaternion_from_euler - - -class MoveItPlanningDemo: - def __init__(self): - # API to initialize move_group,初始化move_group的API - moveit_commander.roscpp_initialize(sys.argv) - - # Initialize the ROS node,初始化ROS节点 - rospy.init_node("moveit_ik_demo") - - # Initialize the scene object to monitor changes in the external environment - # 初始化场景对象，用来监听外部环境的变化 - self.scene = moveit_commander.PlanningSceneInterface() - rospy.sleep(1) - - # Initialize self.arm group in the robotic arm that needs to be controlled by move group - # 初始化需要使用move group控制的机械臂中的self.arm group - self.arm = moveit_commander.MoveGroupCommander("arm_group") - - # Get the name of the terminal link,获取终端link的名称 - self.end_effector_link = self.arm.get_end_effector_link() - - # Set the reference coordinate system used for the target position - # 设置目标位置所使用的参考坐标系 - self.reference_frame = "joint1" - self.arm.set_pose_reference_frame(self.reference_frame) - - # Allow replanning when motion planning fails,当运动规划失败后，允许重新规划 - self.arm.allow_replanning(True) - - # Set the allowable error of position (unit: meter) and attitude (unit: radian) - # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 - self.arm.set_goal_position_tolerance(0.01) - self.arm.set_goal_orientation_tolerance(0.05) - - def moving(self): - # # Control the robotic arm to return to the initialization position first - # 控制机械臂先回到初始化位置 - self.arm.set_named_target("init_pose") - self.arm.go() - rospy.sleep(2) - - # Set the target pose in the robotic arm workspace, the position is described by x, y, z coordinates, - # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， - # Pose is described by quaternion, based on base_link coordinate system - # 姿态使用四元数描述，基于base_link坐标系 - target_pose = PoseStamped() - target_pose.header.frame_id = self.reference_frame - target_pose.header.stamp = rospy.Time.now() - target_pose.pose.position.x = 0.132 - target_pose.pose.position.y = -0.150 - target_pose.pose.position.z = 0.075 - target_pose.pose.orientation.x = 0.026 - target_pose.pose.orientation.y = 1.0 - target_pose.pose.orientation.z = 0.0 - target_pose.pose.orientation.w = 0.014 - - # Set the current state of the robot arm as the initial state of motion - # 设置机器臂当前的状态作为运动初始状态 - self.arm.set_start_state_to_current_state() - - # 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() - - # Control the motion of the robotic arm according to the planned motion path - # 按照规划的运动路径控制机械臂运动 - self.arm.execute(traj) - rospy.sleep(1) - - # Control the terminal of the robotic arm to move 5cm to the right. Parameter 1 represents y, 0,1,2,3,4,5 represents xyzrpy - # 控制机械臂终端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy - self.arm.shift_pose_target(1, 0.12, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - self.arm.shift_pose_target(1, 0.1, self.end_effector_link) - self.arm.go() - rospy.sleep(1) - - # Control the terminal of the robotic arm to rotate 90 degrees in the opposite direction. 0,1,2,3,4,5 represent xyzrpy - # 控制机械臂终端反向旋转90度 0,1,2,3,4,5 代表xyzrpy - # self.arm.shift_pose_target(3, -1.57, end_effector_link) - # self.arm.go() - # rospy.sleep(1) - - def run(self): - self.scene.remove_world_object("suit") - - # Run once without obstacles,没有障碍物运行一次 - self.moving() - - # Add environment,添加环境 - quat = quaternion_from_euler(3.1415, 0, -1.57) - - suit_post = PoseStamped() - suit_post.header.frame_id = self.reference_frame - suit_post.pose.position.x = 0.0 - suit_post.pose.position.y = 0.0 - suit_post.pose.position.z = -0.02 - suit_post.pose.orientation.x = quat[0] - suit_post.pose.orientation.y = quat[1] - suit_post.pose.orientation.z = quat[2] - suit_post.pose.orientation.w = quat[3] - - suit_path = ( - roslib.packages.get_pkg_dir("mycobot_description") - + "/urdf/mycobot/suit_env.dae" - ) - # need `pyassimp==3.3` - self.scene.add_mesh("suit", suit_post, suit_path) - rospy.sleep(2) - - # Run it again if there is an environmental impact,有环境影响后再运行一次 - self.moving() - - # close and exit moveit，关闭并退出moveit - moveit_commander.roscpp_shutdown() - moveit_commander.os._exit(0) - - -if __name__ == "__main__": - o = MoveItPlanningDemo() - o.run()