From 4b70ba8ac005f5e2d5399640f3d66fafdf501896 Mon Sep 17 00:00:00 2001 From: weijian Date: Wed, 31 Aug 2022 15:21:09 +0800 Subject: [PATCH 1/5] add mybuddy_socket ros --- Mybuddy/mybuddy_socket/CMakeLists.txt | 36 ++ Mybuddy/mybuddy_socket/LICENSE | 25 + Mybuddy/mybuddy_socket/config/dual_arm.rviz | 269 ++++++++++ Mybuddy/mybuddy_socket/config/mycobot.rviz | 203 ++++++++ .../config/mycobot_with_marker.rviz | 216 ++++++++ .../launch/mybuddy_follow.launch | 11 + .../mybuddy_socket/launch/simple_gui.launch | 22 + .../launch/slider_control.launch | 24 + .../launch/teleop_keyboard.launch | 21 + Mybuddy/mybuddy_socket/launch/test.launch | 18 + Mybuddy/mybuddy_socket/package.xml | 47 ++ .../mybuddy_socket/scripts/follow_display.py | 140 ++++++ Mybuddy/mybuddy_socket/scripts/listen_real.py | 78 +++ .../scripts/listen_real_of_topic.py | 63 +++ Mybuddy/mybuddy_socket/scripts/simple_gui.py | 474 ++++++++++++++++++ .../mybuddy_socket/scripts/slider_control.py | 67 +++ .../mybuddy_socket/scripts/teleop_keyboard.py | 172 +++++++ .../mybuddy_socket/scripts/test_control.py | 44 ++ Mybuddy/mybuddy_socket/src/camera_display.cpp | 29 ++ Mybuddy/mybuddy_socket/src/opencv_camera.cpp | 59 +++ .../mybuddy_socket_moveit/.setup_assistant | 11 + Mybuddy/mybuddy_socket_moveit/CMakeLists.txt | 10 + .../config/cartesian_limits.yaml | 5 + .../config/chomp_planning.yaml | 18 + .../config/fake_controllers.yaml | 32 ++ .../config/firefighter.srdf | 96 ++++ .../config/gazebo_controllers.yaml | 4 + .../config/joint_limits.yaml | 75 +++ .../config/kinematics.yaml | 12 + .../config/ompl_planning.yaml | 214 ++++++++ .../config/ros_controllers.yaml | 102 ++++ .../config/sensors_3d.yaml | 2 + .../config/simple_moveit_controllers.yaml | 31 ++ .../config/stomp_planning.yaml | 117 +++++ .../launch/chomp_planning_pipeline.launch.xml | 23 + .../launch/default_warehouse_db.launch | 15 + .../mybuddy_socket_moveit/launch/demo.launch | 70 +++ .../launch/demo_gazebo.launch | 21 + .../fake_moveit_controller_manager.launch.xml | 12 + ...ghter_moveit_controller_manager.launch.xml | 10 + ...refighter_moveit_sensor_manager.launch.xml | 3 + .../launch/gazebo.launch | 32 ++ .../launch/joystick_control.launch | 17 + .../launch/move_group.launch | 101 ++++ .../mybuddy_socket_moveit/launch/moveit.rviz | 319 ++++++++++++ .../launch/moveit_rviz.launch | 15 + .../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 | 26 + .../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 + Mybuddy/mybuddy_socket_moveit/package.xml | 41 ++ .../scripts/sync_plan.py | 49 ++ 63 files changed, 3727 insertions(+) create mode 100644 Mybuddy/mybuddy_socket/CMakeLists.txt create mode 100644 Mybuddy/mybuddy_socket/LICENSE create mode 100644 Mybuddy/mybuddy_socket/config/dual_arm.rviz create mode 100644 Mybuddy/mybuddy_socket/config/mycobot.rviz create mode 100644 Mybuddy/mybuddy_socket/config/mycobot_with_marker.rviz create mode 100644 Mybuddy/mybuddy_socket/launch/mybuddy_follow.launch create mode 100644 Mybuddy/mybuddy_socket/launch/simple_gui.launch create mode 100644 Mybuddy/mybuddy_socket/launch/slider_control.launch create mode 100644 Mybuddy/mybuddy_socket/launch/teleop_keyboard.launch create mode 100644 Mybuddy/mybuddy_socket/launch/test.launch create mode 100644 Mybuddy/mybuddy_socket/package.xml create mode 100755 Mybuddy/mybuddy_socket/scripts/follow_display.py create mode 100644 Mybuddy/mybuddy_socket/scripts/listen_real.py create mode 100644 Mybuddy/mybuddy_socket/scripts/listen_real_of_topic.py create mode 100644 Mybuddy/mybuddy_socket/scripts/simple_gui.py create mode 100755 Mybuddy/mybuddy_socket/scripts/slider_control.py create mode 100644 Mybuddy/mybuddy_socket/scripts/teleop_keyboard.py create mode 100644 Mybuddy/mybuddy_socket/scripts/test_control.py create mode 100644 Mybuddy/mybuddy_socket/src/camera_display.cpp create mode 100644 Mybuddy/mybuddy_socket/src/opencv_camera.cpp create mode 100644 Mybuddy/mybuddy_socket_moveit/.setup_assistant create mode 100644 Mybuddy/mybuddy_socket_moveit/CMakeLists.txt create mode 100644 Mybuddy/mybuddy_socket_moveit/config/cartesian_limits.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/chomp_planning.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/fake_controllers.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/firefighter.srdf create mode 100644 Mybuddy/mybuddy_socket_moveit/config/gazebo_controllers.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/joint_limits.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/kinematics.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/ompl_planning.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/ros_controllers.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/sensors_3d.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/simple_moveit_controllers.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/config/stomp_planning.yaml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/chomp_planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/default_warehouse_db.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/demo.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/demo_gazebo.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/fake_moveit_controller_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_controller_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_sensor_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/gazebo.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/joystick_control.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/move_group.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/moveit_rviz.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/ompl-chomp_planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/ompl_planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/planning_context.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/ros_control_moveit_controller_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/ros_controllers.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/run_benchmark_ompl.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/sensor_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/setup_assistant.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/simple_moveit_controller_manager.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/stomp_planning_pipeline.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/trajectory_execution.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/warehouse.launch create mode 100644 Mybuddy/mybuddy_socket_moveit/launch/warehouse_settings.launch.xml create mode 100644 Mybuddy/mybuddy_socket_moveit/package.xml create mode 100755 Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py diff --git a/Mybuddy/mybuddy_socket/CMakeLists.txt b/Mybuddy/mybuddy_socket/CMakeLists.txt new file mode 100644 index 0000000..6d892b8 --- /dev/null +++ b/Mybuddy/mybuddy_socket/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 2.8.3) +project(mybuddy_socket) +add_compile_options(-std=c++11) + +## Find catkin and any catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + actionlib + + +) + +## Declare a catkin package +catkin_package( + CATKIN_DEPENDS std_msgs actionlib +) + +## Build talker and listener +include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) + +catkin_install_python(PROGRAMS + scripts/follow_display.py + scripts/slider_control.py + scripts/teleop_keyboard.py + scripts/listen_real.py + scripts/listen_real_of_topic.py + scripts/simple_gui.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/Mybuddy/mybuddy_socket/LICENSE b/Mybuddy/mybuddy_socket/LICENSE new file mode 100644 index 0000000..b8468e6 --- /dev/null +++ b/Mybuddy/mybuddy_socket/LICENSE @@ -0,0 +1,25 @@ +BSD 2-Clause License + +Copyright (c) 2020, Elephant Robotics +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/Mybuddy/mybuddy_socket/config/dual_arm.rviz b/Mybuddy/mybuddy_socket/config/dual_arm.rviz new file mode 100644 index 0000000..38df910 --- /dev/null +++ b/Mybuddy/mybuddy_socket/config/dual_arm.rviz @@ -0,0 +1,269 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Axes1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 607 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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.0299999993 + 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 + - Class: rviz/Axes + Enabled: true + Length: 0.100000001 + Name: Axes + Radius: 0.00999999978 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + 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 + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + link1: + Value: true + link1_L: + Value: true + link1_R: + Value: true + link2_L: + Value: true + link2_R: + Value: true + link3_L: + Value: true + link3_R: + Value: true + link4_L: + Value: true + link4_R: + Value: true + link5_L: + Value: true + link5_R: + Value: true + link6_L: + Value: true + link6_R: + Value: true + Marker Scale: 0.2 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base: + link1: + link1_L: + link2_L: + link3_L: + link4_L: + link5_L: + link6_L: + {} + link1_R: + link2_R: + link3_R: + link4_R: + link5_R: + link6_R: + {} + Update Interval: 0 + 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 + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.71708035 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.219795272 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.67218757 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 888 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a000002eefc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002ee000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002eefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002ee000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006450000003efc0100000002fb0000000800540069006d00650100000000000006450000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004d5000002ee00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1605 + X: 290 + Y: 26 diff --git a/Mybuddy/mybuddy_socket/config/mycobot.rviz b/Mybuddy/mybuddy_socket/config/mycobot.rviz new file mode 100644 index 0000000..51ba33e --- /dev/null +++ b/Mybuddy/mybuddy_socket/config/mycobot.rviz @@ -0,0 +1,203 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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.0299999993 + 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 + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + 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 + joint6_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + joint1: + Value: true + joint2: + Value: true + joint3: + Value: true + joint4: + Value: true + joint5: + Value: true + joint6: + Value: true + joint6_flange: + Value: true + Marker Scale: 0.300000012 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + joint1: + joint2: + joint3: + joint4: + joint5: + joint6: + joint6_flange: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: joint1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 1.20289087 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0706475973 + Y: -0.0814988762 + Z: 0.107583851 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.440398335 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.430389732 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/Mybuddy/mybuddy_socket/config/mycobot_with_marker.rviz b/Mybuddy/mybuddy_socket/config/mycobot_with_marker.rviz new file mode 100644 index 0000000..8b0dc7f --- /dev/null +++ b/Mybuddy/mybuddy_socket/config/mycobot_with_marker.rviz @@ -0,0 +1,216 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + - /Marker1 + - /Marker1/Namespaces1 + Splitter Ratio: 0.5 + Tree Height: 775 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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.0299999993 + 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 + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + 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 + joint6_flange: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + basic_shapes: + Value: true + joint1: + Value: true + joint2: + Value: true + joint3: + Value: true + joint4: + Value: true + joint5: + Value: true + joint6: + Value: true + joint6_flange: + Value: true + Marker Scale: 0.300000012 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + joint1: + joint2: + joint3: + joint4: + joint5: + joint6: + joint6_flange: + basic_shapes: + {} + Update Interval: 0 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /visualization_marker + Name: Marker + Namespaces: + basic_cube: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: joint1 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.11990476 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.0706475973 + Y: -0.0814988762 + Z: 0.107583851 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.375398338 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.235389769 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000022f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000039a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/Mybuddy/mybuddy_socket/launch/mybuddy_follow.launch b/Mybuddy/mybuddy_socket/launch/mybuddy_follow.launch new file mode 100644 index 0000000..e0c6930 --- /dev/null +++ b/Mybuddy/mybuddy_socket/launch/mybuddy_follow.launch @@ -0,0 +1,11 @@ + + + + + + + + ["joint_states"] + + + diff --git a/Mybuddy/mybuddy_socket/launch/simple_gui.launch b/Mybuddy/mybuddy_socket/launch/simple_gui.launch new file mode 100644 index 0000000..36d7025 --- /dev/null +++ b/Mybuddy/mybuddy_socket/launch/simple_gui.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket/launch/slider_control.launch b/Mybuddy/mybuddy_socket/launch/slider_control.launch new file mode 100644 index 0000000..e056f92 --- /dev/null +++ b/Mybuddy/mybuddy_socket/launch/slider_control.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket/launch/teleop_keyboard.launch b/Mybuddy/mybuddy_socket/launch/teleop_keyboard.launch new file mode 100644 index 0000000..1ae9d06 --- /dev/null +++ b/Mybuddy/mybuddy_socket/launch/teleop_keyboard.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket/launch/test.launch b/Mybuddy/mybuddy_socket/launch/test.launch new file mode 100644 index 0000000..6264a2e --- /dev/null +++ b/Mybuddy/mybuddy_socket/launch/test.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket/package.xml b/Mybuddy/mybuddy_socket/package.xml new file mode 100644 index 0000000..3a05dd8 --- /dev/null +++ b/Mybuddy/mybuddy_socket/package.xml @@ -0,0 +1,47 @@ + + + mybuddy_socket + 0.3.0 + The mybuddy_socket package + + ZhangLijun + ZhangLijun + + BSD + + https://github.com/elephantrobotics/mycobot_ros + + catkin + + roscpp + rospy + std_msgs + actionlib + mycobot_description + mybuddy_communication + + mybuddy_communication + mycobot_description + + roscpp + rospy + std_msgs + actionlib + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + xacro + joy + rviz + controller_manager + python-tk + mycobot_description + mybuddy_communication + + + + + + + + diff --git a/Mybuddy/mybuddy_socket/scripts/follow_display.py b/Mybuddy/mybuddy_socket/scripts/follow_display.py new file mode 100755 index 0000000..0a22bef --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/follow_display.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +import time +import os +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from visualization_msgs.msg import Marker +from pymycobot.mybuddysocket import MyBuddySocket +import math + +def talker(): + rospy.init_node("display", anonymous=True) + + print("Try connect real mybuddy...") + ip = rospy.get_param("~ip", "192.168.123.240") + port = rospy.get_param("~port", 9000) + print("ip: {}, port: {}\n".format(ip, port)) + try: + mb = MyBuddySocket(ip, port) + mb.connect(serialport="/dev/ttyACM0", baudrate="115200") + except Exception as e: + print(e) + print( + """\ + \rTry connect mybuddy failed! + \rPlease check wether connected with mybuddy. + \rPlease chckt wether the port or baud is right. + """ + ) + exit(1) + + mb.release_all_servos() + time.sleep(0.5) + print("Rlease all servos over.\n") + + pub = rospy.Publisher("joint_states", JointState, queue_size=20) + pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=20) + rate = rospy.Rate(30) # hz + + # pub joint state + joint_state_send = JointState() + joint_state_send.header = Header() + + joint_state_send.name = [ + "joint1_L", + "joint2_L", + "joint3_L", + "joint4_L", + "joint5_L", + "joint6_L", + "joint1_R", + "joint2_R", + "joint3_R", + "joint4_R", + "joint5_R", + "joint6_R", + "base_link1", + + ] + joint_state_send.velocity = [0.0] + joint_state_send.effort = [] + + marker_ = Marker() + marker_.header.frame_id = "/base_link1" + marker_.ns = "my_namespace" + + + print("publishing ...") + while not rospy.is_shutdown(): + joint_state_send.header.stamp = rospy.Time.now() + + left_radians = mb.get_radians(1) + + right_radians = mb.get_radians(2) + + wangles = mb.get_angles(3)[0]*(math.pi/180) + waist_radian =[] + waist_radian.append(wangles) + + print('left:',left_radians,'right:',right_radians,'w:',waist_radian) + + # =======all_radians======= + all_radians = left_radians + right_radians + waist_radian + data_list = [] + for index, value in enumerate(all_radians): + data_list.append(value) + + # rospy.loginfo('{}'.format(data_list)) + joint_state_send.position = data_list + + print("all_radians: %s" % data_list) + + pub.publish(joint_state_send) + + # =======left_coords======= + left_coords = mb.get_coords(1) + + # =======right_coords======= + right_coords = mb.get_coords(2) + + waist_coords = mb.get_angles(3) + + # 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 + marker_.pose.position.x = left_coords[1] / 1000 * -1 + marker_.pose.position.y = left_coords[0] / 1000 + marker_.pose.position.z = left_coords[2] / 1000 + + time.sleep(0.02) + + marker_.pose.position.x = right_coords[1] / 1000 * -1 + marker_.pose.position.y = right_coords[0] / 1000 + marker_.pose.position.z = right_coords[2] / 1000 + + time.sleep(0.02) + + marker_.pose.position.x = waist_coords[0] / 1000 * -1 + + marker_.color.a = 1.0 + marker_.color.r = 0.0 + marker_.color.g = 1.0 + marker_.color.b = 0.0 + pub_marker.publish(marker_) + rate.sleep() + + print("\n") + + +if __name__ == "__main__": + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/Mybuddy/mybuddy_socket/scripts/listen_real.py b/Mybuddy/mybuddy_socket/scripts/listen_real.py new file mode 100644 index 0000000..6d500a7 --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/listen_real.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +# license removed for brevity +import time +import math + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from mycobot_communication.srv import GetAngles + + +def talker(): + rospy.loginfo("start ...") + rospy.init_node("real_listener", anonymous=True) + pub = rospy.Publisher("joint_states", JointState, queue_size=15) + 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", + "base_link1", + "joint1_L", + "joint2_L", + "joint3_L", + "joint4_L", + "joint5_L", + "joint6_L", + "joint1_R", + "joint2_R", + "joint3_R", + "joint4_R", + "joint5_R", + "joint6_R", + ] + joint_state_send.velocity = [0] + joint_state_send.effort = [] + + # waiting util server `get_joint_angles` enable. + rospy.loginfo("wait service") + rospy.wait_for_service("get_joint_angles") + func = rospy.ServiceProxy("get_joint_angles", GetAngles) + + rospy.loginfo("start loop ...") + while not rospy.is_shutdown(): + # get real angles from server. + res = func() + if res.joint_1 == res.joint_2 == res.joint_3 == 0.0: + continue + radians_list = [ + res.joint_1 * (math.pi / 180), + res.joint_2 * (math.pi / 180), + res.joint_3 * (math.pi / 180), + res.joint_4 * (math.pi / 180), + res.joint_5 * (math.pi / 180), + res.joint_6 * (math.pi / 180), + ] + rospy.loginfo("res: {}".format(radians_list)) + + # publish angles. + joint_state_send.header.stamp = rospy.Time.now() + joint_state_send.position = radians_list + pub.publish(joint_state_send) + rate.sleep() + + +if __name__ == "__main__": + try: + talker() + except rospy.ROSInterruptException: + pass diff --git a/Mybuddy/mybuddy_socket/scripts/listen_real_of_topic.py b/Mybuddy/mybuddy_socket/scripts/listen_real_of_topic.py new file mode 100644 index 0000000..169567a --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/listen_real_of_topic.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python2 +import math + +import rospy +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +from mycobot_communication.msg import MycobotAngles + + +class Listener(object): + def __init__(self): + super(Listener, self).__init__() + + rospy.loginfo("start ...") + rospy.init_node("real_listener_1", anonymous=True) + # init publisher. + self.pub = rospy.Publisher("joint_states", JointState, queue_size=10) + # init subscriber. + self.sub = rospy.Subscriber("mycobot/angles_real", MycobotAngles, self.callback) + rospy.spin() + + def callback(self, data): + """`mycobot/angles_real` subscriber callback method. + + Args: + data (MycobotAngles): callback argument. + """ + # ini publisher object. + 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 = [] + joint_state_send.header.stamp = rospy.Time.now() + + # process callback data. + radians_list = [ + data.joint_1 * (math.pi / 180), + data.joint_2 * (math.pi / 180), + data.joint_3 * (math.pi / 180), + data.joint_4 * (math.pi / 180), + data.joint_5 * (math.pi / 180), + data.joint_6 * (math.pi / 180), + ] + rospy.loginfo("res: {}".format(radians_list)) + + joint_state_send.position = radians_list + self.pub.publish(joint_state_send) + + +if __name__ == "__main__": + try: + Listener() + except rospy.ROSInterruptException: + pass diff --git a/Mybuddy/mybuddy_socket/scripts/simple_gui.py b/Mybuddy/mybuddy_socket/scripts/simple_gui.py new file mode 100644 index 0000000..a72227a --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/simple_gui.py @@ -0,0 +1,474 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import tkinter as tk +from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus +import rospy +import time +from rospy import ServiceException + + +class Window: + def __init__(self, handle): + self.win = handle + self.win.resizable(0, 0) # 固定窗口大小 + + self.model = 0 + self.speed = rospy.get_param("~speed", 50) + + # 设置默认速度 + self.speed_d = tk.StringVar() + self.speed_d.set(str(self.speed)) + # print(self.speed) + self.connect_ser() + + # 获取机械臂数据 + self.record_coords = [0, 0, 0, 0, 0, 0, self.speed, self.model] + self.res_angles = [0, 0, 0, 0, 0, 0, self.speed, self.model] + self.get_date() + + # get screen width and height + self.ws = self.win.winfo_screenwidth() # width of the screen + self.hs = self.win.winfo_screenheight() # height of the screen + # calculate x and y coordinates for the Tk root window + x = (self.ws / 2) - 190 + y = (self.hs / 2) - 250 + self.win.geometry("430x370+{}+{}".format(x, y)) + # 布局 + self.set_layout() + # 输入部分 + self.need_input() + # 展示部分 + self.show_init() + + # joint 设置按钮 + tk.Button(self.frmLT, text="设置", width=5, command=self.get_joint_input).grid( + row=6, column=1, sticky="w", padx=3, pady=2 + ) + + # coordination 设置按钮 + tk.Button(self.frmRT, text="设置", width=5, command=self.get_coord_input).grid( + row=6, column=1, sticky="w", padx=3, pady=2 + ) + + # 夹爪开关按钮 + tk.Button(self.frmLB, text="夹爪(开)", command=self.gripper_open, width=5).grid( + row=1, column=0, sticky="w", padx=3, pady=50 + ) + tk.Button(self.frmLB, text="夹爪(关)", command=self.gripper_close, width=5).grid( + row=1, column=1, sticky="w", padx=3, pady=2 + ) + + def connect_ser(self): + rospy.init_node("simple_gui", anonymous=True, disable_signals=True) + + rospy.wait_for_service("get_joint_angles") + rospy.wait_for_service("set_joint_angles") + rospy.wait_for_service("get_joint_coords") + rospy.wait_for_service("set_joint_coords") + rospy.wait_for_service("switch_gripper_status") + try: + self.get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) + self.set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) + self.get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) + self.set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) + self.switch_gripper = rospy.ServiceProxy( + "switch_gripper_status", GripperStatus + ) + except: + print("start error ...") + exit(1) + + print("Connect service success.") + + def set_layout(self): + self.frmLT = tk.Frame(width=200, height=200) + self.frmLC = tk.Frame(width=200, height=200) + self.frmLB = tk.Frame(width=200, height=200) + self.frmRT = tk.Frame(width=200, height=200) + self.frmLT.grid(row=0, column=0, padx=1, pady=3) + self.frmLC.grid(row=1, column=0, padx=1, pady=3) + self.frmLB.grid(row=1, column=1, padx=2, pady=3) + self.frmRT.grid(row=0, column=1, padx=2, pady=3) + + def need_input(self): + # 输入提示 + tk.Label(self.frmLT, text="Joint 1 ").grid(row=0) + tk.Label(self.frmLT, text="Joint 2 ").grid(row=1) # 第二行 + tk.Label(self.frmLT, text="Joint 3 ").grid(row=2) + tk.Label(self.frmLT, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLT, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLT, text="Joint 6 ").grid(row=5) + + tk.Label(self.frmRT, text=" x ").grid(row=0) + tk.Label(self.frmRT, text=" y ").grid(row=1) # 第二行 + tk.Label(self.frmRT, text=" z ").grid(row=2) + tk.Label(self.frmRT, text=" rx ").grid(row=3) + tk.Label(self.frmRT, text=" ry ").grid(row=4) + tk.Label(self.frmRT, text=" rz ").grid(row=5) + + # 设置输入框的默认值 + self.j1_default = tk.StringVar() + self.j1_default.set(self.res_angles[0]) + self.j2_default = tk.StringVar() + self.j2_default.set(self.res_angles[1]) + self.j3_default = tk.StringVar() + self.j3_default.set(self.res_angles[2]) + self.j4_default = tk.StringVar() + self.j4_default.set(self.res_angles[3]) + self.j5_default = tk.StringVar() + self.j5_default.set(self.res_angles[4]) + self.j6_default = tk.StringVar() + self.j6_default.set(self.res_angles[5]) + + self.x_default = tk.StringVar() + self.x_default.set(self.record_coords[0]) + self.y_default = tk.StringVar() + self.y_default.set(self.record_coords[1]) + self.z_default = tk.StringVar() + self.z_default.set(self.record_coords[2]) + self.rx_default = tk.StringVar() + self.rx_default.set(self.record_coords[3]) + self.ry_default = tk.StringVar() + self.ry_default.set(self.record_coords[4]) + self.rz_default = tk.StringVar() + self.rz_default.set(self.record_coords[5]) + + # joint 输入框 + self.J_1 = tk.Entry(self.frmLT, textvariable=self.j1_default) + self.J_1.grid(row=0, column=1, pady=3) + self.J_2 = tk.Entry(self.frmLT, textvariable=self.j2_default) + self.J_2.grid(row=1, column=1, pady=3) + self.J_3 = tk.Entry(self.frmLT, textvariable=self.j3_default) + self.J_3.grid(row=2, column=1, pady=3) + self.J_4 = tk.Entry(self.frmLT, textvariable=self.j4_default) + self.J_4.grid(row=3, column=1, pady=3) + self.J_5 = tk.Entry(self.frmLT, textvariable=self.j5_default) + self.J_5.grid(row=4, column=1, pady=3) + self.J_6 = tk.Entry(self.frmLT, textvariable=self.j6_default) + self.J_6.grid(row=5, column=1, pady=3) + + # coord 输入框 + self.x = tk.Entry(self.frmRT, textvariable=self.x_default) + self.x.grid(row=0, column=1, pady=3, padx=0) + self.y = tk.Entry(self.frmRT, textvariable=self.y_default) + self.y.grid(row=1, column=1, pady=3) + self.z = tk.Entry(self.frmRT, textvariable=self.z_default) + self.z.grid(row=2, column=1, pady=3) + self.rx = tk.Entry(self.frmRT, textvariable=self.rx_default) + self.rx.grid(row=3, column=1, pady=3) + self.ry = tk.Entry(self.frmRT, textvariable=self.ry_default) + self.ry.grid(row=4, column=1, pady=3) + self.rz = tk.Entry(self.frmRT, textvariable=self.rz_default) + self.rz.grid(row=5, column=1, pady=3) + + # 所有输入框，用于拿输入的数据 + self.all_j = [self.J_1, self.J_2, self.J_3, self.J_4, self.J_5, self.J_6] + self.all_c = [self.x, self.y, self.z, self.rx, self.ry, self.rz] + + # 速度输入框 + tk.Label( + self.frmLB, + text="speed", + ).grid(row=0, column=0) + self.get_speed = tk.Entry(self.frmLB, textvariable=self.speed_d, width=10) + self.get_speed.grid(row=0, column=1) + + def show_init(self): + # 显示 + tk.Label(self.frmLC, text="Joint 1 ").grid(row=0) + tk.Label(self.frmLC, text="Joint 2 ").grid(row=1) # 第二行 + tk.Label(self.frmLC, text="Joint 3 ").grid(row=2) + tk.Label(self.frmLC, text="Joint 4 ").grid(row=3) + tk.Label(self.frmLC, text="Joint 5 ").grid(row=4) + tk.Label(self.frmLC, text="Joint 6 ").grid(row=5) + + # get数据 + + # ，展示出来 + self.cont_1 = tk.StringVar(self.frmLC) + self.cont_1.set(str(self.res_angles[0]) + "°") + self.cont_2 = tk.StringVar(self.frmLC) + self.cont_2.set(str(self.res_angles[1]) + "°") + self.cont_3 = tk.StringVar(self.frmLC) + self.cont_3.set(str(self.res_angles[2]) + "°") + self.cont_4 = tk.StringVar(self.frmLC) + self.cont_4.set(str(self.res_angles[3]) + "°") + self.cont_5 = tk.StringVar(self.frmLC) + self.cont_5.set(str(self.res_angles[4]) + "°") + self.cont_6 = tk.StringVar(self.frmLC) + self.cont_6.set(str(self.res_angles[5]) + "°") + self.cont_all = [ + self.cont_1, + self.cont_2, + self.cont_3, + self.cont_4, + self.cont_5, + self.cont_6, + self.speed, + self.model, + ] + + self.show_j1 = tk.Label( + self.frmLC, + textvariable=self.cont_1, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=1, padx=0, pady=5) + + self.show_j2 = tk.Label( + self.frmLC, + textvariable=self.cont_2, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=1, padx=0, pady=5) + self.show_j3 = tk.Label( + self.frmLC, + textvariable=self.cont_3, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=1, padx=0, pady=5) + self.show_j4 = tk.Label( + self.frmLC, + textvariable=self.cont_4, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=1, padx=0, pady=5) + self.show_j5 = tk.Label( + self.frmLC, + textvariable=self.cont_5, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=1, padx=0, pady=5) + self.show_j6 = tk.Label( + self.frmLC, + textvariable=self.cont_6, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=1, padx=5, pady=5) + + self.all_jo = [ + self.show_j1, + self.show_j2, + self.show_j3, + self.show_j4, + self.show_j5, + self.show_j6, + ] + + # 显示 + tk.Label(self.frmLC, text=" x ").grid(row=0, column=3) + tk.Label(self.frmLC, text=" y ").grid(row=1, column=3) # 第二行 + tk.Label(self.frmLC, text=" z ").grid(row=2, column=3) + tk.Label(self.frmLC, text=" rx ").grid(row=3, column=3) + tk.Label(self.frmLC, text=" ry ").grid(row=4, column=3) + tk.Label(self.frmLC, text=" rz ").grid(row=5, column=3) + self.coord_x = tk.StringVar() + self.coord_x.set(str(self.record_coords[0])) + self.coord_y = tk.StringVar() + self.coord_y.set(str(self.record_coords[1])) + self.coord_z = tk.StringVar() + self.coord_z.set(str(self.record_coords[2])) + self.coord_rx = tk.StringVar() + self.coord_rx.set(str(self.record_coords[3])) + self.coord_ry = tk.StringVar() + self.coord_ry.set(str(self.record_coords[4])) + self.coord_rz = tk.StringVar() + self.coord_rz.set(str(self.record_coords[5])) + + self.coord_all = [ + self.coord_x, + self.coord_y, + self.coord_z, + self.coord_rx, + self.coord_ry, + self.coord_rz, + self.speed, + self.model, + ] + + self.show_x = tk.Label( + self.frmLC, + textvariable=self.coord_x, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=0, column=4, padx=5, pady=5) + self.show_y = tk.Label( + self.frmLC, + textvariable=self.coord_y, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=1, column=4, padx=5, pady=5) + self.show_z = tk.Label( + self.frmLC, + textvariable=self.coord_z, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=2, column=4, padx=5, pady=5) + self.show_rx = tk.Label( + self.frmLC, + textvariable=self.coord_rx, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=3, column=4, padx=5, pady=5) + self.show_ry = tk.Label( + self.frmLC, + textvariable=self.coord_ry, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=4, column=4, padx=5, pady=5) + self.show_rz = tk.Label( + self.frmLC, + textvariable=self.coord_rz, + font=("Arial", 9), + width=7, + height=1, + bg="white", + ).grid(row=5, column=4, padx=5, pady=5) + + # mm 单位展示 + self.unit = tk.StringVar() + self.unit.set("mm") + for i in range(6): + tk.Label(self.frmLC, textvariable=self.unit, font=("Arial", 9)).grid( + row=i, column=5 + ) + + def gripper_open(self): + try: + self.switch_gripper(True) + except ServiceException: + # 可能由于该方法没有返回值，服务抛出无法处理的错误 + pass + + def gripper_close(self): + try: + self.switch_gripper(False) + except ServiceException: + pass + + def get_coord_input(self): + # 获取 coord 输入的数据，发送给机械臂 + c_value = [] + for i in self.all_c: + # print(type(i.get())) + c_value.append(float(i.get())) + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) + c_value.append(self.speed) + c_value.append(self.model) + # print(c_value) + try: + self.set_coords(*c_value) + except ServiceException: + pass + self.show_j_date(c_value[:-2], "coord") + + def get_joint_input(self): + # 获取joint输入的数据，发送给机械臂 + j_value = [] + for i in self.all_j: + # print(type(i.get())) + j_value.append(float(i.get())) + self.speed = ( + int(float(self.get_speed.get())) if self.get_speed.get() else self.speed + ) + j_value.append(self.speed) + + try: + self.set_angles(*j_value) + except ServiceException: + pass + self.show_j_date(j_value[:-1]) + # return j_value,c_value,speed + + def get_date(self): + # 拿机械臂的数据，用于展示 + t = time.time() + while time.time() - t < 2: + self.res = self.get_coords() + if self.res.x > 1: + break + time.sleep(0.1) + + t = time.time() + while time.time() - t < 2: + self.angles = self.get_angles() + if self.angles.joint_1 > 1: + break + time.sleep(0.1) + # print(self.angles.joint_1) + self.record_coords = [ + round(self.res.x, 2), + round(self.res.y, 2), + round(self.res.z, 2), + round(self.res.rx, 2), + round(self.res.ry, 2), + round(self.res.rz, 2), + self.speed, + self.model, + ] + self.res_angles = [ + round(self.angles.joint_1, 2), + round(self.angles.joint_2, 2), + round(self.angles.joint_3, 2), + round(self.angles.joint_4, 2), + round(self.angles.joint_5, 2), + round(self.angles.joint_6, 2), + ] + # print('coord:',self.record_coords) + # print('angles:',self.res_angles) + + # def send_input(self,dates): + def show_j_date(self, date, way=""): + # 展示数据 + if way == "coord": + for i, j in zip(date, self.coord_all): + # print(i) + j.set(str(i)) + else: + for i, j in zip(date, self.cont_all): + j.set(str(i) + "°") + + def run(self): + while True: + try: + self.win.update() + time.sleep(0.001) + except tk.TclError as e: + if "application has been destroyed" in str(e): + break + else: + raise + + +def main(): + window = tk.Tk() + window.title("mycobot ros GUI") + Window(window).run() + + +if __name__ == "__main__": + main() diff --git a/Mybuddy/mybuddy_socket/scripts/slider_control.py b/Mybuddy/mybuddy_socket/scripts/slider_control.py new file mode 100755 index 0000000..98c5c1a --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/slider_control.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 + +"""[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/ttyACM0' + baud: serial prot baudrate. Defaults is 115200. +""" + +import rospy +from sensor_msgs.msg import JointState +from pymycobot.mybuddysocket import MyBuddySocket +import time +import os +import math + +mb = None + +def callback(data): + # rospy.loginfo(rospy.get_caller_id() + "%s", data.position) + # print(data.position) + data_list = [] + for index, value in enumerate(data.position): + data_list.append(value) + + + print(data_list) + + data_list1 = data_list[:6] + data_list2 = data_list[6:-1] + data_list3 = data_list[-1:] + + print("left_arm: %s" % data_list1) + print("right_arm: %s" % data_list2) + print("waist: %s" % data_list3) + + mb.send_radians(1,data_list1, 50) + time.sleep(0.02) + mb.send_radians(2,data_list2, 50) + time.sleep(0.02) + + # print(data_list3[0]* (180 / math.pi)) + print("\n") + + mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10) + # mb.send_radians(3,data_list3, 50) + time.sleep(0.02) + +def listener(): + global mb + rospy.init_node("control_slider", anonymous=True) + rospy.Subscriber("joint_states", JointState, callback) + ip = rospy.get_param("~ip", "192.168.123.240") + port = rospy.get_param("~port", 9000) + print(ip, port) + mb = MyBuddySocket(ip, port) + mb.connect(serialport="/dev/ttyACM0", baudrate="115200") + + # spin() simply keeps python from exiting until this node is stopped + print("spin ...") + rospy.spin() + + +if __name__ == "__main__": + listener() diff --git a/Mybuddy/mybuddy_socket/scripts/teleop_keyboard.py b/Mybuddy/mybuddy_socket/scripts/teleop_keyboard.py new file mode 100644 index 0000000..f24da3d --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/teleop_keyboard.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +from __future__ import print_function +from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus +import rospy +import sys +import select +import termios +import tty +import time + +import roslib + + +msg = """\ +Mycobot Teleop Keyboard Controller +--------------------------- +Movimg options(control coordinations [x,y,z,rx,ry,rz]): + w(x+) + + a(y-) s(x-) d(y+) + + z(z-) x(z+) + +u(rx+) i(ry+) o(rz+) +j(rx-) k(ry-) l(rz-) + +Gripper control: + g - open + h - close + +Other: + 1 - Go to init pose + 2 - Go to home pose + 3 - Resave home pose + q - Quit +""" + + +def vels(speed, turn): + return "currently:\tspeed: %s\tchange percent: %s " % (speed, turn) + + +class Raw(object): + def __init__(self, stream): + self.stream = stream + self.fd = self.stream.fileno() + + def __enter__(self): + self.original_stty = termios.tcgetattr(self.stream) + tty.setcbreak(self.stream) + + def __exit__(self, type, value, traceback): + termios.tcsetattr(self.stream, termios.TCSANOW, self.original_stty) + + +def teleop_keyboard(): + rospy.init_node("teleop_keyboard") + + model = 0 + speed = rospy.get_param("~speed", 50) + change_percent = rospy.get_param("~change_percent", 5) + + change_angle = 180 * change_percent / 100 + change_len = 250 * change_percent / 100 + + rospy.wait_for_service("get_joint_angles") + rospy.wait_for_service("set_joint_angles") + rospy.wait_for_service("get_joint_coords") + rospy.wait_for_service("set_joint_coords") + rospy.wait_for_service("switch_gripper_status") + print("service ready.") + try: + get_coords = rospy.ServiceProxy("get_joint_coords", GetCoords) + set_coords = rospy.ServiceProxy("set_joint_coords", SetCoords) + get_angles = rospy.ServiceProxy("get_joint_angles", GetAngles) + set_angles = rospy.ServiceProxy("set_joint_angles", SetAngles) + switch_gripper = rospy.ServiceProxy("switch_gripper_status", GripperStatus) + except: + print("start error ...") + exit(1) + + init_pose = [0, 0, 0, 0, 0, 0, speed] + home_pose = [0, 0, 0, 0, 0, 0, speed] + + # rsp = set_angles(*init_pose) + + while True: + res = get_coords() + if res.x > 1: + break + time.sleep(0.1) + + record_coords = [res.x, res.y, res.z, res.rx, res.ry, res.rz, speed, model] + print(record_coords) + + try: + print(msg) + print(vels(speed, change_percent)) + while 1: + try: + # print("\r current coords: %s" % record_coords, end="") + with Raw(sys.stdin): + key = sys.stdin.read(1) + if key == "q": + break + elif key in ["w", "W"]: + record_coords[0] += change_len + set_coords(*record_coords) + elif key in ["s", "S"]: + record_coords[0] -= change_len + set_coords(*record_coords) + elif key in ["a", "A"]: + record_coords[1] -= change_len + set_coords(*record_coords) + elif key in ["d", "D"]: + record_coords[1] += change_len + set_coords(*record_coords) + elif key in ["z", "Z"]: + record_coords[2] -= change_len + set_coords(*record_coords) + elif key in ["x", "X"]: + record_coords[2] += change_len + set_coords(*record_coords) + elif key in ["u", "U"]: + record_coords[3] += change_angle + set_coords(*record_coords) + elif key in ["j", "J"]: + record_coords[3] -= change_angle + set_coords(*record_coords) + elif key in ["i", "I"]: + record_coords[4] += change_angle + set_coords(*record_coords) + elif key in ["k", "K"]: + record_coords[4] -= change_angle + set_coords(*record_coords) + elif key in ["o", "O"]: + record_coords[5] += change_angle + set_coords(*record_coords) + elif key in ["l", "L"]: + record_coords[5] -= change_angle + set_coords(*record_coords) + elif key in ["g", "G"]: + switch_gripper(True) + elif key in ["h", "H"]: + switch_gripper(False) + elif key == "1": + rsp = set_angles(*init_pose) + elif key in "2": + rsp = set_angles(*home_pose) + elif key in "3": + rep = get_angles() + home_pose[0] = rep.joint_1 + home_pose[1] = rep.joint_2 + home_pose[2] = rep.joint_3 + home_pose[3] = rep.joint_4 + home_pose[5] = rep.joint_5 + else: + continue + + except Exception as e: + # print(e) + continue + + except Exception as e: + print(e) + + +if __name__ == "__main__": + try: + teleop_keyboard() + except rospy.ROSInterruptException: + pass diff --git a/Mybuddy/mybuddy_socket/scripts/test_control.py b/Mybuddy/mybuddy_socket/scripts/test_control.py new file mode 100644 index 0000000..c1f0bd7 --- /dev/null +++ b/Mybuddy/mybuddy_socket/scripts/test_control.py @@ -0,0 +1,44 @@ +from time import time +from pymycobot.mybuddy import MyBuddy +import rospy +import time +import os + +port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) +baud = rospy.get_param("~baud", 115200) +print(port, baud) +mb = MyBuddy(port, baud) + +# mb.set_free_mode(2) +# mb.power_on() +# mb.release_all_servos() +# mb.set_servo_calibration(3,1) + +# i = 1 +# while i < 7: +# mb.release_servo(2,i) +# i += 1 +# time.sleep(1) + +''' +------------get info-------------- +''' +# print(mb.get_radians(1)) +# print(mb.get_encoders(3)) +# print(mb.get_encoder(1,1)) +# print(mb.get_angles(1)) +# print(mb.get_coords(1)) +# print(mb.get_coords(2)) +# print("\n") + +''' +------------send info-------------- +''' +# mb.send_radians(2,[-0.136, -0.462, -0.62, 0.813, 0.218, -0.071],30) +# mb.send_angles(2,[6.32, -94.39, 13.62, -15.38, -94.21, 62.84],30,1) +# time.sleep(2) +print(mb.get_angles(3)) +# mb.send_angles(1,[0,0,0,0,0,0],30,0) +# mb.send_angle(1,3,30,30) +# mb.set_encoder(3,1,2048,1) +time.sleep(1) \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket/src/camera_display.cpp b/Mybuddy/mybuddy_socket/src/camera_display.cpp new file mode 100644 index 0000000..1f3abeb --- /dev/null +++ b/Mybuddy/mybuddy_socket/src/camera_display.cpp @@ -0,0 +1,29 @@ +#include +#include +#include +#include + +void imageCallback(const sensor_msgs::ImageConstPtr &msg) +{ + try + { + cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image); + cv::waitKey(30); + } + catch (cv_bridge::Exception &e) + { + ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str()); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "image_listener"); + ros::NodeHandle nh; + cv::namedWindow("view"); + cv::startWindowThread(); + image_transport::ImageTransport it(nh); + image_transport::Subscriber sub = it.subscribe("camera/image", 1, imageCallback); + ros::spin(); + cv::destroyWindow("view"); +} diff --git a/Mybuddy/mybuddy_socket/src/opencv_camera.cpp b/Mybuddy/mybuddy_socket/src/opencv_camera.cpp new file mode 100644 index 0000000..44b4522 --- /dev/null +++ b/Mybuddy/mybuddy_socket/src/opencv_camera.cpp @@ -0,0 +1,59 @@ +#include +#include +#include +#include +#include // for converting the command line parameter to integer + +int main(int argc, char **argv) +{ + // Check if video source has been passed as a parameter + if (argv[1] == NULL) + { + ROS_INFO("argv[1]=NULL\n"); + return 1; + } + + ros::init(argc, argv, "image_publisher"); // Initialize node + ros::NodeHandle nh; + image_transport::ImageTransport it(nh); + image_transport::Publisher pub = it.advertise("camera/image", 1); // Publish topic + + ros::Rate loop_rate(200); // refresh Hz. + + // Convert the passed as command line parameter index for the video device to an integer + std::istringstream video_sourceCmd(argv[1]); + int video_source; + // Check if it is indeed a number + if (!(video_sourceCmd >> video_source)) + { + ROS_INFO("video_sourceCmd is %d\n", video_source); + return 1; + } + + cv::VideoCapture cap(video_source); + // Check if video device can be opened with the given index + if (!cap.isOpened()) + { + ROS_INFO("can not opencv video device\n"); + return 1; + } + cv::Mat frame; + sensor_msgs::ImagePtr msg; + + while (nh.ok()) + { + cap >> frame; + // cv::imshow("veiwer", frame); + // Check if grabbed frame is actually full with some content + if (!frame.empty()) + { + msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg(); + pub.publish(msg); + //cv::Wait(1); + } + ros::spinOnce(); + loop_rate.sleep(); + // if(cv::waitKey(2) >= 0) + // break; + } +} diff --git a/Mybuddy/mybuddy_socket_moveit/.setup_assistant b/Mybuddy/mybuddy_socket_moveit/.setup_assistant new file mode 100644 index 0000000..fa016a5 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: mycobot_description + relative_path: urdf/mybuddy/mybuddy.urdf + xacro_args: "" + SRDF: + relative_path: config/firefighter.srdf + CONFIG: + author_name: wangweijian + author_email: weijian.wang@elephatrobotics.com + generated_timestamp: 1661409645 \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/CMakeLists.txt b/Mybuddy/mybuddy_socket_moveit/CMakeLists.txt new file mode 100644 index 0000000..a8948b4 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(mybuddy_socket_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/Mybuddy/mybuddy_socket_moveit/config/cartesian_limits.yaml b/Mybuddy/mybuddy_socket_moveit/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/Mybuddy/mybuddy_socket_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/Mybuddy/mybuddy_socket_moveit/config/chomp_planning.yaml b/Mybuddy/mybuddy_socket_moveit/config/chomp_planning.yaml new file mode 100644 index 0000000..eb9c912 --- /dev/null +++ b/Mybuddy/mybuddy_socket_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/Mybuddy/mybuddy_socket_moveit/config/fake_controllers.yaml b/Mybuddy/mybuddy_socket_moveit/config/fake_controllers.yaml new file mode 100644 index 0000000..068dd2f --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/fake_controllers.yaml @@ -0,0 +1,32 @@ +controller_list: + - name: fake_right_arm_controller + type: $(arg fake_execution_type) + joints: + - base_link1 + - joint1_R + - joint2_R + - joint3_R + - joint4_R + - joint5_R + - joint6_R + - name: fake_left_arm_controller + type: $(arg fake_execution_type) + joints: + - base_link1 + - joint1_L + - joint2_L + - joint3_L + - joint4_L + - joint5_L + - joint6_L + - name: fake_waist_controller + type: $(arg fake_execution_type) + joints: + - base_link1 +initial: # Define initial robot poses per group + - group: right_arm + pose: init_right + - group: left_arm + pose: init_left + - group: waist + pose: init_waist \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/config/firefighter.srdf b/Mybuddy/mybuddy_socket_moveit/config/firefighter.srdf new file mode 100644 index 0000000..1857dd7 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/firefighter.srdf @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/config/gazebo_controllers.yaml b/Mybuddy/mybuddy_socket_moveit/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/Mybuddy/mybuddy_socket_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/Mybuddy/mybuddy_socket_moveit/config/joint_limits.yaml b/Mybuddy/mybuddy_socket_moveit/config/joint_limits.yaml new file mode 100644 index 0000000..d44e6de --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/joint_limits.yaml @@ -0,0 +1,75 @@ +# 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: + base_link1: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint1_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint1_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint2_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint2_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint3_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint3_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint4_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint4_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint5_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint5_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6_L: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 + joint6_R: + has_velocity_limits: false + max_velocity: 0 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/config/kinematics.yaml b/Mybuddy/mybuddy_socket_moveit/config/kinematics.yaml new file mode 100644 index 0000000..6d4a865 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/kinematics.yaml @@ -0,0 +1,12 @@ +right_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 +left_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 +waist: + 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/Mybuddy/mybuddy_socket_moveit/config/ompl_planning.yaml b/Mybuddy/mybuddy_socket_moveit/config/ompl_planning.yaml new file mode 100644 index 0000000..192af7f --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/ompl_planning.yaml @@ -0,0 +1,214 @@ +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 +right_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(base_link1,joint1_R) + longest_valid_segment_fraction: 0.005 +left_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(base_link1,joint1_L) + longest_valid_segment_fraction: 0.005 +waist: + 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/Mybuddy/mybuddy_socket_moveit/config/ros_controllers.yaml b/Mybuddy/mybuddy_socket_moveit/config/ros_controllers.yaml new file mode 100644 index 0000000..7b6880a --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/ros_controllers.yaml @@ -0,0 +1,102 @@ +right_arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - base_link1 + - joint1_R + - joint2_R + - joint3_R + - joint4_R + - joint5_R + - joint6_R + gains: + base_link1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint1_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint2_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint3_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint4_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint5_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint6_R: + p: 100 + d: 1 + i: 1 + i_clamp: 1 +left_arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - base_link1 + - joint1_L + - joint2_L + - joint3_L + - joint4_L + - joint5_L + - joint6_L + gains: + base_link1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint1_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint2_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint3_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint4_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint5_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint6_L: + p: 100 + d: 1 + i: 1 + i_clamp: 1 +waist_controller: + type: effort_controllers/JointTrajectoryController + joints: + - base_link1 + gains: + base_link1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/config/sensors_3d.yaml b/Mybuddy/mybuddy_socket_moveit/config/sensors_3d.yaml new file mode 100644 index 0000000..51010a3 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/config/simple_moveit_controllers.yaml b/Mybuddy/mybuddy_socket_moveit/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..24d21c5 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/simple_moveit_controllers.yaml @@ -0,0 +1,31 @@ +controller_list: + - name: right_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - base_link1 + - joint1_R + - joint2_R + - joint3_R + - joint4_R + - joint5_R + - joint6_R + - name: left_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - base_link1 + - joint1_L + - joint2_L + - joint3_L + - joint4_L + - joint5_L + - joint6_L + - name: waist_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - base_link1 \ No newline at end of file diff --git a/Mybuddy/mybuddy_socket_moveit/config/stomp_planning.yaml b/Mybuddy/mybuddy_socket_moveit/config/stomp_planning.yaml new file mode 100644 index 0000000..cbed236 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/config/stomp_planning.yaml @@ -0,0 +1,117 @@ +stomp/right_arm: + group_name: right_arm + 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, 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/left_arm: + group_name: left_arm + 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, 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/waist: + group_name: waist + 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/Mybuddy/mybuddy_socket_moveit/launch/chomp_planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..f2188d2 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/default_warehouse_db.launch b/Mybuddy/mybuddy_socket_moveit/launch/default_warehouse_db.launch new file mode 100644 index 0000000..573890a --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/demo.launch b/Mybuddy/mybuddy_socket_moveit/launch/demo.launch new file mode 100644 index 0000000..677a781 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/demo.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/demo_gazebo.launch b/Mybuddy/mybuddy_socket_moveit/launch/demo_gazebo.launch new file mode 100644 index 0000000..a9f320c --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/fake_moveit_controller_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..70b221a --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_controller_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..93c8a44 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_controller_manager.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_sensor_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/firefighter_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/gazebo.launch b/Mybuddy/mybuddy_socket_moveit/launch/gazebo.launch new file mode 100644 index 0000000..45991bd --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/gazebo.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/joystick_control.launch b/Mybuddy/mybuddy_socket_moveit/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/move_group.launch b/Mybuddy/mybuddy_socket_moveit/launch/move_group.launch new file mode 100644 index 0000000..4ff2e39 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/move_group.launch @@ -0,0 +1,101 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz new file mode 100644 index 0000000..a1d64d9 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz @@ -0,0 +1,319 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - /MotionPlanning1/Planning Request1 + - /MotionPlanning1/Planned Path1 + Splitter Ratio: 0.5 + Tree Height: 135 + - 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 + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + 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 + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_R: + 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 + Joint Violation Color: 255; 0; 255 + Planning Group: left_arm + Query Goal State: true + Query Start State: false + 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 + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5_R: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_L: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6_R: + 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 + 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: 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: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Target Frame: base + Yaw: -0.6232355833053589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 848 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd000000010000000000000203000002f1fc0200000007fb000000100044006900730070006c00610079007301000000410000011c000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a00560069006500770073000000010c000000a4000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000001630000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b30000017f0000017400ffffff00000302000002f100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1291 + X: 452 + Y: 28 diff --git a/Mybuddy/mybuddy_socket_moveit/launch/moveit_rviz.launch b/Mybuddy/mybuddy_socket_moveit/launch/moveit_rviz.launch new file mode 100644 index 0000000..a4605c0 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..b078992 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/ompl_planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..2a516de --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/planning_context.launch b/Mybuddy/mybuddy_socket_moveit/launch/planning_context.launch new file mode 100644 index 0000000..883029b --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/planning_context.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..4b4d0d6 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/ros_controllers.launch b/Mybuddy/mybuddy_socket_moveit/launch/ros_controllers.launch new file mode 100644 index 0000000..c6b4cd7 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/run_benchmark_ompl.launch b/Mybuddy/mybuddy_socket_moveit/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..38b71f8 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/sensor_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..ff2be73 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/setup_assistant.launch b/Mybuddy/mybuddy_socket_moveit/launch/setup_assistant.launch new file mode 100644 index 0000000..dd677c2 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/simple_moveit_controller_manager.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..5f7fac6 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/stomp_planning_pipeline.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..d497d10 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/trajectory_execution.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..20c3dfc --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/trajectory_execution.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/warehouse.launch b/Mybuddy/mybuddy_socket_moveit/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/launch/warehouse_settings.launch.xml b/Mybuddy/mybuddy_socket_moveit/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/Mybuddy/mybuddy_socket_moveit/package.xml b/Mybuddy/mybuddy_socket_moveit/package.xml new file mode 100644 index 0000000..6b745c8 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/package.xml @@ -0,0 +1,41 @@ + + + mybuddy_socket_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 + + wangweijian + wangweijian + + 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/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py b/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py new file mode 100755 index 0000000..ea60600 --- /dev/null +++ b/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import time +import rospy +import os +from sensor_msgs.msg import JointState +from pymycobot.mybuddysocket import MyBuddySocket +import math + +mb = None + + +def callback(data): + rospy.loginfo(rospy.get_caller_id() + "%s", data) + data_list = [] + for index, value in enumerate(data.position): + data_list.append(value) + + data_list1 = data_list[:6] + data_list2 = data_list[6:-1] + data_list3 = list(data_list[-1:]) + + print("left_arm: %s" % data_list1) + print("right_arm: %s" % data_list2) + print("waist: %s" % data_list3) + + print("\n") + mb.send_radians(1,data_list1, 50) + time.sleep(0.02) + mb.send_radians(2,data_list2, 50) + time.sleep(0.02) + mb.send_angle(3, 1, data_list3[0]* (180 / math.pi), 10) + time.sleep(0.02) + +def listener(): + global mb + rospy.init_node("mybuddy_reciver", anonymous=True) + ip = rospy.get_param("~ip", "192.168.123.240") + port = rospy.get_param("~port", 9000) + print(ip, port) + mb = MyBuddySocket(ip, port) + mb.connect(serialport="/dev/ttyACM0", baudrate="115200") + + rospy.Subscriber("joint_states", JointState, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == "__main__": + listener() From 3e0a31708c6cf1394adcea125b2487bf1951f7ca Mon Sep 17 00:00:00 2001 From: weijian Date: Mon, 5 Sep 2022 18:43:35 +0800 Subject: [PATCH 2/5] update --- .../mybuddy_socket/scripts/follow_display.py | 2 +- .../mybuddy_socket/scripts/slider_control.py | 2 +- .../mybuddy_socket/scripts/test_control.py | 22 +++++++++++++------ 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Mybuddy/mybuddy_socket/scripts/follow_display.py b/Mybuddy/mybuddy_socket/scripts/follow_display.py index 0a22bef..0a7b549 100755 --- a/Mybuddy/mybuddy_socket/scripts/follow_display.py +++ b/Mybuddy/mybuddy_socket/scripts/follow_display.py @@ -12,7 +12,7 @@ def talker(): rospy.init_node("display", anonymous=True) print("Try connect real mybuddy...") - ip = rospy.get_param("~ip", "192.168.123.240") + ip = rospy.get_param("~ip", "192.168.123.219") port = rospy.get_param("~port", 9000) print("ip: {}, port: {}\n".format(ip, port)) try: diff --git a/Mybuddy/mybuddy_socket/scripts/slider_control.py b/Mybuddy/mybuddy_socket/scripts/slider_control.py index 98c5c1a..9101a11 100755 --- a/Mybuddy/mybuddy_socket/scripts/slider_control.py +++ b/Mybuddy/mybuddy_socket/scripts/slider_control.py @@ -52,7 +52,7 @@ def listener(): global mb rospy.init_node("control_slider", anonymous=True) rospy.Subscriber("joint_states", JointState, callback) - ip = rospy.get_param("~ip", "192.168.123.240") + ip = rospy.get_param("~ip", "192.168.123.219") port = rospy.get_param("~port", 9000) print(ip, port) mb = MyBuddySocket(ip, port) diff --git a/Mybuddy/mybuddy_socket/scripts/test_control.py b/Mybuddy/mybuddy_socket/scripts/test_control.py index c1f0bd7..680fdd0 100644 --- a/Mybuddy/mybuddy_socket/scripts/test_control.py +++ b/Mybuddy/mybuddy_socket/scripts/test_control.py @@ -4,10 +4,15 @@ import rospy import time import os -port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) -baud = rospy.get_param("~baud", 115200) -print(port, baud) -mb = MyBuddy(port, baud) +from pymycobot.mybuddysocket import MyBuddySocket +import time + +mb = MyBuddySocket('192.168.123.219',9000) +mb.connect('/dev/ttyACM0','115200') +# port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1]) +# baud = rospy.get_param("~baud", 115200) +# print(port, baud) +# mb = MyBuddy(port, baud) # mb.set_free_mode(2) # mb.power_on() @@ -23,7 +28,7 @@ mb = MyBuddy(port, baud) ''' ------------get info-------------- ''' -# print(mb.get_radians(1)) +print(mb.get_radians()) # print(mb.get_encoders(3)) # print(mb.get_encoder(1,1)) # print(mb.get_angles(1)) @@ -37,8 +42,11 @@ mb = MyBuddy(port, baud) # mb.send_radians(2,[-0.136, -0.462, -0.62, 0.813, 0.218, -0.071],30) # mb.send_angles(2,[6.32, -94.39, 13.62, -15.38, -94.21, 62.84],30,1) # time.sleep(2) -print(mb.get_angles(3)) +print(mb.get_angles(1)) # mb.send_angles(1,[0,0,0,0,0,0],30,0) # mb.send_angle(1,3,30,30) # mb.set_encoder(3,1,2048,1) -time.sleep(1) \ No newline at end of file +time.sleep(1) +print(mb.get_angles(2)) +time.sleep(1) +print(mb.get_angles(3)) \ No newline at end of file From 61b091a1059ccf43ff4e19a0d1f1af939d2a4dc7 Mon Sep 17 00:00:00 2001 From: weijian Date: Tue, 6 Sep 2022 09:35:59 +0800 Subject: [PATCH 3/5] update --- Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py b/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py index ea60600..f82388b 100755 --- a/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py +++ b/Mybuddy/mybuddy_socket_moveit/scripts/sync_plan.py @@ -34,7 +34,7 @@ def callback(data): def listener(): global mb rospy.init_node("mybuddy_reciver", anonymous=True) - ip = rospy.get_param("~ip", "192.168.123.240") + ip = rospy.get_param("~ip", "192.168.123.219") port = rospy.get_param("~port", 9000) print(ip, port) mb = MyBuddySocket(ip, port) From 78b514a437792b9c0e3c0350c2a5d44d64990227 Mon Sep 17 00:00:00 2001 From: weijian Date: Tue, 6 Sep 2022 16:07:05 +0800 Subject: [PATCH 4/5] fix moveit.rviz --- Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz index a1d64d9..c664a53 100644 --- a/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz +++ b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz @@ -8,7 +8,7 @@ Panels: - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5 - Tree Height: 135 + Tree Height: 144 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -163,7 +163,7 @@ Visualization Manager: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 + Interactive Marker Size: 0.10000000149011612 Joint Violation Color: 255; 0; 255 Planning Group: left_arm Query Goal State: true @@ -311,9 +311,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd000000010000000000000203000002f1fc0200000007fb000000100044006900730070006c00610079007301000000410000011c000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a00560069006500770073000000010c000000a4000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000001630000004a0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b30000017f0000017400ffffff00000302000002f100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000121000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000164000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ab000001880000017d00ffffff00000312000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false Width: 1291 X: 452 - Y: 28 + Y: 27 From 952a62379f38d395c92b0dc8af821911556cbad1 Mon Sep 17 00:00:00 2001 From: weijian Date: Wed, 7 Sep 2022 14:20:53 +0800 Subject: [PATCH 5/5] update moveit.rviz --- Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz index c664a53..fe79aac 100644 --- a/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz +++ b/Mybuddy/mybuddy_socket_moveit/launch/moveit.rviz @@ -45,14 +45,14 @@ Visualization Manager: Class: moveit_rviz_plugin/MotionPlanning Enabled: 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: false - MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Use_Constraint_Aware_IK: true MoveIt_Workspace: Center: X: 0 @@ -295,9 +295,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 + Pitch: 0.16499997675418854 Target Frame: base - Yaw: -0.6232355833053589 + Yaw: 5.879950046539307 Saved: ~ Window Geometry: Displays: