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)