From 4388a637b40adbf5c140208d69b60b73f925fe5d Mon Sep 17 00:00:00 2001 From: Tenero-Jz <2316126650@qq.com> Date: Tue, 13 Aug 2024 10:14:26 +0800 Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E5=A2=9E=20mycobot600=20=E6=9C=BA?= =?UTF-8?q?=E6=A2=B0=E8=87=82=E8=B7=9F=E9=9A=8F=E4=BB=BF=E7=9C=9F=E8=BF=90?= =?UTF-8?q?=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mycobot_280/scripts/follow_display.py | 3 +- .../.setup_assistant | 4 +- .../config/fake_controllers.yaml | 19 +- .../launch/moveit.rviz | 83 +-- .../launch/planning_context.launch | 6 +- .../scripts/sync_plan.py | 30 +- .../scripts/sync_plan_630.py | 102 +++ .../scripts/test.py | 35 ++ .../mycobot_pro_600_gripper.urdf | 10 +- .../mycobot_pro_630/mycobot_630_gripper.urdf | 16 +- .../mycobot_600/config/mycobot_600.rviz | 12 +- .../launch/mycobot_600_follow_display.launch | 11 + .../mycobot_600_gripper_follow_display.launch | 0 .../scripts/mycobot_600_follow_display.py | 105 ++++ .../mycobot_600_gripper_follow_diplay.py | 0 mycobot_pro/mycobot_600/scripts/test.py | 23 + .../mycobot_600_gripper_moveit/CMakeLists.txt | 0 .../config/cartesian_limits.yaml | 0 .../config/chomp_planning.yaml | 0 .../config/fake_controllers.yaml | 21 +- .../config/firefighter.srdf | 142 ++++- .../config/gazebo_controllers.yaml | 0 .../config/gazebo_firefighter.urdf | 579 ------------------ .../config/joint_limits.yaml | 30 - .../config/kinematics.yaml | 11 +- .../config/ompl_planning.yaml | 59 +- .../config/ros_controllers.yaml | 14 +- .../config/sensors_3d.yaml | 0 .../config/simple_moveit_controllers.yaml | 11 +- .../config/stomp_planning.yaml | 10 +- .../launch/chomp_planning_pipeline.launch.xml | 6 +- .../launch/default_warehouse_db.launch | 0 .../launch/demo.launch | 3 + .../launch/demo_gazebo.launch | 24 +- .../fake_moveit_controller_manager.launch.xml | 0 ...refighter_moveit_sensor_manager.launch.xml | 0 .../launch/gazebo.launch | 36 +- .../launch/joystick_control.launch | 0 .../launch/move_group.launch | 4 - .../launch/moveit.rviz | 306 ++++++++- .../launch/moveit_rviz.launch | 0 .../launch/mycobot600_gripper_moveit.launch | 69 +++ .../ompl-chomp_planning_pipeline.launch.xml | 21 +- .../launch/ompl_planning_pipeline.launch.xml | 6 +- ...otion_planner_planning_pipeline.launch.xml | 0 .../launch/planning_context.launch | 4 +- .../launch/planning_pipeline.launch.xml | 0 ...ntrol_moveit_controller_manager.launch.xml | 0 .../launch/ros_controllers.launch | 2 +- .../launch/run_benchmark_ompl.launch | 0 .../launch/sensor_manager.launch.xml | 0 .../launch/setup_assistant.launch | 0 ...imple_moveit_controller_manager.launch.xml | 0 .../launch/stomp_planning_pipeline.launch.xml | 6 +- .../launch/trajectory_execution.launch.xml | 0 .../launch/warehouse.launch | 0 .../launch/warehouse_settings.launch.xml | 0 .../mycobot_600_gripper_moveit/package.xml | 10 +- .../scripts/sync_plan.py | 102 +++ .../scripts/sync_plan_320.py | 88 +++ .../scripts/test.py | 37 ++ .../launch/mycobot_630_follow_display.launch | 11 + .../scripts/mycobot_630_follow_display.py | 105 ++++ .../config/fake_controllers.yaml | 4 +- .../config/firefighter.srdf | 27 +- .../config/kinematics.yaml | 1 + .../config/ompl_planning.yaml | 1 + .../launch/moveit.rviz | 26 +- .../launch/ompl_planning_pipeline.launch.xml | 2 + .../launch/run_benchmark_ompl.launch | 2 + .../scripts/sync_plan.py | 68 ++ .../fake_moveit_controller_manager.launch.xml | 3 + ...ghter_moveit_controller_manager.launch.xml | 1 + .../mycobot_630_moveit/launch/moveit.rviz | 197 +++++- .../launch/planning_context.launch | 8 +- .../mycobot_630_moveit/scripts/sync_plan.py | 2 +- .../CMakeLists.txt | 10 + .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 26 + .../config/firefighter.srdf | 225 +++++++ .../config/gazebo_controllers.yaml | 4 + .../config/joint_limits.yaml | 45 ++ .../config/kinematics.yaml | 10 + .../config/ompl_planning.yaml | 187 ++++++ .../config/ros_controllers.yaml | 52 ++ .../config/sensors_3d.yaml | 2 + .../config/simple_moveit_controllers.yaml | 20 + .../config/stomp_planning.yaml | 78 +++ .../launch/chomp_planning_pipeline.launch.xml | 23 + .../launch/default_warehouse_db.launch | 15 + .../launch/demo.launch | 69 +++ .../launch/demo_gazebo.launch | 21 + .../fake_moveit_controller_manager.launch.xml | 12 + ...refighter_moveit_sensor_manager.launch.xml | 3 + .../launch/gazebo.launch | 32 + .../launch/joystick_control.launch | 17 + .../launch/move_group.launch | 101 +++ .../launch/moveit.rviz | 387 ++++++++++++ .../launch/moveit_rviz.launch | 15 + .../launch/mycobot630_gripper_moveit.launch | 69 +++ .../ompl-chomp_planning_pipeline.launch.xml | 19 + .../launch/ompl_planning_pipeline.launch.xml | 26 + ...otion_planner_planning_pipeline.launch.xml | 15 + .../launch/planning_context.launch | 28 + .../launch/planning_pipeline.launch.xml | 10 + ...ntrol_moveit_controller_manager.launch.xml | 4 + .../launch/ros_controllers.launch | 11 + .../launch/run_benchmark_ompl.launch | 21 + .../launch/sensor_manager.launch.xml | 17 + .../launch/setup_assistant.launch | 16 + ...imple_moveit_controller_manager.launch.xml | 8 + .../launch/stomp_planning_pipeline.launch.xml | 25 + .../launch/trajectory_execution.launch.xml | 23 + .../launch/warehouse.launch | 15 + .../launch/warehouse_settings.launch.xml | 16 + .../package.xml | 41 ++ .../scripts/sync_plan.py | 102 +++ .../scripts/sync_plan_320.py | 88 +++ .../scripts/test.py | 35 ++ 120 files changed, 3601 insertions(+), 883 deletions(-) create mode 100755 mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan_630.py create mode 100644 mycobot_320/mycobot_320_gripper_moveit/scripts/test.py create mode 100644 mycobot_pro/mycobot_600/launch/mycobot_600_follow_display.launch create mode 100644 mycobot_pro/mycobot_600/launch/mycobot_600_gripper_follow_display.launch create mode 100644 mycobot_pro/mycobot_600/scripts/mycobot_600_follow_display.py create mode 100644 mycobot_pro/mycobot_600/scripts/mycobot_600_gripper_follow_diplay.py create mode 100644 mycobot_pro/mycobot_600/scripts/test.py mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/CMakeLists.txt mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/cartesian_limits.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/chomp_planning.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_controllers.yaml delete mode 100644 mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_firefighter.urdf mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/joint_limits.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/kinematics.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/ompl_planning.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/ros_controllers.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/sensors_3d.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/simple_moveit_controllers.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/config/stomp_planning.yaml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/chomp_planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/default_warehouse_db.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/demo.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/demo_gazebo.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/gazebo.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/joystick_control.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/move_group.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/moveit.rviz mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/moveit_rviz.launch create mode 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/mycobot600_gripper_moveit.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/ompl_planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/planning_context.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/ros_controllers.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/run_benchmark_ompl.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/sensor_manager.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/setup_assistant.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/stomp_planning_pipeline.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/trajectory_execution.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse.launch mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse_settings.launch.xml mode change 100644 => 100755 mycobot_pro/mycobot_600_gripper_moveit/package.xml create mode 100755 mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan.py create mode 100755 mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan_320.py create mode 100755 mycobot_pro/mycobot_600_gripper_moveit/scripts/test.py create mode 100644 mycobot_pro/mycobot_630/launch/mycobot_630_follow_display.launch create mode 100644 mycobot_pro/mycobot_630/scripts/mycobot_630_follow_display.py create mode 100755 mycobot_pro/mycobot_630_gripper_moveit/scripts/sync_plan.py create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/CMakeLists.txt create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/cartesian_limits.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/chomp_planning.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/fake_controllers.yaml create mode 100644 mycobot_pro/new_mycobot_630_gripper_moveit/config/firefighter.srdf create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/gazebo_controllers.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/joint_limits.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/kinematics.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/ompl_planning.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/ros_controllers.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/sensors_3d.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/simple_moveit_controllers.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/config/stomp_planning.yaml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/chomp_planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/default_warehouse_db.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo_gazebo.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/gazebo.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/joystick_control.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/move_group.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit.rviz create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit_rviz.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/mycobot630_gripper_moveit.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_context.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_controllers.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/sensor_manager.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/setup_assistant.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/stomp_planning_pipeline.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/trajectory_execution.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse.launch create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse_settings.launch.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/package.xml create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan.py create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan_320.py create mode 100755 mycobot_pro/new_mycobot_630_gripper_moveit/scripts/test.py diff --git a/mycobot_280/mycobot_280/scripts/follow_display.py b/mycobot_280/mycobot_280/scripts/follow_display.py index d661974..27aaa2f 100755 --- a/mycobot_280/mycobot_280/scripts/follow_display.py +++ b/mycobot_280/mycobot_280/scripts/follow_display.py @@ -13,7 +13,7 @@ def talker(): rospy.init_node("display", anonymous=True) print("Try connect real mycobot...") - port = rospy.get_param("~port", "/dev/ttyUSB0") + port = rospy.get_param("~port", "/dev/ttyACM0") baud = rospy.get_param("~baud", 115200) print("port: {}, baud: {}\n".format(port, baud)) try: @@ -41,6 +41,7 @@ def talker(): joint_state_send.header = Header() joint_state_send.name = [ + "joint2_to_joint1", "joint3_to_joint2", "joint4_to_joint3", diff --git a/mycobot_320/mycobot_320_gripper_moveit/.setup_assistant b/mycobot_320/mycobot_320_gripper_moveit/.setup_assistant index 6c5202e..799978e 100644 --- a/mycobot_320/mycobot_320_gripper_moveit/.setup_assistant +++ b/mycobot_320/mycobot_320_gripper_moveit/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/firefighter.srdf CONFIG: - author_name: wwj - author_email: wwj@elephantrobot.com + author_name: Jz + author_email: Jz@elephantrobot.com generated_timestamp: 1683628879 \ No newline at end of file diff --git a/mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml b/mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml index 8b0dde7..4d8a414 100644 --- a/mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml +++ b/mycobot_320/mycobot_320_gripper_moveit/config/fake_controllers.yaml @@ -2,18 +2,25 @@ 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 + - joint1_to_base - joint2_to_joint1 - joint3_to_joint2 - joint4_to_joint3 - joint5_to_joint4 - joint6_to_joint5 - - joint6output_to_joint6 + - joint6output_to_tool1 - name: fake_gripper_group_controller type: $(arg fake_execution_type) joints: - gripper_controller -initial: # Define initial robot poses per group - - group: arm_group - pose: init_pose - - group: gripper_group - pose: init_gripper \ No newline at end of file +# initial: # Define initial robot poses per group +# - group: arm_group +# pose: init_pose +# - group: gripper_group +# pose: init_gripper \ No newline at end of file diff --git a/mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz b/mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz index df0f653..90bbe8a 100644 --- a/mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz +++ b/mycobot_320/mycobot_320_gripper_moveit/launch/moveit.rviz @@ -1,6 +1,6 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: @@ -9,7 +9,7 @@ Panels: - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5 - Tree Height: 109 + Tree Height: 200 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -45,6 +45,7 @@ Visualization Manager: - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true + JointsTab_Use_Radians: true Move Group Namespace: "" MoveIt_Allow_Approximate_IK: true MoveIt_Allow_External_Program: false @@ -52,7 +53,7 @@ Visualization Manager: MoveIt_Allow_Sensor_Positioning: false MoveIt_Planning_Attempts: 10 MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Cartesian_Path: true MoveIt_Use_Constraint_Aware_IK: true MoveIt_Workspace: Center: @@ -78,10 +79,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false gripper_base: Alpha: 1 Show Axes: false @@ -117,32 +114,42 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - link1: + joint1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link2: + joint2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link3: + joint3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link4: + joint4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link5: + joint5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link6: + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: Alpha: 1 Show Axes: false Show Trail: false @@ -168,11 +175,11 @@ Visualization Manager: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.15000000596046448 + Interactive Marker Size: 0.20000000298023224 Joint Violation Color: 255; 0; 255 Planning Group: arm_group Query Goal State: true - Query Start State: false + Query Start State: true Show Workspace: false Start State Alpha: 1 Start State Color: 0; 255; 0 @@ -198,10 +205,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false gripper_base: Alpha: 1 Show Axes: false @@ -237,32 +240,42 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - link1: + joint1: Alpha: 1 Show Axes: false Show Trail: false Value: true - link2: + joint2: Alpha: 1 Show Axes: false Show Trail: false Value: true - link3: + joint3: Alpha: 1 Show Axes: false Show Trail: false Value: true - link4: + joint4: Alpha: 1 Show Axes: false Show Trail: false Value: true - link5: + joint5: Alpha: 1 Show Axes: false Show Trail: false Value: true - link6: + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: Alpha: 1 Show Axes: false Show Trail: false @@ -314,7 +327,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2 + Distance: 1.4812843799591064 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -322,17 +335,17 @@ Visualization Manager: Value: false Field of View: 0.75 Focal Point: - X: -0.10000000149011612 - Y: 0.25 - Z: 0.30000001192092896 + X: -0.07944566011428833 + Y: 0.29104751348495483 + Z: 0.29134249687194824 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3399999737739563 + Pitch: 1.0449998378753662 Target Frame: base - Yaw: 2.9449498653411865 + Yaw: 6.149951934814453 Saved: - Class: rviz/Orbit Distance: 2 @@ -357,7 +370,7 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 919 + Height: 820 Help: collapsed: false Hide Left Dock: false @@ -366,9 +379,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000033dfc0200000007fb000000100044006900730070006c006100790073010000003d000000fe000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000141000000520000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000199000001e10000017d00ffffff000003120000033d00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002dafc0200000007fb000000100044006900730070006c006100790073010000003d00000105000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000148000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018f000001880000017d00ffffff00000539000002da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1291 - X: 454 + Width: 1842 + X: 72 Y: 27 diff --git a/mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch b/mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch index f8c672f..8da3d8a 100644 --- a/mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch +++ b/mycobot_320/mycobot_320_gripper_moveit/launch/planning_context.launch @@ -6,10 +6,12 @@ - + + - + + diff --git a/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py b/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py index 953196a..c5d83fa 100755 --- a/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py +++ b/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan.py @@ -8,7 +8,16 @@ Passable parameters: port: serial prot string. Defaults is '/dev/ttyUSB0' baud: serial prot baudrate. Defaults is 115200. """ +from socket import * +import math +import sys +import time +from multiprocessing import Lock +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot import rospy import time from sensor_msgs.msg import JointState @@ -38,17 +47,24 @@ def callback(data): def listener(): global mc global gripper_value - rospy.init_node("control_slider", anonymous=True) + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + rospy.Subscriber("joint_states", JointState, callback) - port = rospy.get_param("~port", "/dev/ttyUSB0") - baud = rospy.get_param("~baud", 115200) - print(port, baud) - mc = MyCobot(port, baud) - + # spin() simply keeps python from exiting until this node is stopped - print("spin ...") + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") rospy.spin() diff --git a/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan_630.py b/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan_630.py new file mode 100755 index 0000000..0aef4bc --- /dev/null +++ b/mycobot_320/mycobot_320_gripper_moveit/scripts/sync_plan_630.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +from socket import * +import math +import sys +import time +from multiprocessing import Lock +import numpy as np +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot + +global mc +gripper_value = [] + + + +def linear_mapping(value, input_start, input_end, output_start, output_end): + """ + Maps a value from one range to another using a linear transformation. + + Args: + value (float): The input value to be mapped. + input_start (float): The start of the input range. + input_end (float): The end of the input range. + output_start (float): The start of the output range. + output_end (float): The end of the output range. + + Returns: + float: The mapped value in the output range. + """ + # Calculate the slope of the linear function + slope = (output_end - output_start) / (input_end - input_start) + # Calculate the mapped value + mapped_value = output_start + slope * (value - input_start) + return mapped_value + + +def callback(data): + """callback function,回调函数""" + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + + data_list = [] + for index, value in enumerate(data.position): + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + # mc.write_angles(data_list, 800) + + list_i = [] + for i in data_list[:6]: + list_i.append(i) + mc.write_angles(list_i,800) + + list_j = [] + for j in data_list[6:]: + list_j.append(j) + # list_j = [int(a) for a in list_j] + + mc.set_gripper_mode(0) + # Define the input and output ranges + input_start = 17 + input_end = -40 + output_start = 100 + output_end = 0 + num = [linear_mapping(value, input_start, input_end, output_start, output_end) for value in list_j] + num2 = [int(i) for i in num] + num2 = num2[1] + mc.set_gripper_value(num2,100) + rospy.loginfo(num2) + + + + +def listener(): + global mc + global gripper_value + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/mycobot_320/mycobot_320_gripper_moveit/scripts/test.py b/mycobot_320/mycobot_320_gripper_moveit/scripts/test.py new file mode 100644 index 0000000..74686bb --- /dev/null +++ b/mycobot_320/mycobot_320_gripper_moveit/scripts/test.py @@ -0,0 +1,35 @@ +from pymycobot.elephantrobot import ElephantRobot +import time + +global mc + +mc = ElephantRobot("192.168.1.161",5001) + + + + +# 启动机器人必要指令 +mc.start_client() +time.sleep(1) +mc.set_gripper_mode(0) +time.sleep(1) + + + +# mc.power_off()#夹爪透传换IO模式时需要先关闭机器再重启机器人一次，仅使用夹爪透传模式不必关闭机器人 +# mc.state_off() +# time.sleep(3) +# mc.power_on() +# time.sleep(3) +# mc.state_on() +# time.sleep(3) +# mc.set_gripper_mode(0) +mc.set_gripper_calibrate() + +# #透传模式 + +# for i in range(3): +# mc.set_gripper_value(46, 20) +# time.sleep(1) +# mc.set_gripper_value(96, 20) +# time.sleep(1) \ No newline at end of file diff --git a/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf b/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf index 384e64e..c95ff40 100644 --- a/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf +++ b/mycobot_description/urdf/mycobot_pro_600/mycobot_pro_600_gripper.urdf @@ -291,8 +291,8 @@ - - + + @@ -310,8 +310,8 @@ - - + + @@ -330,7 +330,7 @@ - + diff --git a/mycobot_description/urdf/mycobot_pro_630/mycobot_630_gripper.urdf b/mycobot_description/urdf/mycobot_pro_630/mycobot_630_gripper.urdf index bed60e5..b42144b 100644 --- a/mycobot_description/urdf/mycobot_pro_630/mycobot_630_gripper.urdf +++ b/mycobot_description/urdf/mycobot_pro_630/mycobot_630_gripper.urdf @@ -378,39 +378,39 @@ - + - + - + - + - + - + - + @@ -420,7 +420,7 @@ - + diff --git a/mycobot_pro/mycobot_600/config/mycobot_600.rviz b/mycobot_pro/mycobot_600/config/mycobot_600.rviz index 59f0ae5..6ac6713 100644 --- a/mycobot_pro/mycobot_600/config/mycobot_600.rviz +++ b/mycobot_pro/mycobot_600/config/mycobot_600.rviz @@ -246,7 +246,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 0.6429708003997803 + Distance: 1.742307186126709 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -254,17 +254,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.05385429784655571 - Y: 0.09180382639169693 - Z: 0.726940929889679 + X: -0.07060671597719193 + Y: 0.09759367257356644 + Z: 0.5344932079315186 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.2553977966308594 + Pitch: 0.4803977310657501 Target Frame: - Yaw: 2.845386028289795 + Yaw: 3.7303872108459473 Saved: ~ Window Geometry: Displays: diff --git a/mycobot_pro/mycobot_600/launch/mycobot_600_follow_display.launch b/mycobot_pro/mycobot_600/launch/mycobot_600_follow_display.launch new file mode 100644 index 0000000..fefafc8 --- /dev/null +++ b/mycobot_pro/mycobot_600/launch/mycobot_600_follow_display.launch @@ -0,0 +1,11 @@ + + + + + + + + ["joint_states"] + + + \ No newline at end of file diff --git a/mycobot_pro/mycobot_600/launch/mycobot_600_gripper_follow_display.launch b/mycobot_pro/mycobot_600/launch/mycobot_600_gripper_follow_display.launch new file mode 100644 index 0000000..e69de29 diff --git a/mycobot_pro/mycobot_600/scripts/mycobot_600_follow_display.py b/mycobot_pro/mycobot_600/scripts/mycobot_600_follow_display.py new file mode 100644 index 0000000..fdd5423 --- /dev/null +++ b/mycobot_pro/mycobot_600/scripts/mycobot_600_follow_display.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +from cgi import print_environ +import time + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from visualization_msgs.msg import Marker + +from pymycobot.mycobot import MyCobot + + +def talker(): + rospy.init_node("display", anonymous=True) + + print("Try connect real mycobot...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + print("port: {}, baud: {}\n".format(port, baud)) + try: + mycobot = MyCobot(port, baud) + except Exception as e: + print(e) + print( + """\ + \rTry connect mycobot failed! + \rPlease check wether connected with mycobot. + \rPlease chckt wether the port or baud is right. + """ + ) + exit(1) + mycobot.release_all_servos() + time.sleep(0.1) + print("Rlease all servos over.\n") + + pub = rospy.Publisher("joint_states", JointState, queue_size=10) + pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) + rate = rospy.Rate(30) # 30hz + + # pub joint state + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + "joint6output_to_joint6", + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + marker_ = Marker() + marker_.header.frame_id = "/joint1" + marker_.ns = "my_namespace" + + print("publishing ...") + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + + angles = mycobot.get_radians() + data_list = [] + for index, value in enumerate(angles): + data_list.append(value) + + # rospy.loginfo('{}'.format(data_list)) + joint_state_send.position = data_list + + pub.publish(joint_state_send) + + coords = mycobot.get_coords() + + # marker + marker_.header.stamp = rospy.Time.now() + marker_.type = marker_.SPHERE + marker_.action = marker_.ADD + marker_.scale.x = 0.04 + marker_.scale.y = 0.04 + marker_.scale.z = 0.04 + + # marker position initial + if not coords: + coords = [0, 0, 0, 0, 0, 0] + + rospy.loginfo("error [101]: can not get coord values") + + marker_.pose.position.x = coords[1] / 1000 * -1 + marker_.pose.position.y = coords[0] / 1000 + marker_.pose.position.z = coords[2] / 1000 + + marker_.color.a = 1.0 + marker_.color.g = 1.0 + pub_marker.publish(marker_) + + rate.sleep() + + +if __name__ == "__main__": + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/mycobot_pro/mycobot_600/scripts/mycobot_600_gripper_follow_diplay.py b/mycobot_pro/mycobot_600/scripts/mycobot_600_gripper_follow_diplay.py new file mode 100644 index 0000000..e69de29 diff --git a/mycobot_pro/mycobot_600/scripts/test.py b/mycobot_pro/mycobot_600/scripts/test.py new file mode 100644 index 0000000..0996b45 --- /dev/null +++ b/mycobot_pro/mycobot_600/scripts/test.py @@ -0,0 +1,23 @@ +from pymycobot import ElephantRobot +import time +if __name__=='__main__': + # "Connect to robot server" + elephant_client = ElephantRobot("192.168.1.6", 5001) + + # "Enable TCP communication" + elephant_client.start_client() + + # "Setting" + # elephant_client.set_speed(20) # set speed(20%) + # elephant_client.set_payload(2.0) # set payload(2.0Kg) + + # "Power on the robot" + # elephant_client.power_on() + + # "Start the system" + # elephant_client.state_on() + + # "test move" + elephant_client.write_coords([-108, -140, 620, 88, -2, -37], 3000) + elephant_client.wait(3) + elephant_client.command_wait_done() diff --git a/mycobot_pro/mycobot_600_gripper_moveit/CMakeLists.txt b/mycobot_pro/mycobot_600_gripper_moveit/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/cartesian_limits.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/cartesian_limits.yaml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/chomp_planning.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/chomp_planning.yaml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml old mode 100644 new mode 100755 index 2151852..19e76bf --- a/mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/fake_controllers.yaml @@ -1,7 +1,14 @@ controller_list: - - name: fake_arm_controller + - 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 + - joint2_to_joint1 - joint3_to_joint2 - joint4_to_joint3 @@ -9,12 +16,12 @@ controller_list: - joint6_to_joint5 - joint6output_to_joint6 - joint6output_to_tool1 - - name: fake_gripper_controller + - name: fake_gripper_group_controller type: $(arg fake_execution_type) joints: - gripper_controller -initial: # Define initial robot poses per group -# - group: arm -# pose: home - - [] \ No newline at end of file +# initial: # Define initial robot poses per group +# - group: arm_group +# pose: init_pose +# - group: gripper_group +# pose: init_gripper \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf b/mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf old mode 100644 new mode 100755 index ff6cd02..bf4dda5 --- a/mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/firefighter.srdf @@ -9,30 +9,136 @@ - - - - - - - - - + + - - - - - - - - + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_controllers.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_controllers.yaml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_firefighter.urdf b/mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_firefighter.urdf deleted file mode 100644 index 98795b3..0000000 --- a/mycobot_pro/mycobot_600_gripper_moveit/config/gazebo_firefighter.urdf +++ /dev/null @@ -1,579 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 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 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - / - - - - diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/joint_limits.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/joint_limits.yaml old mode 100644 new mode 100755 index bbb8d38..17203f3 --- a/mycobot_pro/mycobot_600_gripper_moveit/config/joint_limits.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/joint_limits.yaml @@ -8,36 +8,11 @@ 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: - gripper_base_to_gripper_left2: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - gripper_base_to_gripper_right2: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - gripper_base_to_gripper_right3: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 gripper_controller: has_velocity_limits: false max_velocity: 0 has_acceleration_limits: false max_acceleration: 0 - gripper_left3_to_gripper_left1: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - gripper_right3_to_gripper_right1: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 joint2_to_joint1: has_velocity_limits: false max_velocity: 0 @@ -64,11 +39,6 @@ joint_limits: has_acceleration_limits: false max_acceleration: 0 joint6output_to_joint6: - has_velocity_limits: false - max_velocity: 0 - has_acceleration_limits: false - max_acceleration: 0 - joint6output_to_tool1: has_velocity_limits: false max_velocity: 0 has_acceleration_limits: false diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/kinematics.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/kinematics.yaml old mode 100644 new mode 100755 index d3ee3f5..05777c2 --- a/mycobot_pro/mycobot_600_gripper_moveit/config/kinematics.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/kinematics.yaml @@ -1,7 +1,10 @@ -arm: +arm_group: +# arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - goal_joint_tolerance: 0.0001 - goal_position_tolerance: 0.0001 - goal_orientation_tolerance: 0.001 \ No newline at end of file +gripper_group: +# gripper: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/ompl_planning.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/ompl_planning.yaml old mode 100644 new mode 100755 index 13ace17..59f5233 --- a/mycobot_pro/mycobot_600_gripper_moveit/config/ompl_planning.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/ompl_planning.yaml @@ -51,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 - 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 + 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 k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() PRM: type: geometric::PRM @@ -95,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 - 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 + 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 cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf LBTRRT: type: geometric::LBTRRT @@ -127,44 +127,9 @@ 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: +arm_group: +# arm: + default_planner_config: RRTConnect planner_configs: - AnytimePathShortening - SBL @@ -190,12 +155,11 @@ arm: - LazyPRMstar - SPARS - SPARStwo - - AITstar - - ABITstar - - BITstar projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2) longest_valid_segment_fraction: 0.005 -gripper: +gripper_group: +# gripper: + default_planner_config: RRTConnect planner_configs: - AnytimePathShortening - SBL @@ -221,6 +185,3 @@ gripper: - LazyPRMstar - SPARS - SPARStwo - - AITstar - - ABITstar - - BITstar diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/ros_controllers.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/ros_controllers.yaml old mode 100644 new mode 100755 index d9646fd..bee9fae --- a/mycobot_pro/mycobot_600_gripper_moveit/config/ros_controllers.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/ros_controllers.yaml @@ -1,5 +1,6 @@ -arm_controller: - type: position_controllers/JointTrajectoryController +arm_group_controller: +# arm_controller: + type: effort_controllers/JointTrajectoryController joints: - joint2_to_joint1 - joint3_to_joint2 @@ -7,7 +8,6 @@ arm_controller: - joint5_to_joint4 - joint6_to_joint5 - joint6output_to_joint6 - - joint6output_to_tool1 gains: joint2_to_joint1: p: 100 @@ -39,7 +39,13 @@ arm_controller: d: 1 i: 1 i_clamp: 1 - joint6output_to_tool1: +gripper_group_controller: +# gripper_controller: + type: effort_controllers/JointTrajectoryController + joints: + - gripper_controller + gains: + gripper_controller: p: 100 d: 1 i: 1 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/sensors_3d.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/sensors_3d.yaml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/simple_moveit_controllers.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/simple_moveit_controllers.yaml old mode 100644 new mode 100755 index 371a176..0031f9c --- a/mycobot_pro/mycobot_600_gripper_moveit/config/simple_moveit_controllers.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/simple_moveit_controllers.yaml @@ -1,5 +1,6 @@ controller_list: - - name: arm_controller + - name: arm_group_controller + # - name: arm_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: True @@ -10,4 +11,10 @@ controller_list: - joint5_to_joint4 - joint6_to_joint5 - joint6output_to_joint6 - - joint6output_to_tool1 \ No newline at end of file + - name: gripper_group_controller + # - name: gripper_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - gripper_controller \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_gripper_moveit/config/stomp_planning.yaml b/mycobot_pro/mycobot_600_gripper_moveit/config/stomp_planning.yaml old mode 100644 new mode 100755 index 7a95589..90716ca --- a/mycobot_pro/mycobot_600_gripper_moveit/config/stomp_planning.yaml +++ b/mycobot_pro/mycobot_600_gripper_moveit/config/stomp_planning.yaml @@ -1,5 +1,5 @@ -stomp/arm: - group_name: arm +stomp/arm_group: + group_name: arm_group optimization: num_timesteps: 60 num_iterations: 40 @@ -11,7 +11,7 @@ stomp/arm: task: noise_generator: - class: stomp_moveit/NormalDistributionSampling - stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] cost_functions: - class: stomp_moveit/CollisionCheck collision_penalty: 1.0 @@ -37,8 +37,8 @@ stomp/arm: publish_intermediate: True marker_topic: stomp_trajectory marker_namespace: optimized -stomp/gripper: - group_name: gripper +stomp/gripper_group: + group_name: gripper_group optimization: num_timesteps: 60 num_iterations: 40 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/chomp_planning_pipeline.launch.xml old mode 100644 new mode 100755 index 8d57b35..1a44a41 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/chomp_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/chomp_planning_pipeline.launch.xml @@ -3,8 +3,7 @@ + + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/default_warehouse_db.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/default_warehouse_db.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/demo.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/demo.launch old mode 100644 new mode 100755 index 7ff39c8..6b4e7a1 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/demo.launch +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/demo.launch @@ -24,6 +24,9 @@ + + + - - - - - - + + - - + + + + + + + + + - - + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/gazebo.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/gazebo.launch old mode 100644 new mode 100755 index fc762bc..22bfac2 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/gazebo.launch +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/gazebo.launch @@ -1,34 +1,32 @@ - - - - - - + + + - - + + + + - - + + - + - - + + - + + + + - - - - - diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/joystick_control.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/joystick_control.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/move_group.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/move_group.launch old mode 100644 new mode 100755 index 2330bcb..4ff2e39 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/move_group.launch +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/move_group.launch @@ -91,10 +91,6 @@ - - - diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit.rviz b/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit.rviz old mode 100644 new mode 100755 index 5487c94..90bbe8a --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit.rviz +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit.rviz @@ -1,12 +1,15 @@ Panels: - Class: rviz/Displays - Help Height: 84 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5 - Tree Height: 226 + Tree Height: 200 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -14,6 +17,10 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -23,7 +30,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -35,32 +42,144 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning Enabled: true + JointsTab_Use_Radians: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: true + 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: true + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: - Links: ~ - Loop Animation: true + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false Robot Alpha: 0.5 + Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false 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.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 + Interactive Marker Size: 0.20000000298023224 Joint Violation Color: 255; 0; 255 + Planning Group: arm_group Query Goal State: true - Query Start State: false + Query Start State: true Show Workspace: false Start State Alpha: 1 Start State Color: 0; 255; 0 @@ -68,19 +187,136 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: ~ + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Robot Alpha: 0.5 - Show Scene Robot: true + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz/Marker + Enabled: true + Marker Topic: visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: "" + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: "" Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base + Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -91,30 +327,50 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.0 + Distance: 1.4812843799591064 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 + X: -0.07944566011428833 + Y: 0.29104751348495483 + Z: 0.29134249687194824 Focal Shape Fixed Size: true - Focal Shape Size: 0.05 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.0449998378753662 Target Frame: base - Yaw: -0.6232355833053589 - Saved: ~ + Yaw: 6.149951934814453 + Saved: + - Class: rviz/Orbit + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3399999737739563 + Target Frame: base + Yaw: 2.9449498653411865 Window Geometry: Displays: collapsed: false - Height: 848 + Height: 820 Help: collapsed: false Hide Left Dock: false @@ -123,9 +379,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002dafc0200000007fb000000100044006900730070006c006100790073010000003d00000105000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000148000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018f000001880000017d00ffffff00000539000002da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1291 - X: 454 - Y: 25 + Width: 1842 + X: 72 + Y: 27 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit_rviz.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/moveit_rviz.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/mycobot600_gripper_moveit.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/mycobot600_gripper_moveit.launch new file mode 100755 index 0000000..6b4e7a1 --- /dev/null +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/mycobot600_gripper_moveit.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml old mode 100644 new mode 100755 index 072eab0..b078992 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -1,19 +1,18 @@ - - + + - + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl_planning_pipeline.launch.xml old mode 100644 new mode 100755 index e115c25..a976cbb --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/ompl_planning_pipeline.launch.xml @@ -2,8 +2,7 @@ + + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_context.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_context.launch old mode 100644 new mode 100755 index 6c1419f..3adf296 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_context.launch +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_context.launch @@ -6,9 +6,11 @@ + - + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/planning_pipeline.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_controllers.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_controllers.launch old mode 100644 new mode 100755 index f7fb9f9..fcbfb0d --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_controllers.launch +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/ros_controllers.launch @@ -6,6 +6,6 @@ + output="screen" args="arm_group_controller gripper_group_controller "/> diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/run_benchmark_ompl.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/run_benchmark_ompl.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/sensor_manager.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/sensor_manager.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/setup_assistant.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/setup_assistant.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/stomp_planning_pipeline.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/stomp_planning_pipeline.launch.xml old mode 100644 new mode 100755 index 4b88c85..e4f9a06 --- a/mycobot_pro/mycobot_600_gripper_moveit/launch/stomp_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_600_gripper_moveit/launch/stomp_planning_pipeline.launch.xml @@ -6,8 +6,7 @@ + + + diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/trajectory_execution.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/trajectory_execution.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse.launch b/mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse.launch old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse_settings.launch.xml b/mycobot_pro/mycobot_600_gripper_moveit/launch/warehouse_settings.launch.xml old mode 100644 new mode 100755 diff --git a/mycobot_pro/mycobot_600_gripper_moveit/package.xml b/mycobot_pro/mycobot_600_gripper_moveit/package.xml old mode 100644 new mode 100755 index cbb4c83..f28df63 --- a/mycobot_pro/mycobot_600_gripper_moveit/package.xml +++ b/mycobot_pro/mycobot_600_gripper_moveit/package.xml @@ -5,21 +5,21 @@ An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt Motion Planning Framework - aa - aa + jz + jz BSD http://moveit.ros.org/ - https://github.com/moveit/moveit/issues - https://github.com/moveit/moveit + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit catkin moveit_ros_move_group moveit_fake_controller_manager moveit_kinematics - moveit_planners + moveit_planners_ompl moveit_ros_visualization moveit_setup_assistant moveit_simple_controller_manager diff --git a/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan.py new file mode 100755 index 0000000..0aef4bc --- /dev/null +++ b/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +from socket import * +import math +import sys +import time +from multiprocessing import Lock +import numpy as np +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot + +global mc +gripper_value = [] + + + +def linear_mapping(value, input_start, input_end, output_start, output_end): + """ + Maps a value from one range to another using a linear transformation. + + Args: + value (float): The input value to be mapped. + input_start (float): The start of the input range. + input_end (float): The end of the input range. + output_start (float): The start of the output range. + output_end (float): The end of the output range. + + Returns: + float: The mapped value in the output range. + """ + # Calculate the slope of the linear function + slope = (output_end - output_start) / (input_end - input_start) + # Calculate the mapped value + mapped_value = output_start + slope * (value - input_start) + return mapped_value + + +def callback(data): + """callback function,回调函数""" + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + + data_list = [] + for index, value in enumerate(data.position): + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + # mc.write_angles(data_list, 800) + + list_i = [] + for i in data_list[:6]: + list_i.append(i) + mc.write_angles(list_i,800) + + list_j = [] + for j in data_list[6:]: + list_j.append(j) + # list_j = [int(a) for a in list_j] + + mc.set_gripper_mode(0) + # Define the input and output ranges + input_start = 17 + input_end = -40 + output_start = 100 + output_end = 0 + num = [linear_mapping(value, input_start, input_end, output_start, output_end) for value in list_j] + num2 = [int(i) for i in num] + num2 = num2[1] + mc.set_gripper_value(num2,100) + rospy.loginfo(num2) + + + + +def listener(): + global mc + global gripper_value + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan_320.py b/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan_320.py new file mode 100755 index 0000000..c9783de --- /dev/null +++ b/mycobot_pro/mycobot_600_gripper_moveit/scripts/sync_plan_320.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +# -*- coding:utf-8 -*- +"""[summary] +This file obtains the joint angle of the manipulator in ROS, +and then sends it directly to the real manipulator using `pymycobot` API. +This file is [slider_control.launch] related script. +Passable parameters: + port: serial prot string. Defaults is '/dev/ttyUSB0' + baud: serial prot baudrate. Defaults is 115200. +""" +from socket import * +import math +import sys +import time +from multiprocessing import Lock + +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot +import rospy +import time +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +mc = None +gripper_value = [] + +def callback(data): + global mc + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + data_list = [] + for index, value in enumerate(data.position): + data_list.append(round(value, 3)) + + data_list = data_list[:7] + print("radians:%s"%data_list[:6]) + mc.send_radians(data_list[:6], 80) + gripper_value = int(abs(-0.7-data_list[6])* 117) + print("gripper_value:%s"%gripper_value) + # mc.set_gripper_mode(0) + # time.sleep(1) + mc.set_gripper_value(gripper_value, 100) + +def listener(): + # global mc + # global gripper_value + + # rospy.init_node("control_slider", anonymous=True) + + # rospy.Subscriber("joint_states", JointState, callback) + # port = rospy.get_param("~port", "/dev/ttyUSB0") + # baud = rospy.get_param("~baud", 115200) + # print(port, baud) + # mc = MyCobot(port, baud) + + # # spin() simply keeps python from exiting until this node is stopped + # print("spin ...") + # rospy.spin() + + + global mc + global gripper_value + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() \ No newline at end of file diff --git a/mycobot_pro/mycobot_600_gripper_moveit/scripts/test.py b/mycobot_pro/mycobot_600_gripper_moveit/scripts/test.py new file mode 100755 index 0000000..25f6c40 --- /dev/null +++ b/mycobot_pro/mycobot_600_gripper_moveit/scripts/test.py @@ -0,0 +1,37 @@ +from pymycobot import ElephantRobot +import time + +global mc + +mc = ElephantRobot("192.168.1.6",5001) + + + + + +# 启动机器人必要指令 +mc.start_client() +time.sleep(1) +# mc.set_gripper_calibrate() +# time.sleep(1) +# mc.set_gripper_mode(0) +# time.sleep(1) + + + +# mc.power_off()#夹爪透传换IO模式时需要先关闭机器再重启机器人一次，仅使用夹爪透传模式不必关闭机器人 +mc.state_off() +time.sleep(3) +mc.power_on() +time.sleep(3) +mc.state_on() +time.sleep(3) +mc.set_gripper_mode(0) + +#透传模式 + +for i in range(3): + mc.set_gripper_value(10, 20) + time.sleep(1) + mc.set_gripper_value(96, 20) + time.sleep(1) \ No newline at end of file diff --git a/mycobot_pro/mycobot_630/launch/mycobot_630_follow_display.launch b/mycobot_pro/mycobot_630/launch/mycobot_630_follow_display.launch new file mode 100644 index 0000000..2a336fd --- /dev/null +++ b/mycobot_pro/mycobot_630/launch/mycobot_630_follow_display.launch @@ -0,0 +1,11 @@ + + + + + + + + ["joint_states"] + + + \ No newline at end of file diff --git a/mycobot_pro/mycobot_630/scripts/mycobot_630_follow_display.py b/mycobot_pro/mycobot_630/scripts/mycobot_630_follow_display.py new file mode 100644 index 0000000..7044f1d --- /dev/null +++ b/mycobot_pro/mycobot_630/scripts/mycobot_630_follow_display.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python2 +# -*- coding:utf-8 -*- +from cgi import print_environ +import time + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from visualization_msgs.msg import Marker + +from pymycobot.mycobot import MyCobot + + +def talker(): + rospy.init_node("display", anonymous=True) + + print("Try connect real mycobot...") + port = rospy.get_param("~port", "/dev/ttyUSB0") + baud = rospy.get_param("~baud", 115200) + print("port: {}, baud: {}\n".format(port, baud)) + try: + mycobot = MyCobot(port, baud) + except Exception as e: + print(e) + print( + """\ + \rTry connect mycobot failed! + \rPlease check wether connected with mycobot. + \rPlease chckt wether the port or baud is right. + """ + ) + exit(1) + mycobot.release_all_servos() + time.sleep(0.1) + print("Rlease all servos over.\n") + + pub = rospy.Publisher("joint_states", JointState, queue_size=10) + pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) + rate = rospy.Rate(30) # 30hz + + # pub joint state + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint1_to_base", + "joint2_to_joint1", + "joint3_to_joint2", + "joint4_to_joint3", + "joint5_to_joint4", + "joint6_to_joint5", + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + marker_ = Marker() + marker_.header.frame_id = "/joint1" + marker_.ns = "my_namespace" + + print("publishing ...") + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + + angles = mycobot.get_radians() + data_list = [] + for index, value in enumerate(angles): + data_list.append(value) + + # rospy.loginfo('{}'.format(data_list)) + joint_state_send.position = data_list + + pub.publish(joint_state_send) + + coords = mycobot.get_coords() + + # marker + marker_.header.stamp = rospy.Time.now() + marker_.type = marker_.SPHERE + marker_.action = marker_.ADD + marker_.scale.x = 0.04 + marker_.scale.y = 0.04 + marker_.scale.z = 0.04 + + # marker position initial + if not coords: + coords = [0, 0, 0, 0, 0, 0] + + rospy.loginfo("error [101]: can not get coord values") + + marker_.pose.position.x = coords[1] / 1000 * -1 + marker_.pose.position.y = coords[0] / 1000 + marker_.pose.position.z = coords[2] / 1000 + + marker_.color.a = 1.0 + marker_.color.g = 1.0 + pub_marker.publish(marker_) + + rate.sleep() + + +if __name__ == "__main__": + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/mycobot_pro/mycobot_630_gripper_moveit/config/fake_controllers.yaml b/mycobot_pro/mycobot_630_gripper_moveit/config/fake_controllers.yaml index b74c668..e957dff 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/config/fake_controllers.yaml +++ b/mycobot_pro/mycobot_630_gripper_moveit/config/fake_controllers.yaml @@ -1,6 +1,6 @@ controller_list: - name: fake_arm_controller - type: $(arg fake_execution_type) + # type: $(arg fake_execution_type) joints: - joint1_to_base - joint2_to_joint1 @@ -10,7 +10,7 @@ controller_list: - joint6_to_joint5 - joint6output_to_tool1 - name: fake_gripper_controller - type: $(arg fake_execution_type) + # type: $(arg fake_execution_type) joints: - gripper_controller initial: # Define initial robot poses per group diff --git a/mycobot_pro/mycobot_630_gripper_moveit/config/firefighter.srdf b/mycobot_pro/mycobot_630_gripper_moveit/config/firefighter.srdf index 21d516a..7f01618 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/config/firefighter.srdf +++ b/mycobot_pro/mycobot_630_gripper_moveit/config/firefighter.srdf @@ -9,7 +9,7 @@ - + + + + + + + + + + + + + + + + + - + + + + + + + diff --git a/mycobot_pro/mycobot_630_gripper_moveit/config/kinematics.yaml b/mycobot_pro/mycobot_630_gripper_moveit/config/kinematics.yaml index d3ee3f5..0417636 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/config/kinematics.yaml +++ b/mycobot_pro/mycobot_630_gripper_moveit/config/kinematics.yaml @@ -2,6 +2,7 @@ arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + # kinematics_solver_attempts: 3 goal_joint_tolerance: 0.0001 goal_position_tolerance: 0.0001 goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/mycobot_pro/mycobot_630_gripper_moveit/config/ompl_planning.yaml b/mycobot_pro/mycobot_630_gripper_moveit/config/ompl_planning.yaml index a5b6be6..be72f98 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/config/ompl_planning.yaml +++ b/mycobot_pro/mycobot_630_gripper_moveit/config/ompl_planning.yaml @@ -43,6 +43,7 @@ planner_configs: range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + planning_attempts: 3 TRRT: type: geometric::TRRT range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() diff --git a/mycobot_pro/mycobot_630_gripper_moveit/launch/moveit.rviz b/mycobot_pro/mycobot_630_gripper_moveit/launch/moveit.rviz index e81e37f..a7e44e5 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/launch/moveit.rviz +++ b/mycobot_pro/mycobot_630_gripper_moveit/launch/moveit.rviz @@ -7,7 +7,7 @@ Panels: - /MotionPlanning1 - /MotionPlanning1/Planning Request1 Splitter Ratio: 0.5 - Tree Height: 388 + Tree Height: 200 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -40,18 +40,18 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 + - Acceleration_Scaling_Factor: 1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true JointsTab_Use_Radians: true Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_Approximate_IK: true 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: true + MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: true MoveIt_Workspace: Center: @@ -177,10 +177,10 @@ Visualization Manager: Joint Violation Color: 255; 0; 255 Planning Group: arm Query Goal State: true - Query Start State: false + Query Start State: true Show Workspace: false Start State Alpha: 1 - Start State Color: 0; 255; 0 + Start State Color: 115; 210; 22 Planning Scene Topic: move_group/monitored_planning_scene Robot Description: robot_description Scene Geometry: @@ -280,9 +280,9 @@ Visualization Manager: Value: true Robot Alpha: 0.5 Show Robot Collision: false - Show Robot Visual: true + Show Robot Visual: false Value: true - Velocity_Scaling_Factor: 0.1 + Velocity_Scaling_Factor: 1 - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -411,14 +411,14 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5400000810623169 + Pitch: 0.4850001335144043 Target Frame: base - Yaw: 4.9563188552856445 + Yaw: 6.006320953369141 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 820 Help: collapsed: false Hide Left Dock: false @@ -427,9 +427,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001c1000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000204000001d70000017d00ffffff0000053f0000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002dafc0200000007fb000000100044006900730070006c006100790073010000003d00000105000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000148000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018f000001880000017d00ffffff00000539000002da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1848 + Width: 1842 X: 72 Y: 27 diff --git a/mycobot_pro/mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_pro/mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml index 7ff49cf..8204a1b 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml +++ b/mycobot_pro/mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml @@ -20,5 +20,7 @@ + + diff --git a/mycobot_pro/mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch b/mycobot_pro/mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch index 891dff4..1490088 100644 --- a/mycobot_pro/mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch +++ b/mycobot_pro/mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch @@ -16,6 +16,8 @@ + + diff --git a/mycobot_pro/mycobot_630_gripper_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_630_gripper_moveit/scripts/sync_plan.py new file mode 100755 index 0000000..d376b72 --- /dev/null +++ b/mycobot_pro/mycobot_630_gripper_moveit/scripts/sync_plan.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +from socket import * +import math +import sys +import time +from multiprocessing import Lock + +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot + +global mc + + +def callback(data): + """callback function,回调函数""" + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + + data_list = [] + for index, value in enumerate(data.position): + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + # mc.write_angles(data_list, 800) + + for joint,angle in enumerate(data_list[:6]): + mc.write_angle(joint,angle,800) + + for gripper_angle in data_list[6:]: + mc.set_cag_gripper_value(gripper_angle,800) + + + + # excepted_joint_count = 13 + # if len(data_list) == excepted_joint_count: + # mc.write_angles(data_list, 800) + # else: + # rospy.logwarn("关节数量不匹配，期望数量是%d,实际数量是%d",excepted_joint_count,len(data_list)) + + +def listener(): + global mc + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml index 06e4891..3280efc 100644 --- a/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml +++ b/mycobot_pro/mycobot_630_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + diff --git a/mycobot_pro/mycobot_630_moveit/launch/firefighter_moveit_controller_manager.launch.xml b/mycobot_pro/mycobot_630_moveit/launch/firefighter_moveit_controller_manager.launch.xml index f8e427e..ebfd8c9 100644 --- a/mycobot_pro/mycobot_630_moveit/launch/firefighter_moveit_controller_manager.launch.xml +++ b/mycobot_pro/mycobot_630_moveit/launch/firefighter_moveit_controller_manager.launch.xml @@ -7,4 +7,5 @@ + diff --git a/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz b/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz index aa25b44..e33f20e 100644 --- a/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz +++ b/mycobot_pro/mycobot_630_moveit/launch/moveit.rviz @@ -5,14 +5,13 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Status1 - /MotionPlanning1/Scene Geometry1 - /MotionPlanning1/Scene Robot1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 - /RobotModel1 Splitter Ratio: 0.4869215190410614 - Tree Height: 274 + Tree Height: 396 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -82,6 +81,41 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -112,6 +146,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Loop Animation: false Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -133,9 +177,9 @@ Visualization Manager: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.10000000149011612 + Interactive Marker Size: 0.20000000298023224 Joint Violation Color: 255; 0; 255 - Planning Group: arm_group + Planning Group: arm Query Goal State: true Query Start State: true Show Workspace: false @@ -163,6 +207,41 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -193,6 +272,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: false @@ -224,6 +313,41 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -254,6 +378,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Robot Alpha: 1 Show Robot Collision: false Show Robot Visual: true @@ -273,6 +407,41 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true joint1: Alpha: 1 Show Axes: false @@ -303,6 +472,16 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -333,17 +512,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -0.06067162752151489 - Y: 0.045795738697052 + X: 0.041976600885391235 + Y: -0.019326984882354736 Z: 1.490174383889098e-08 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.25979647040367126 + Pitch: 0.5597964525222778 Target Frame: base - Yaw: 3.2899534702301025 + Yaw: 6.1299567222595215 Saved: ~ Window Geometry: Displays: @@ -357,7 +536,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d0000014f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000192000000530000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001eb000001f00000018800ffffff0000053f0000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001c9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c0069006400650072010000020c000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000018800ffffff0000053f0000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1848 diff --git a/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch b/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch index 027d56a..99bf987 100644 --- a/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch +++ b/mycobot_pro/mycobot_630_moveit/launch/planning_context.launch @@ -6,24 +6,28 @@ - + - + + + + diff --git a/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py b/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py index 2b918e7..c9c7f13 100755 --- a/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py +++ b/mycobot_pro/mycobot_630_moveit/scripts/sync_plan.py @@ -31,7 +31,7 @@ def listener(): global mc rospy.init_node("control_slider", anonymous=True) - ip = rospy.get_param("~ip", "192.168.1.159") + ip = rospy.get_param("~ip", "192.168.1.161") port = rospy.get_param("~port", 5001) print (ip, port) mc = ElephantRobot(ip, int(port)) diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/CMakeLists.txt b/mycobot_pro/new_mycobot_630_gripper_moveit/CMakeLists.txt new file mode 100755 index 0000000..0138f48 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(new_mycobot_630_gripper_moveit) + +find_package(catkin REQUIRED) + +catkin_package() + +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/new_mycobot_630_gripper_moveit/config/cartesian_limits.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/cartesian_limits.yaml new file mode 100755 index 0000000..7df72f6 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_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/new_mycobot_630_gripper_moveit/config/chomp_planning.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/chomp_planning.yaml new file mode 100755 index 0000000..eb9c912 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/fake_controllers.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/fake_controllers.yaml new file mode 100755 index 0000000..4d8a414 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/fake_controllers.yaml @@ -0,0 +1,26 @@ +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 + - joint1_to_base + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_tool1 + - name: fake_gripper_group_controller + type: $(arg fake_execution_type) + joints: + - gripper_controller +# initial: # Define initial robot poses per group +# - group: arm_group +# pose: init_pose +# - group: gripper_group +# pose: init_gripper \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/firefighter.srdf b/mycobot_pro/new_mycobot_630_gripper_moveit/config/firefighter.srdf new file mode 100644 index 0000000..7f01618 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/firefighter.srdf @@ -0,0 +1,225 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/gazebo_controllers.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/gazebo_controllers.yaml new file mode 100755 index 0000000..e4d2eb0 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_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/new_mycobot_630_gripper_moveit/config/joint_limits.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/joint_limits.yaml new file mode 100755 index 0000000..17203f3 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/joint_limits.yaml @@ -0,0 +1,45 @@ +# 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: + gripper_controller: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint2_to_joint1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint3_to_joint2: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint4_to_joint3: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint5_to_joint4: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6_to_joint5: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6output_to_joint6: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/kinematics.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/kinematics.yaml new file mode 100755 index 0000000..05777c2 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/kinematics.yaml @@ -0,0 +1,10 @@ +arm_group: +# arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 +gripper_group: +# gripper: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/ompl_planning.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/ompl_planning.yaml new file mode 100755 index 0000000..59f5233 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/ompl_planning.yaml @@ -0,0 +1,187 @@ +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() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + 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 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + 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 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + 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: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + 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 +arm_group: +# arm: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2) + longest_valid_segment_fraction: 0.005 +gripper_group: +# gripper: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/ros_controllers.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/ros_controllers.yaml new file mode 100755 index 0000000..bee9fae --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/ros_controllers.yaml @@ -0,0 +1,52 @@ +arm_group_controller: +# arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - joint2_to_joint1 + - joint3_to_joint2 + - joint4_to_joint3 + - joint5_to_joint4 + - joint6_to_joint5 + - joint6output_to_joint6 + 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 +gripper_group_controller: +# gripper_controller: + type: effort_controllers/JointTrajectoryController + joints: + - gripper_controller + gains: + gripper_controller: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/sensors_3d.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/sensors_3d.yaml new file mode 100755 index 0000000..51010a3 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/simple_moveit_controllers.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/simple_moveit_controllers.yaml new file mode 100755 index 0000000..0031f9c --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/simple_moveit_controllers.yaml @@ -0,0 +1,20 @@ +controller_list: + - name: arm_group_controller + # - name: arm_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 + - name: gripper_group_controller + # - name: gripper_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - gripper_controller \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/config/stomp_planning.yaml b/mycobot_pro/new_mycobot_630_gripper_moveit/config/stomp_planning.yaml new file mode 100755 index 0000000..90716ca --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/config/stomp_planning.yaml @@ -0,0 +1,78 @@ +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 +stomp/gripper_group: + group_name: gripper_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] + 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/new_mycobot_630_gripper_moveit/launch/chomp_planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/chomp_planning_pipeline.launch.xml new file mode 100755 index 0000000..1969861 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/default_warehouse_db.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/default_warehouse_db.launch new file mode 100755 index 0000000..efe2b55 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo.launch new file mode 100755 index 0000000..344254c --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo_gazebo.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo_gazebo.launch new file mode 100755 index 0000000..a9f320c --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml new file mode 100755 index 0000000..faf051e --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml new file mode 100755 index 0000000..5d02698 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/firefighter_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/gazebo.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/gazebo.launch new file mode 100755 index 0000000..5db1141 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/gazebo.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/joystick_control.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/joystick_control.launch new file mode 100755 index 0000000..9411f6e --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/move_group.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/move_group.launch new file mode 100755 index 0000000..4ff2e39 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/move_group.launch @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit.rviz b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit.rviz new file mode 100755 index 0000000..6fe812f --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit.rviz @@ -0,0 +1,387 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Scene Robot1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.5 + Tree Height: 200 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + JointsTab_Use_Radians: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: true + 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: true + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + 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.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.20000000298023224 + Joint Violation Color: 255; 0; 255 + Planning Group: arm_group + Query Goal State: true + Query Start State: true + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + joint6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + - Class: rviz/Marker + Enabled: true + Marker Topic: visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: "" + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: "" + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.4812843799591064 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: 0.25592494010925293 + Y: 0.2098711133003235 + Z: 0.08102890849113464 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5900000929832458 + Target Frame: base + Yaw: 6.124949932098389 + Saved: + - Class: rviz/Orbit + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3399999737739563 + Target Frame: base + Yaw: 2.9449498653411865 +Window Geometry: + Displays: + collapsed: false + Height: 820 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002dafc0200000007fb000000100044006900730070006c006100790073010000003d00000105000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000148000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000018f000001880000018800ffffff00000539000002da00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1842 + X: 72 + Y: 27 diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit_rviz.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit_rviz.launch new file mode 100755 index 0000000..a4605c0 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/mycobot630_gripper_moveit.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/mycobot630_gripper_moveit.launch new file mode 100755 index 0000000..344254c --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/mycobot630_gripper_moveit.launch @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100755 index 0000000..b078992 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml new file mode 100755 index 0000000..002b2d1 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100755 index 0000000..c7c4cf5 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_context.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_context.launch new file mode 100755 index 0000000..f482fa9 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_context.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_pipeline.launch.xml new file mode 100755 index 0000000..4b4d0d6 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100755 index 0000000..9ebc91c --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_controllers.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_controllers.launch new file mode 100755 index 0000000..9ea2c8f --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch new file mode 100755 index 0000000..a61f47b --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/sensor_manager.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/sensor_manager.launch.xml new file mode 100755 index 0000000..28bcecf --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/setup_assistant.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/setup_assistant.launch new file mode 100755 index 0000000..6c6e06a --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml new file mode 100755 index 0000000..3e9b2b5 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/stomp_planning_pipeline.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/stomp_planning_pipeline.launch.xml new file mode 100755 index 0000000..45e6bc5 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/trajectory_execution.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/trajectory_execution.launch.xml new file mode 100755 index 0000000..20c3dfc --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/trajectory_execution.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse.launch b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse.launch new file mode 100755 index 0000000..0712e67 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse_settings.launch.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse_settings.launch.xml new file mode 100755 index 0000000..e473b08 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/package.xml b/mycobot_pro/new_mycobot_630_gripper_moveit/package.xml new file mode 100755 index 0000000..de76c61 --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/package.xml @@ -0,0 +1,41 @@ + + + new_mycobot_630_gripper_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 + + wwj + wwj + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + mycobot_description + + + diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan.py b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan.py new file mode 100755 index 0000000..0aef4bc --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +from socket import * +import math +import sys +import time +from multiprocessing import Lock +import numpy as np +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot + +global mc +gripper_value = [] + + + +def linear_mapping(value, input_start, input_end, output_start, output_end): + """ + Maps a value from one range to another using a linear transformation. + + Args: + value (float): The input value to be mapped. + input_start (float): The start of the input range. + input_end (float): The end of the input range. + output_start (float): The start of the output range. + output_end (float): The end of the output range. + + Returns: + float: The mapped value in the output range. + """ + # Calculate the slope of the linear function + slope = (output_end - output_start) / (input_end - input_start) + # Calculate the mapped value + mapped_value = output_start + slope * (value - input_start) + return mapped_value + + +def callback(data): + """callback function,回调函数""" + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + + data_list = [] + for index, value in enumerate(data.position): + radians_to_angles = round(math.degrees(value), 2) + data_list.append(radians_to_angles) + + rospy.loginfo(rospy.get_caller_id() + "%s", data_list) + # mc.write_angles(data_list, 800) + + list_i = [] + for i in data_list[:6]: + list_i.append(i) + mc.write_angles(list_i,800) + + list_j = [] + for j in data_list[6:]: + list_j.append(j) + # list_j = [int(a) for a in list_j] + + mc.set_gripper_mode(0) + # Define the input and output ranges + input_start = 17 + input_end = -40 + output_start = 100 + output_end = 0 + num = [linear_mapping(value, input_start, input_end, output_start, output_end) for value in list_j] + num2 = [int(i) for i in num] + num2 = num2[1] + mc.set_gripper_value(num2,100) + rospy.loginfo(num2) + + + + +def listener(): + global mc + global gripper_value + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan_320.py b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan_320.py new file mode 100755 index 0000000..c9783de --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/sync_plan_320.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +# -*- coding:utf-8 -*- +"""[summary] +This file obtains the joint angle of the manipulator in ROS, +and then sends it directly to the real manipulator using `pymycobot` API. +This file is [slider_control.launch] related script. +Passable parameters: + port: serial prot string. Defaults is '/dev/ttyUSB0' + baud: serial prot baudrate. Defaults is 115200. +""" +from socket import * +import math +import sys +import time +from multiprocessing import Lock + +import rospy +from sensor_msgs.msg import JointState + +from pymycobot.elephantrobot import ElephantRobot +import rospy +import time +from sensor_msgs.msg import JointState + +from pymycobot.mycobot import MyCobot + + +mc = None +gripper_value = [] + +def callback(data): + global mc + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + data_list = [] + for index, value in enumerate(data.position): + data_list.append(round(value, 3)) + + data_list = data_list[:7] + print("radians:%s"%data_list[:6]) + mc.send_radians(data_list[:6], 80) + gripper_value = int(abs(-0.7-data_list[6])* 117) + print("gripper_value:%s"%gripper_value) + # mc.set_gripper_mode(0) + # time.sleep(1) + mc.set_gripper_value(gripper_value, 100) + +def listener(): + # global mc + # global gripper_value + + # rospy.init_node("control_slider", anonymous=True) + + # rospy.Subscriber("joint_states", JointState, callback) + # port = rospy.get_param("~port", "/dev/ttyUSB0") + # baud = rospy.get_param("~baud", 115200) + # print(port, baud) + # mc = MyCobot(port, baud) + + # # spin() simply keeps python from exiting until this node is stopped + # print("spin ...") + # rospy.spin() + + + global mc + global gripper_value + rospy.init_node("control_slider", anonymous=True) + + ip = rospy.get_param("~ip", "192.168.1.161") + port = rospy.get_param("~port", 5001) + print (ip, port) + mc = ElephantRobot(ip, int(port)) + # START CLIENT,启动客户端 + res = mc.start_client() + if res != "": + sys.exit(1) + + mc.set_speed(90) + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + # spin()只是阻止python退出，直到该节点停止 + print ("sping ...") + rospy.spin() + + +if __name__ == "__main__": + listener() \ No newline at end of file diff --git a/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/test.py b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/test.py new file mode 100755 index 0000000..74686bb --- /dev/null +++ b/mycobot_pro/new_mycobot_630_gripper_moveit/scripts/test.py @@ -0,0 +1,35 @@ +from pymycobot.elephantrobot import ElephantRobot +import time + +global mc + +mc = ElephantRobot("192.168.1.161",5001) + + + + +# 启动机器人必要指令 +mc.start_client() +time.sleep(1) +mc.set_gripper_mode(0) +time.sleep(1) + + + +# mc.power_off()#夹爪透传换IO模式时需要先关闭机器再重启机器人一次，仅使用夹爪透传模式不必关闭机器人 +# mc.state_off() +# time.sleep(3) +# mc.power_on() +# time.sleep(3) +# mc.state_on() +# time.sleep(3) +# mc.set_gripper_mode(0) +mc.set_gripper_calibrate() + +# #透传模式 + +# for i in range(3): +# mc.set_gripper_value(46, 20) +# time.sleep(1) +# mc.set_gripper_value(96, 20) +# time.sleep(1) \ No newline at end of file